Added individual testing motors onto button board + added shooter equations back in

.DataLogTool/datalogtool.json

@@ -0,0 +1 @@
+{}

.SysId/sysid.json

@@ -0,0 +1 @@
+{}

.gitignore

@@ -0,0 +1,187 @@
+# This gitignore has been specially created by the WPILib team.
+# If you remove items from this file, intellisense might break.
+
+### C++ ###
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+### Java ###
+# Compiled class file
+*.class
+
+# Log file
+*.log
+
+# BlueJ files
+*.ctxt
+
+# Mobile Tools for Java (J2ME)
+.mtj.tmp/
+
+# Package Files #
+*.jar
+*.war
+*.nar
+*.ear
+*.zip
+*.tar.gz
+*.rar
+
+# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml
+hs_err_pid*
+
+### Linux ###
+*~
+
+# temporary files which can be created if a process still has a handle open of a deleted file
+.fuse_hidden*
+
+# KDE directory preferences
+.directory
+
+# Linux trash folder which might appear on any partition or disk
+.Trash-*
+
+# .nfs files are created when an open file is removed but is still being accessed
+.nfs*
+
+### macOS ###
+# General
+.DS_Store
+.AppleDouble
+.LSOverride
+
+# Icon must end with two \r
+Icon
+
+# Thumbnails
+._*
+
+# Files that might appear in the root of a volume
+.DocumentRevisions-V100
+.fseventsd
+.Spotlight-V100
+.TemporaryItems
+.Trashes
+.VolumeIcon.icns
+.com.apple.timemachine.donotpresent
+
+# Directories potentially created on remote AFP share
+.AppleDB
+.AppleDesktop
+Network Trash Folder
+Temporary Items
+.apdisk
+
+### VisualStudioCode ###
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+
+### Windows ###
+# Windows thumbnail cache files
+Thumbs.db
+ehthumbs.db
+ehthumbs_vista.db
+
+# Dump file
+*.stackdump
+
+# Folder config file
+[Dd]esktop.ini
+
+# Recycle Bin used on file shares
+$RECYCLE.BIN/
+
+# Windows Installer files
+*.cab
+*.msi
+*.msix
+*.msm
+*.msp
+
+# Windows shortcuts
+*.lnk
+
+### Gradle ###
+.gradle
+/build/
+
+# Ignore Gradle GUI config
+gradle-app.setting
+
+# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored)
+!gradle-wrapper.jar
+
+# Cache of project
+.gradletasknamecache
+
+# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898
+# gradle/wrapper/gradle-wrapper.properties
+
+# # VS Code Specific Java Settings
+# DO NOT REMOVE .classpath and .project
+.classpath
+.project
+.settings/
+bin/
+
+# IntelliJ
+*.iml
+*.ipr
+*.iws
+.idea/
+out/
+
+# Fleet
+.fleet
+
+# Simulation GUI and other tools window save file
+networktables.json
+simgui.json
+*-window.json
+
+# Simulation data log directory
+logs/
+
+# Folder that has CTRE Phoenix Sim device config storage
+ctre_sim/
+
+# clangd
+/.cache
+compile_commands.json
+
+# Eclipse generated file for annotation processors
+.factorypath

.vscode/launch.json

@@ -0,0 +1,21 @@
+{
+  // Use IntelliSense to learn about possible attributes.
+  // Hover to view descriptions of existing attributes.
+  // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+  "version": "0.2.0",
+  "configurations": [
+
+    {
+      "type": "wpilib",
+      "name": "WPILib Desktop Debug",
+      "request": "launch",
+      "desktop": true,
+    },
+    {
+      "type": "wpilib",
+      "name": "WPILib roboRIO Debug",
+      "request": "launch",
+      "desktop": false,
+    }
+  ]
+}

.vscode/settings.json

@@ -0,0 +1,61 @@
+{
+  "java.configuration.updateBuildConfiguration": "automatic",
+  "java.server.launchMode": "Standard",
+  "files.exclude": {
+    "**/.git": true,
+    "**/.svn": true,
+    "**/.hg": true,
+    "**/CVS": true,
+    "**/.DS_Store": true,
+    "bin/": true,
+    "**/.classpath": true,
+    "**/.project": true,
+    "**/.settings": true,
+    "**/.factorypath": true,
+    "**/*~": true
+  },
+  "java.test.config": [
+    {
+      "name": "WPIlibUnitTests",
+      "workingDirectory": "${workspaceFolder}/build/jni/release",
+      "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ],
+      "env": {
+        "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" ,
+        "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
+      }
+    },
+  ],
+  "java.test.defaultConfig": "WPIlibUnitTests",
+  "java.import.gradle.annotationProcessing.enabled": false,
+  "java.completion.favoriteStaticMembers": [
+    "org.junit.Assert.*",
+    "org.junit.Assume.*",
+    "org.junit.jupiter.api.Assertions.*",
+    "org.junit.jupiter.api.Assumptions.*",
+    "org.junit.jupiter.api.DynamicContainer.*",
+    "org.junit.jupiter.api.DynamicTest.*",
+    "org.mockito.Mockito.*",
+    "org.mockito.ArgumentMatchers.*",
+    "org.mockito.Answers.*",
+    "edu.wpi.first.units.Units.*"
+  ],
+  "java.completion.filteredTypes": [
+    "java.awt.*",
+    "com.sun.*",
+    "sun.*",
+    "jdk.*",
+    "org.graalvm.*",
+    "io.micrometer.shaded.*",
+    "java.beans.*",
+    "java.util.Base64.*",
+    "java.util.Timer",
+    "java.sql.*",
+    "javax.swing.*",
+    "javax.management.*",
+    "javax.smartcardio.*",
+    "edu.wpi.first.math.proto.*",
+    "edu.wpi.first.math.**.proto.*",
+    "edu.wpi.first.math.**.struct.*",
+  ],
+  "java.dependency.enableDependencyCheckup": false
+}

.wpilib/wpilib_preferences.json

@@ -0,0 +1,6 @@
+{
+    "enableCppIntellisense": false,
+    "currentLanguage": "java",
+    "projectYear": "2026",
+    "teamNumber": 2890
+}

src/main/deploy/pathplanner/autos/90 Degree Test Auto.auto

@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "test_90Degree_Turn"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": true,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/autos/Center Auto.auto

@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Deploy_Intake_Command"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Start_Intake_Wheels"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center to Floor Intake"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Floor Intake"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Floor Intake to Left Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot_Fuel_Command"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Assist_Shooter"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Left Shoot to Offsetted-Left Climb"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Offsetted-Left to Left Climb"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lift_Robot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Human Player Auto.auto

@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Deploy_Intake_Command"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Starting to HP"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.25
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP to Hub"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot_Fuel_Command"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Assist_Shooter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Stop_Shooter_Command"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Hub to side of Climb"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Side into Climb"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lift_Robot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Left Side + Center Fuel Auto.auto

@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to Over Left Ramp"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Deploy_Intake_Command"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Start_Intake_Wheels"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Collect Left-Center Fuel"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Collect Left-Center Fuel to Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot_Fuel_Command"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 1.0
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Assist_Shooter"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Left Shoot to Offsetted-Left Climb"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Offsetted-Left to Left Climb"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Lift_Robot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Center to Floor Intake.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.528863636363636,
+        "y": 5.086295454545454
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.539647727272727,
+        "y": 5.818079545454545
+      },
+      "isLocked": false,
+      "linkedName": "Middle Start Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 1.1789090909090907,
+        "y": 5.941761363636363
+      },
+      "prevControl": {
+        "x": 2.9929090909090905,
+        "y": 5.261511363636364
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Left Intake Balls Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Middle Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Collect Left-Center Fuel to Shoot.path

@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.734045454548223,
+        "y": 4.6843295454545455
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.147034090909091,
+        "y": 5.282125000000001
+      },
+      "isLocked": false,
+      "linkedName": "Collect Fuel Left Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 2.6837045454545447,
+        "y": 4.591568181818182
+      },
+      "prevControl": {
+        "x": 2.6837045454545447,
+        "y": 6.805519750430038
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Floor Intake to Shoot Waypoint"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.6572327044025181,
+      "rotationDegrees": -91.02718603450556
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -18.80579011375007
+  },
+  "reversed": false,
+  "folder": "Left Auto",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Collect Left-Center Fuel.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.734045454548223,
+        "y": 7.054897727274547
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.734045454545454,
+        "y": 5.075988636363636
+      },
+      "isLocked": false,
+      "linkedName": "Over Left Ramp Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 7.734045454548223,
+        "y": 4.6843295454545455
+      },
+      "prevControl": {
+        "x": 7.734045454548223,
+        "y": 6.8982811140664015
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Collect Fuel Left Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "Left Auto",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Floor Intake to Left Shoot.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.5192727272727271,
+        "y": 5.941761363636363
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.6630909090909087,
+        "y": 4.591568181818182
+      },
+      "isLocked": false,
+      "linkedName": "Floor Intake Fuel Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 2.6837045454545447,
+        "y": 4.591568181818182
+      },
+      "prevControl": {
+        "x": 0.8490909090909089,
+        "y": 5.745931818181819
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Floor Intake to Shoot Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -18.80579011375007
+  },
+  "reversed": false,
+  "folder": "Middle Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Floor Intake.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1789090909090907,
+        "y": 5.941761363636363
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.5089659090909093,
+        "y": 5.931454545454544
+      },
+      "isLocked": false,
+      "linkedName": "Left Intake Balls Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 0.5192727272727271,
+        "y": 5.941761363636363
+      },
+      "prevControl": {
+        "x": 0.9521590909090447,
+        "y": 5.9314545454499195
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Floor Intake Fuel Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Middle Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/HP to Hub.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.46,
+        "y": 0.634
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.8135533905932739,
+        "y": 0.9875533905932734
+      },
+      "isLocked": false,
+      "linkedName": "HP Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 2.5,
+        "y": 2.5
+      },
+      "prevControl": {
+        "x": 2.146446609406726,
+        "y": 2.146446609406726
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Right Shoot Position Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 36.0
+  },
+  "reversed": false,
+  "folder": "HP to Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Hub to side of Climb.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.5,
+        "y": 2.5
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.030153689607046,
+        "y": 2.3289899283371653
+      },
+      "isLocked": false,
+      "linkedName": "Right Shoot Position Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 1.14,
+        "y": 2.046
+      },
+      "prevControl": {
+        "x": 1.6098463103929541,
+        "y": 2.2170100716628345
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Offsetted-Climb Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "HP to Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 36.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Left Shoot to Offsetted-Left Climb.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.6837045454545447,
+        "y": 4.591568181818182
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.4468863636391924,
+        "y": 4.911079545452217
+      },
+      "isLocked": false,
+      "linkedName": "Floor Intake to Shoot Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 1.14,
+        "y": 5.0
+      },
+      "prevControl": {
+        "x": 2.467261363639192,
+        "y": 4.653409090906762
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Ofsetted-Left Climb Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Middle Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -18.80579011375007
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Left to Floor Intake.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5185568181818176,
+        "y": 7.219806818181819
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.5190340909090907,
+        "y": 6.12728409090909
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.1789090909090907,
+        "y": 5.941761363636363
+      },
+      "prevControl": {
+        "x": 2.9929090909090905,
+        "y": 6.910602272727273
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Left Intake Balls Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Middle Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Offsetted-Left to Left Climb.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.0,
-        "y": 7.0
+        "x": 1.14,
+        "y": 5.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.75,
-        "y": 7.0
+        "x": 1.14,
+        "y": 4.292625343835472
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "Ofsetted-Left Climb Waypoint"
     },
     {
       "anchor": {
-        "x": 3.0,
-        "y": 7.0
+        "x": 1.14,
+        "y": 4.25
       },
       "prevControl": {
-        "x": 3.48768579117494,
-        "y": 7.0
+        "x": 1.14,
+        "y": 4.70494908797556
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,7 +33,7 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.0,
+    "maxVelocity": 1.0,
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -90.0
+    "rotation": 90.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Middle Shoot to Climb",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": 90.0
   },
   "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/Side into Climb.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.14,
+        "y": 2.046
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.1561466905187832,
+        "y": 2.7126296958855094
+      },
+      "isLocked": false,
+      "linkedName": "Offsetted-Right Climb Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 1.14,
+        "y": 3.25
+      },
+      "prevControl": {
+        "x": 1.146147727272727,
+        "y": 2.940795454545455
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Right Climb Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "HP to Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Start to Over Left Ramp.path

@@ -0,0 +1,70 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5185568181818176,
+        "y": 7.2095
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.1273750000027687,
+        "y": 5.385193181820003
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.672920454548223,
+        "y": 5.477954545456365
+      },
+      "prevControl": {
+        "x": 3.3539345896947985,
+        "y": 5.471848129415376
+      },
+      "nextControl": {
+        "x": 6.899193181820951,
+        "y": 5.488261363638184
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.734045454548223,
+        "y": 7.054897727274547
+      },
+      "prevControl": {
+        "x": 5.971579545454545,
+        "y": 7.271340909090909
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "Over Left Ramp Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "Left Auto",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/Starting to HP.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5159570661896247,
+        "y": 0.8
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.6424772727272727,
+        "y": 0.7471249999999998
+      },
+      "isLocked": false,
+      "linkedName": "Start Right Trench Waypoint"
+    },
+    {
+      "anchor": {
+        "x": 0.46,
+        "y": 0.634
+      },
+      "prevControl": {
+        "x": 1.0119022424324617,
+        "y": 0.6340000000000003
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "HP Waypoint"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "HP to Shoot to Climb",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/test_1_Meter.path

@@ -4,24 +4,24 @@
     {
       "anchor": {
         "x": 1.0,
-        "y": 7.0
+        "y": 1.87
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.0,
-        "y": 7.0
+        "x": 1.9999999999999858,
+        "y": 1.87
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.075,
-        "y": 4.75
+        "x": 3.0,
+        "y": 1.87
       },
       "prevControl": {
-        "x": 2.380178890876563,
-        "y": 5.341144901610018
+        "x": 1.8572206587126847,
+        "y": 1.8700000000000003
       },
       "nextControl": null,
       "isLocked": false,
@@ -42,7 +42,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -90.0
+    "rotation": 0.0
   },
   "reversed": false,
   "folder": null,

src/main/deploy/pathplanner/settings.json

@@ -2,7 +2,11 @@
   "robotWidth": 0.9,
   "robotLength": 0.9,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
+    "HP to Shoot to Climb",
+    "Middle Shoot to Climb",
+    "Left Auto"
+  ],
   "autoFolders": [],
   "defaultMaxVel": 3.0,
   "defaultMaxAccel": 3.0,

src/main/deploy/swerve/neo/controllerproperties.json

@@ -2,7 +2,7 @@
   "angleJoystickRadiusDeadband": 0.5,
   "heading": {
     "p": 0.4,
-    "i": 0,
+    "i": 0.01,
     "d": 0.01
   }
 }

src/main/deploy/swerve/neo/modules/backleft.json

@@ -3,7 +3,7 @@
     "front": -10.9,
     "left": 10.9
   },
-  "absoluteEncoderOffset": 216.38671875,
+  "absoluteEncoderOffset": 215.947265625,
   "drive": {
     "type": "sparkflex_neo",
     "id": 12,

src/main/deploy/swerve/neo/modules/backright.json

@@ -3,7 +3,7 @@
     "front": -10.9,
     "left": -10.9
   },
-  "absoluteEncoderOffset": 191.6015625,
+  "absoluteEncoderOffset": 283.7109375,
   "drive": {
     "type": "sparkflex_neo",
     "id": 11,

src/main/deploy/swerve/neo/modules/frontleft.json

@@ -3,7 +3,7 @@
     "front": 10.9,
     "left": 10.9
   },
-  "absoluteEncoderOffset": 198.896484375,
+  "absoluteEncoderOffset": 34.716796875,
   "drive": {
     "type": "sparkflex_neo",
     "id": 13,

src/main/deploy/swerve/neo/modules/frontright.json

@@ -3,7 +3,7 @@
     "front": 10.9,
     "left": -10.9
   },
-  "absoluteEncoderOffset": 231.064453125,
+  "absoluteEncoderOffset": 318.779296875,
   "drive": {
     "type": "sparkflex_neo",
     "id": 10,

src/main/deploy/swerve/neo/modules/pidfproperties.json

@@ -1,16 +1,16 @@
 {
   "drive": {
-    "p": 0.001,
+    "p": 0.15,
     "i": 0,
     "d": 0,
-    "f": 0,
+    "f": 0.15,
     "iz": 0
   },
   "angle": {
-    "p": 0.002,
-    "i": 0.0,
-    "d": 0.0,
-    "f": 0,
+    "p": 0.008,
+    "i": 0.00001,
+    "d": 0,
+    "f": 0.2,
     "iz": 0
   }
 }

src/main/java/frc/robot/Constants.java

@@ -4,11 +4,18 @@
 
 package frc.robot;
 
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+
+import com.pathplanner.lib.path.PathConstraints;
+
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.GenericEntry;
@@ -63,103 +70,162 @@ public final class Constants {
     }
 
     public static class ShooterConstants {
-        private static GenericEntry shooterVelocity = programmingTab.add("Desired Shooter Velocity", -0.5)
-                .withWidget(BuiltInWidgets.kNumberBar).getEntry();
+       // private static GenericEntry shooterRPM = programmingTab.add("Desired Shooter RPM", -4000)
+        //        .withWidget(BuiltInWidgets.kNumberBar).getEntry();
 
-        public static double SHOOTER_VELOCITY = -0.6;
-        public static double SHOOTER_POWER = -0.45;
+        public static double SHOOTER_RPM = -2700;
+        public static final double IDLE_SHOOTER_RPM = -1800;
+        public static final double AUTO_SHOOTER_RPM = -3100; //always negative to shoot
 
-        public static void getShooterVelocity() {
-            SHOOTER_VELOCITY = shooterVelocity.getDouble(-0.5);
-        }
+       // public static void updateShooterRPM() {
+          //  SHOOTER_RPM = shooterRPM.getDouble(-4000);
+       // }
 
         public static final int CENTER_SHOOTER_MOTOR_ID = 42;
         public static final int LEFT_SHOOTER_MOTOR_ID = 41;
         public static final int RIGHT_SHOOTER_MOTOR_ID = 40;
         public static final int INDEXER_MOTOR_ID = 43;
-        public static double INDEXER_MOTOR_SPEED = 0.6;
 
-        private static GenericEntry indexerAndRampSpeed = programmingTab.add("Desired Ramp + Indexer Speed", 0.6)
-                .withWidget(BuiltInWidgets.kNumberBar).getEntry();
+        public static double SHOOTER_MOTOR_P = 0.001;
+        public static double SHOOTER_MOTOR_I = 0;
+        public static double SHOOTER_MOTOR_D = 0.001;
+        public static double SHOOTER_MOTOR_S = 0.2;
+        public static double SHOOTER_MOTOR_V = 0.0015;
+
+        public static double INDEXER_MOTOR_P = 0.0001;
+        public static double INDEXER_MOTOR_I = 0;
+        public static double INDEXER_MOTOR_D = 0;
+
+        /*
+         * private static GenericEntry indexerAndRampRPM =
+         * programmingTab.add("Desired Ramp + Indexer RPM", 2000)
+         * .withWidget(BuiltInWidgets.kNumberBar).getEntry();
+         */
+
+        public static double INDEXER_AND_RAMP_MOTOR_RPM = 15000; //always positive when feeding into shooter
 
         // this method called in robot periodic so values updated in elastic are
         // constantly read and applied to RAMP_MOTOR_SPEED
-        public static void getRampMotorSpeed() {
-            INDEXER_MOTOR_SPEED = indexerAndRampSpeed.getDouble(.6);
-        }
-
-        public static final int LEFT_ACTUATOR_PWM_PORT = 1;
-        public static final int RIGHT_ACTUATOR_PWM_PORT = 3;
-
-        public static double DESIRED_POTENTIOMETER_DISTANCE;
-
-        // TO DO: need to equation that calculates desired potentiometer distance
+        /*
+         * public static void updateIndexerAndRampMotorRPM() {
+         * INDEXER_AND_RAMP_MOTOR_RPM = indexerAndRampRPM.getDouble(2000);
+         * }
+         */
     }
 
     public static class IntakeConstants {
-        private static GenericEntry intakeSpeed = programmingTab.add("Desired Intake Speed", -0.4)
-                .withWidget(BuiltInWidgets.kNumberBar).getEntry();
 
-        public static double INTAKE_WHEELS_MOTOR_SPEED;
+        /*
+         * private static GenericEntry intakeRPM =
+         * programmingTab.add("Desired Intake RPM", -1000)
+         * .withWidget(BuiltInWidgets.kNumberBar).getEntry();
+         */
+        public static double INTAKE_WHEELS_MOTOR_RPM_FAST = -8000; // always negative when intaking
+        public static double INTAKE_WHEELS_MOTOR_RPM_SLOW = -5000; // always negative when intaking
 
-        public static void getIntakeWheelsSpeed() {
-            INTAKE_WHEELS_MOTOR_SPEED = intakeSpeed.getDouble(-0.4);
-        }
+
+        /*
+         * public static void updateIntakeWheelsRPM() {
+         * INTAKE_WHEELS_MOTOR_RPM = intakeRPM.getDouble(-1000);
+         * }
+         */
 
         public static final int INTAKE_WHEELS_MOTOR_ID = 50;
         public static final int INTAKE_ROTATOR_MOTOR_ID = 51;
-        public static final double INTAKE_COLLECT_ENCODER_VALUE = 4.1290459632873535;
-        public static final double INTAKE_MIDDLE_ENCODER_VALUE = 2.2550222873687744;
-        public static final double INTAKE_RETRACT_ENCODER_VALUE = 0;
 
         public static class IntakeRotatorPID {
-            public static final double INTAKE_ROTATOR_P = 0.05;
+            public static final double INTAKE_ROTATOR_P = .07;
             public static final double INTAKE_ROTATOR_I = 0;
-            public static final double INTAKE_ROTATOR_D = 0.001;
+            public static final double INTAKE_ROTATOR_D = 0.03;
         }
 
+        public static final double INTAKE_MOTOR_P = 0.0001;
+        public static final double INTAKE_MOTOR_I = 0;
+        public static final double INTAKE_MOTOR_D = 0.00005;
+
+        public static final double INTAKE_COLLECT_ENCODER_VALUE = 1;
+        public static final double INTAKE_MIDDLE_ENCODER_VALUE = -4;
+        public static final double INTAKE_RETRACT_ENCODER_VALUE = -6.2;
+
+        public static final double INTAKE_THROUGHBORE_ENCODER_DEPLOY = .13;
+        public static final double INTAKE_THROUGHBORE_ENCODER_RETRACT = .49;
+        public static final double INTAKE_THROUGHBORE_ENCODER_MIDDLE = .389;
+
+        public static final double INTAKE_MANUAL_SPEED = .15;
     }
 
-    public static class RampConstants {
-        public static final int RAMP_MOTOR_ID = 45;
-        public static double RAMP_MOTOR_SPEED = .6;
-
-        // create object and a new widget under programming tab in Elastic where object
-        // retrieves value from widget
-
-    }
-
-    public static class LimeLight {
-
-        public static final String LIMELIGHT_NAME = "limelight";
-
-        // public static final int[] ALL_REEF_APRILTAGS = { 6, 7, 8, 9, 10, 11, 17, 18,
-        // 19, 20, 21, 22 };
-
-        public static final AprilTagFieldLayout APRILTAG_FIELD_LAYOUT = AprilTagFieldLayout
-                .loadField(AprilTagFields.k2026RebuiltAndymark);
-
-        public static final double BUMPER_WIDTH = Units.inchesToMeters(0.0); // Get This Value // Original: 2.75
-        // public static final double ROBOT_WIDTH = Units.inchesToMeters(30 +
-        // BUMPER_WIDTH); // Tis a square, don't need this
-        public static final double ROBOT_SIDE_LENGTH = Units.inchesToMeters(27);
-        // public static final Transform2d HALF_ROBOT = new
-        // Transform2d(ROBOT_SIDE_LENGTH / 3.0, 0, new Rotation2d());
-
-        public static double LIMELIGHT_TY;
-    }
+    // create object and a new widget under programming tab in Elastic where object
+    // retrieves value from widget
 
     public static class TargetingConstants {
-        public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90));
-        public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90));
+        public static final Pose2d BLUE_LEFT_CLIMB_POSE = new Pose2d(1.14, 4.25, Rotation2d.fromDegrees(90));
+        public static final Pose2d BLUE_LEFT_CLIMB_POSE_OFFSETTED = new Pose2d(1.14, 5.0,
+                Rotation2d.fromDegrees(90));
+
+        public static final Pose2d BLUE_RIGHT_CLIMB_POSE = new Pose2d(1.14, 2.046, Rotation2d.fromDegrees(-90));
+        public static final Pose2d BLUE_RIGHT_CLIMB_POSE_OFFSETTED = new Pose2d(1.14, 3.25,
+                Rotation2d.fromDegrees(-90));
+
+        public static final Pose2d RED_RIGHT_CLIMB_POSE = new Pose2d(15.474, 5.22, Rotation2d.fromDegrees(90));
+        public static final Pose2d RED_RIGHT_CLIMB_POSE_OFFSETTED = new Pose2d(15.474, 5.72,
+                Rotation2d.fromDegrees(90));
+
+        public static final Pose2d RED_LEFT_CLIMB_POSE = new Pose2d(15.474, 3.437, Rotation2d.fromDegrees(-90));
+        public static final Pose2d RED_LEFT_CLIMB_POSE_OFFSETTED = new Pose2d(15.474, 2.937,
+                Rotation2d.fromDegrees(-90));
+
+        public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Back Left Camera");
+        public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Back Right Camera");
+        public static final PhotonCamera RED_PHOTON_CAM = new PhotonCamera("Front Left Camera");
+        public static final PhotonCamera PURPLE_PHOTON_CAM = new PhotonCamera("Front Right Camera");
+
+        public static Rotation2d HUB_ROTATION_BLUE;
+        public static Rotation2d HUB_ROTATION_RED;
+        public static Pose2d allianceHubPose = new Pose2d();
+
+
+        public static final double HUB_X_POSE_BLUE = 4.625;
+        public static final double HUB_Y_POSE_BLUE = 4.03;
+
+        public static final double HUB_X_POSE_RED = 11.915;
+        public static final double HUB_Y_POSE_RED = 4.03;
+
+        public static PathConstraints DRIVE_INTO_CLIMB_CONSTRAINTS;
+
+        public static final Transform3d ROBOT_TO_BACK_LEFT_CAM = new Transform3d(
+                new Translation3d(-Units.inchesToMeters(12.75), Units.inchesToMeters(6.25),
+                        Units.inchesToMeters(13.1875)),
+                new Rotation3d(0, 0, Math.toRadians(195)));
+
+        public static final Transform3d ROBOT_TO_BACK_RIGHT_CAM = new Transform3d(
+                new Translation3d(-Units.inchesToMeters(12.75), -Units.inchesToMeters(6.25), Units.inchesToMeters(14)),
+                new Rotation3d(0, 0, Math.toRadians(-200)));
+
+        public static final Transform3d ROBOT_TO_FRONT_LEFT_CAM = new Transform3d(
+                new Translation3d(Units.inchesToMeters(1.3), Units.inchesToMeters(11.25), Units.inchesToMeters(26)),
+                new Rotation3d(0, Math.toRadians(10), 0));
+
+        public static final Transform3d ROBOT_TO_FRONT_RIGHT_CAM = new Transform3d(
+                new Translation3d(Units.inchesToMeters(1.5), Units.inchesToMeters(-11.25), Units.inchesToMeters(26)),
+                new Rotation3d(0, Math.toRadians(-10), 0));
     }
 
     public static class ClimberConstants {
-        public static final int CLIMB_MOTOR_ID = 60;
-        public static final int RATCHET_PWM_PORT = 0;
+        public static final int CLIMB_MOTOR_ID = 52;
 
-        public static final double RATCHET_UNLOCK_ANGLE = 0;
-        public static final double RATCHET_LOCK_ANGLE = 180;
         public static final double CLIMBER_SPEED = .5;
+
+        public static final double CLIMBER_PID_P = 5;
+        public static final double CLIMBER_PID_I = 0;
+        public static final double CLIMBER_PID_D = 0;
+
+        public static final double CLIMBER_LIFTED_SETPOINT_VALUE = 65;
+        public static final double CLIMBER_LOWERED_SETPOINT_VALUE = 0;
+
+    }
+
+    public static class LEDConstants {
+        public static final int LED_PWM_PORT = 6;
+
     }
 }

src/main/java/frc/robot/Robot.java

@@ -4,11 +4,18 @@
 
 package frc.robot;
 
+import java.util.logging.Logger;
+
+import org.photonvision.PhotonUtils;
+import org.photonvision.targeting.PhotonPipelineResult;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
@@ -17,6 +24,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.Constants.IntakeConstants;
+import frc.robot.subsystems.ClimberSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.LEDSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.TargetingSubsystems;
 
 /**
@@ -31,10 +43,14 @@ public class Robot extends TimedRobot {
     private Command m_autonomousCommand;
 
     private RobotContainer m_robotContainer;
+    private LEDSubsystem m_LedSubsystem;
+    private ClimberSubsystem m_ClimberSubsystem;
+    private ShooterSubsystem m_ShooterSubsystem;
 
     private Timer disabledTimer;
     private static NetworkTable table;
-    private static GenericEntry distanceFromLimelight;
+
+    private static StructPublisher<Pose2d> advantageScopePublisher = NetworkTableInstance.getDefault().getStructTopic("Robot Pose hahaha", Pose2d.struct).publish();
 
     public Robot() {
         instance = this;
@@ -53,8 +69,11 @@ public class Robot extends TimedRobot {
         // Instantiate our RobotContainer. This will perform all our button bindings,
         // and put our
         // autonomous chooser on the dashboard.
-        m_robotContainer = new RobotContainer();
 
+        m_LedSubsystem = new LEDSubsystem();
+        m_robotContainer = new RobotContainer();
+        m_ClimberSubsystem = new ClimberSubsystem();
+        m_ShooterSubsystem = new ShooterSubsystem();
         // Create a timer to disable motor brake a few seconds after disable. This will
         // let the robot stop
         // immediately when disabled, but then also let it be pushed more
@@ -64,8 +83,8 @@ public class Robot extends TimedRobot {
             DriverStation.silenceJoystickConnectionWarning(true);
         }
 
-        table = NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME);
-        distanceFromLimelight = Shuffleboard.getTab("Programming").add("Distance from Limelight", 0).getEntry();
+        m_robotContainer.getSwerveDrive().zeroGyroWithAlliance();
+        IntakeSubsystem.resetIntakeRotationEncoder();
     }
 
     /**
@@ -88,12 +107,16 @@ public class Robot extends TimedRobot {
         // robot's periodic
         // block in order for anything in the Command-based framework to work.
         CommandScheduler.getInstance().run();
+        TargetingSubsystems.updateShooterRPM(m_robotContainer.getSwerveDrive().getPose());
 
-       // Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
-        Constants.IntakeConstants.getIntakeWheelsSpeed();
-        Constants.ShooterConstants.getShooterVelocity();
-        Constants.LimeLight.LIMELIGHT_TY = table.getEntry("ty").getDouble(0);
-        //distanceFromLimelight.setDouble(TargetingSubsystems.getDistanceFromAprilTag());
+        SmartDashboard.putNumber("Estimated Shooter RPM", Constants.ShooterConstants.SHOOTER_RPM);
+        SmartDashboard.putNumber("Distance From Hub: ", PhotonUtils.getDistanceToPose(m_robotContainer.getSwerveDrive().getPose(), Constants.TargetingConstants.allianceHubPose));
+        //Constants.ShooterConstants.getRampAndIndexerMotorSpeed();
+        //Constants.IntakeConstants.updateIntakeWheelsRPM();
+        //Constants.ShooterConstants.updateShooterRPM();
+        TargetingSubsystems.getHubPoseTheta(m_robotContainer.getSwerveDrive());
+        //Constants.ShooterConstants.updateIndexerAndRampMotorRPM();
+        advantageScopePublisher.set(m_robotContainer.getSwerveDrive().getPose());
     }
 
     /**
@@ -121,8 +144,10 @@ public class Robot extends TimedRobot {
      */
     @Override
     public void autonomousInit() {
+       // IntakeSubsystem.resetIntakeRotationEncoder();
         m_robotContainer.setMotorBrake(true);
         m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+        //ShooterSubsystem.setShooterMotorsRPM(Constants.ShooterConstants.IDLE_SHOOTER_RPM);
 
         // Print the selected autonomous command upon autonomous init
         System.out.println("Auto selected: " + m_autonomousCommand);
@@ -131,6 +156,9 @@ public class Robot extends TimedRobot {
         if (m_autonomousCommand != null) {
             m_autonomousCommand.schedule();
         }
+
+
+
     }
 
     /**
@@ -142,6 +170,8 @@ public class Robot extends TimedRobot {
 
     @Override
     public void teleopInit() {
+        ClimberSubsystem.lowerRobot();
+        //ShooterSubsystem.setShooterMotorsRPM(Constants.ShooterConstants.IDLE_SHOOTER_RPM);
         // This makes sure that the autonomous stops running when
         // teleop starts running. If you want the autonomous to
         // continue until interrupted by another command, remove

src/main/java/frc/robot/RobotContainer.java

@@ -4,6 +4,7 @@
 
 package frc.robot;
 
+import com.fasterxml.jackson.databind.util.Named;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
@@ -22,21 +23,30 @@ import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants.OperatorConstants;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.LEDSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
 import java.io.File;
 import java.lang.annotation.Target;
 import java.util.concurrent.atomic.AtomicBoolean;
 import java.util.function.DoubleSupplier;
+
+import javax.lang.model.util.ElementScanner14;
+
 import frc.robot.subsystems.TargetingSubsystems;
 import swervelib.SwerveInputStream;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.subsystems.ClimberSubsystem;
+import frc.robot.subsystems.swervedrive.Vision;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -48,261 +58,334 @@ import frc.robot.subsystems.ClimberSubsystem;
  */
 public class RobotContainer {
 
-    // Replace with CommandPS4Controller or CommandJoystick if needed
-    final CommandXboxController driverXbox = new CommandXboxController(0);
-    // The robot's subsystems and commands are defined here...
-    private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
-            "swerve/neo"));
+        // Replace with CommandPS4Controller or CommandJoystick if needed
+        final CommandXboxController driverXbox = new CommandXboxController(0);
+        final CommandXboxController operatorXbox = new CommandXboxController(1);
+        private final static CommandJoystick topButtons = new CommandJoystick(2);
+        final CommandJoystick bottomButtons = new CommandJoystick(3);
 
-    // Establish a Sendable Chooser that will be able to be sent to the
-    // SmartDashboard, allowing selection of desired auto
-    private final SendableChooser<Command> autoChooser;
+        // The robot's subsystems and commands are defined here...
+        private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(),
+                        "swerve/neo"));
 
-    private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
-    // private final TargetingSubsystems m_TargetingSubsystems = new
-    // TargetingSubsystems();
-    private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
-    private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
+        // Establish a Sendable Chooser that will be able to be sent to the
+        // SmartDashboard, allowing selection of desired auto
+        private final SendableChooser<Command> autoChooser;
 
-    /**
-     * Converts driver input into a field-relative ChassisSpeeds that is controlled
-     * by angular velocity.
-     */
-    SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
-            () -> driverXbox.getLeftY() * -1,
-            () -> driverXbox.getLeftX() * -1)
-            .withControllerRotationAxis(() -> driverXbox.getRightX() * -1)
-            .deadband(OperatorConstants.DEADBAND)
-            .scaleTranslation(0.8)
-            .allianceRelativeControl(true);
+        private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem();
+        private final TargetingSubsystems m_TargetingSubsystems = new TargetingSubsystems();
+        private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem();
+        private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem();
+        /**
+         * Converts driver input into a field-relative ChassisSpeeds that is controlled
+         * by angular velocity.
+         */
+        SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
+                        () -> driverXbox.getLeftY() * -1,
+                        () -> driverXbox.getLeftX() * -1)
+                        .withControllerRotationAxis(() -> driverXbox.getRightX() * -1)
+                        .deadband(OperatorConstants.DEADBAND)
+                        .scaleTranslation(0.8)
+                        .allianceRelativeControl(true);
 
-    /**
-     * Clone's the angular velocity input stream and converts it to a fieldRelative
-     * input stream.
-     */
-    SwerveInputStream driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis(driverXbox::getRightX,
-            driverXbox::getRightY)
-            .headingWhile(true);
+        /**
+         * Clone's the angular velocity input stream and converts it to a fieldRelative
+         * input stream.
+         */
+        SwerveInputStream driveDirectAngle = driveAngularVelocity.copy()
+                        .withControllerHeadingAxis(driverXbox::getRightX,
+                                        driverXbox::getRightY)
+                        .headingWhile(true);
 
-    /**
-     * Clone's the angular velocity input stream and converts it to a robotRelative
-     * input stream.
-     */
-    SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false)
-            .allianceRelativeControl(true);
+        /**
+         * Clone's the angular velocity input stream and converts it to a robotRelative
+         * input stream.
+         */
+        SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(false)
+                        .allianceRelativeControl(true);
 
-    SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
-            () -> -driverXbox.getLeftY(),
-            () -> -driverXbox.getLeftX())
-            .withControllerRotationAxis(() -> driverXbox.getRawAxis(
-                    2))
-            .deadband(OperatorConstants.DEADBAND)
-            .scaleTranslation(0.8)
-            .allianceRelativeControl(true);
-    // Derive the heading axis with math!
-    SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
-            .withControllerHeadingAxis(() -> Math.sin(
-                    driverXbox.getRawAxis(
-                            2) *
-                            Math.PI)
-                    *
-                    (Math.PI *
-                            2),
-                    () -> Math.cos(
-                            driverXbox.getRawAxis(
-                                    2) *
-                                    Math.PI)
-                            *
-                            (Math.PI *
-                                    2))
-            .headingWhile(true)
-            .translationHeadingOffset(true)
-            .translationHeadingOffset(Rotation2d.fromDegrees(
-                    0));
+        SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
+                        () -> -driverXbox.getLeftY(),
+                        () -> -driverXbox.getLeftX())
+                        .withControllerRotationAxis(() -> driverXbox.getRawAxis(
+                                        2))
+                        .deadband(OperatorConstants.DEADBAND)
+                        .scaleTranslation(0.8)
+                        .allianceRelativeControl(true);
+        // Derive the heading axis with math!
+        SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
+                        .withControllerHeadingAxis(() -> Math.sin(
+                                        driverXbox.getRawAxis(
+                                                        2) *
+                                                        Math.PI)
+                                        *
+                                        (Math.PI *
+                                                        2),
+                                        () -> Math.cos(
+                                                        driverXbox.getRawAxis(
+                                                                        2) *
+                                                                        Math.PI)
+                                                        *
+                                                        (Math.PI *
+                                                                        2))
+                        .headingWhile(true)
+                        .translationHeadingOffset(true)
+                        .translationHeadingOffset(Rotation2d.fromDegrees(
+                                        0));
 
-    /**
-     * The container for the robot. Contains subsystems, OI devices, and commands.
-     */
-    public RobotContainer() {
-        // Configure the trigger bindings
-        configureBindings();
-        DriverStation.silenceJoystickConnectionWarning(true);
+        /**
+         * The container for the robot. Contains subsystems, OI devices, and commands.
+         */
+        public RobotContainer() {
+                // Configure the trigger bindings
+                configureBindings();
+                DriverStation.silenceJoystickConnectionWarning(true);
 
-        // Create the NamedCommands that will be used in PathPlanner
-        NamedCommands.registerCommand("test", Commands.print("I EXIST"));
+                // Create the NamedCommands that will be used in PathPlanner
+                NamedCommands.registerCommand("test", Commands.print("I EXIST"));
+                NamedCommands.registerCommand("Start_Intake_Wheels", m_IntakeSubsystem.startIntakeMotorCommand(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_FAST));
+                NamedCommands.registerCommand("Shoot_Fuel_Command",
+                                m_ShooterSubsystem.setShooterMotorsRPMAutoCommand());
+                NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem.deployIntakeCommand()
+                                .andThen(m_IntakeSubsystem.startIntakeMotorCommand(
+                                                Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_SLOW)));
+                NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand()
+                                .andThen(m_IntakeSubsystem.deployIntakeCommand()));
+                NamedCommands.registerCommand("Lift_Robot_Command", m_ClimberSubsystem.liftRobotCommand());
+                NamedCommands.registerCommand("Assist_Shooter",
+                                m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly().withTimeout(5));
+                NamedCommands.registerCommand("Lift_Robot", m_ClimberSubsystem.liftRobotCommand());
+                NamedCommands.registerCommand("Kill_All", killAllCommand());
+                NamedCommands.registerCommand("Auto_Aim_To_Hub",
+                                m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
+                
+                
+                /*NamedCommands.registerCommand("PathPlan_To_Climb_Right_Offsetted",
+                                drivebase.driveToClimbPoseOffsetted(
+                                                Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED,
+                                                Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED));
+                NamedCommands.registerCommand("PathPlan_To_Climb_Left_Offsetted",
+                                drivebase.driveToClimbPoseOffsetted(
+                                                Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED,
+                                                Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED));
+                NamedCommands.registerCommand("PathPlan_Into_Climb_Right",
+                                drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE,
+                                                Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE));
+                NamedCommands.registerCommand("PathPlan_Into_Climb_Left",
+                                drivebase.driveToClimbPoseOffsetted(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE,
+                                                Constants.TargetingConstants.RED_LEFT_CLIMB_POSE));*/
 
-        // Have the autoChooser pull in all PathPlanner autos as options
-        autoChooser = AutoBuilder.buildAutoChooser();
+                // Have the autoChooser pull in all PathPlanner autos as options
+                autoChooser = AutoBuilder.buildAutoChooser();
 
-        // Set the default auto (do nothing)
-        autoChooser.setDefaultOption("Do Nothing", Commands.none());
+                // Set the default auto (do nothing)
+                autoChooser.setDefaultOption("Do Nothing", Commands.none());
 
-        // Add a simple auto option to have the robot drive forward for 1 second then
-        // stop
-        autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1));
+                // Add a simple auto option to have the robot drive forward for 1 second then
+                // stop
+                autoChooser.addOption("Drive Forward", drivebase.driveForward().withTimeout(1));
 
-        // Put the autoChooser on the SmartDashboard
-        SmartDashboard.putData("Auto Chooser", autoChooser);
-
-    }
-
-    /**
-     * Use this method to define your trigger->command mappings. Triggers can be
-     * created via the
-     * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
-     * an arbitrary predicate, or via the
-     * named factories in
-     * {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
-     * for
-     * {@link CommandXboxController
-     * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
-     * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
-     * Flight joysticks}.
-     */
-    private void configureBindings() {
-        Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
-        Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
-        Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
-        Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
-                driveDirectAngle);
-        Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
-        Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard);
-        Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
-                driveDirectAngleKeyboard);
-
-        driverXbox.leftTrigger().onTrue(m_IntakeSubsystem.startIntakeMotorCommand())
-                .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
-        driverXbox.leftBumper().whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand())
-                .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand());
-        
-        // command for
-        // full shooting system including linear actuators
-        driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()
-                .andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
-
-        driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
-        
-        driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand());
-        driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()).onFalse(m_ClimberSubsystem.stopClimberCommand());
-        driverXbox.povDown().onTrue(m_IntakeSubsystem.retractIntakeCommand());
-        driverXbox.povUp().onTrue(m_IntakeSubsystem.deployintakeCommand());
-        driverXbox.povLeft().onTrue(m_ClimberSubsystem.toggleRatchetCommand(true));
-        driverXbox.povRight().onTrue(m_ClimberSubsystem.toggleRatchetCommand(false));
-
-
-        // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
-        driverXbox.x().onTrue(m_ShooterSubsystem.stopShooterCommand().andThen(m_IntakeSubsystem.deployintakeCommand()));
-        // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
-        // () -> -driverXbox.getLeftX()));
-        if (driverXbox.getRightY() < -0.4) {
-            m_ClimberSubsystem.liftRobotCommand();
-        } else if (driverXbox.getRightY() > 0.4) {
-            m_ClimberSubsystem.lowerRobotCommand();
-        } else {
-            m_ClimberSubsystem.stopClimberCommand();
-        }
-
-        // driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
-
-        if (RobotBase.isSimulation()) {
-            drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
-        } else {
-            drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
-        }
-
-        if (Robot.isSimulation()) {
-            Pose2d target = new Pose2d(new Translation2d(1, 4),
-                    Rotation2d.fromDegrees(90));
-            // drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
-            driveDirectAngleKeyboard.driveToPose(() -> target,
-                    new ProfiledPIDController(5,
-                            0,
-                            0,
-                            new Constraints(5, 2)),
-                    new ProfiledPIDController(5,
-                            0,
-                            0,
-                            new Constraints(Units.degreesToRadians(360),
-                                    Units.degreesToRadians(180))));
-            driverXbox.start()
-                    .onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
-            driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
-            driverXbox.button(2).whileTrue(Commands.runEnd(() -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
-                    () -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
-
-            // driverXbox.b().whileTrue(
-            // drivebase.driveToPose(
-            // new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
-            // );
+                // Put the autoChooser on the SmartDashboard
+                SmartDashboard.putData("Auto Chooser", autoChooser);
 
         }
-        if (DriverStation.isTest()) {
-            drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command above!
 
-            driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
-            driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
-            driverXbox.back().whileTrue(drivebase.centerModulesCommand());
-            driverXbox.leftBumper().onTrue(Commands.none());
-            driverXbox.rightBumper().onTrue(Commands.none());
-        } else {
-            driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
-            driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
-            driverXbox.start().whileTrue(Commands.none());
-            driverXbox.back().whileTrue(Commands.none());
-            driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
-            driverXbox.rightBumper().onTrue(Commands.none());
-        }
+        /**
+         * Use this method to define your trigger->command mappings. Triggers can be
+         * created via the
+         * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with
+         * an arbitrary predicate, or via the
+         * named factories in
+         * {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses
+         * for
+         * {@link CommandXboxController
+         * Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4}
+         * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick
+         * Flight joysticks}.
+         */
+        private void configureBindings() {
+                Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle);
+                Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
+                Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented);
+                Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative(
+                                driveDirectAngle);
+                Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard);
+                Command driveFieldOrientedAnglularVelocityKeyboard = drivebase
+                                .driveFieldOriented(driveAngularVelocityKeyboard);
+                Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative(
+                                driveDirectAngleKeyboard);
 
-    }
+                driverXbox.start().onTrue(m_IntakeSubsystem.stopIntakeMotorCommand());
+                driverXbox.back().onTrue((Commands.runOnce(drivebase::zeroGyro)));
 
-    /**
-     * Use this to pass the autonomous command to the main {@link Robot} class.
-     *
-     * @return the command to run in autonomous
-     */
-    public Command getAutonomousCommand() {
-        // Pass in the selected auto from the SmartDashboard as our desired autnomous
-        // commmand
-        return autoChooser.getSelected();
-    }
+                driverXbox.leftTrigger().onTrue(m_IntakeSubsystem
+                                .startIntakeMotorCommand(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_FAST));
+                driverXbox.rightBumper()
+                                .whileTrue(m_IntakeSubsystem.reverseIntakeMotorCommand()
+                                                .andThen(m_ShooterSubsystem.reverseIndexerAndRampMotorRPMCommand())
+                                                .andThen(m_ShooterSubsystem.reverseShooterCommand()))
+                                .onFalse(m_IntakeSubsystem.stopIntakeMotorCommand()
+                                                .andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand())
+                                                .andThen(m_ShooterSubsystem.setShooterMotorsRPMCommand(
+                                                                Constants.ShooterConstants.IDLE_SHOOTER_RPM)));
+                // command for
+                // full shooting system including linear actuators
+                //driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand().andThen(new WaitCommand(1.5)));
+                                //.andThen(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()));
+                driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
+                driverXbox.leftBumper().onTrue(m_IntakeSubsystem
+                                .startIntakeMotorCommand(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_SLOW));
+                // driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE,
+                // Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY).repeatedly());
 
-    public void setMotorBrake(boolean brake) {
-        drivebase.setMotorBrake(brake);
-    }
+                driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand());
+                driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand());
+                // driverXbox.povDown().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY));
+                // driverXbox.povUp().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT));
+                // driverXbox.povLeft().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE));
 
-    public Command aimAtHopperCommand(DoubleSupplier xSup, DoubleSupplier ySup) {
-        try (PIDController aimPIDs = new PIDController(0.3, 0, 0.001)) {
-            aimPIDs.setTolerance(1.0);
+                driverXbox.povDown().onTrue(m_IntakeSubsystem.deployIntakeCommand());
+                driverXbox.povUp().onTrue(m_IntakeSubsystem.retractIntakeCommand());
+                // driverXbox.povRight().whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase,
+                // driverXbox));
 
-            return Commands.run(() -> {
+                // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand());
+                driverXbox.x().onTrue(setIdleShooterRPMCommand().andThen(m_IntakeSubsystem.deployIntakeCommand()));
+                // .andThen(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY)));
+                // driverXbox.a().whileTrue(aimAtHopperCommand(() -> -driverXbox.getLeftY(),
+                // () -> -driverXbox.getLeftX()));
 
-                double xSpeed = xSup.getAsDouble();
-                double ySpeed = ySup.getAsDouble();
+                // driverXbox.b().whileTrue(m_TargetingSubsystems.aimAndRangeToPose(Constants.TargetingConstants.LEFT_CLIMB_POSE));
 
-                double rot = 0.0;
+                
+                topButtons.button(12).whileTrue(m_ShooterSubsystem.testLeftShooterCommand())
+                  .onFalse(m_ShooterSubsystem.stopLeftShooterCommand());
+                topButtons.button(11).whileTrue(m_ShooterSubsystem.testCenterShooterCommand())
+                  .onFalse(m_ShooterSubsystem.stopCenterShooterCommand());
+                topButtons.button(10).whileTrue(m_ShooterSubsystem.testRightShooterCommand())
+                  .onFalse(m_ShooterSubsystem.stopRightShooterCommand());
+                 
 
-                if (LimelightHelpers.getTV("limelight")) {
-                    double tx = LimelightHelpers.getTX("limelight");
-                    rot = aimPIDs.calculate(tx, 0);
-                    rot = MathUtil.clamp(rot, -1.5, 1.5);
+                topButtons.axisGreaterThan(1, 0.3)
+                                .toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(
+                                                Constants.IntakeConstants.INTAKE_MANUAL_SPEED * -5))
+                                .toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
+                topButtons.axisGreaterThan(1, -0.3)
+                                .toggleOnTrue(m_IntakeSubsystem
+                                                .rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED))
+                                .toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0));
+                topButtons.axisGreaterThan(0, 0.5).onTrue(m_ClimberSubsystem.setClimberSpeedCommand(0.4))
+                                .onFalse(m_ClimberSubsystem.setClimberSpeedCommand(0));
+                topButtons.axisLessThan(0, -0.8).onTrue(m_ClimberSubsystem.setClimberSpeedCommand(-0.4))
+                                .onFalse(m_ClimberSubsystem.setClimberSpeedCommand(0));
+                topButtons.button(3).onTrue(killAllCommand());
+                topButtons.button(6).whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox));
+                topButtons.button(1)
+                                .onTrue(drivebase.driveToClimbPoseOffsetted(
+                                                Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED,
+                                                Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED));
+                topButtons.button(2)
+                                .onTrue(drivebase.driveToClimbPoseOffsetted(
+                                                Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED,
+                                                Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED));
+                topButtons.button(4).whileTrue(m_ShooterSubsystem.setIndexerAndRampMotorRPMCommand())
+                                .onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
+
+                bottomButtons.button(9)
+                                .onTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly().withTimeout(1.5));
+                bottomButtons.button(3).whileTrue(m_IntakeSubsystem.sysIdDynamic(SysIdRoutine.Direction.kForward));
+                bottomButtons.button(7).whileTrue(m_IntakeSubsystem.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+                bottomButtons.button(4).whileTrue(m_IntakeSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+                bottomButtons.button(8).whileTrue(m_IntakeSubsystem.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+
+                // topButtons.button(1).onTrue(drivebase.driveToPose(Constants.))
+
+                if (RobotBase.isSimulation()) {
+                        drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
+                } else {
+                        drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity);
+                }
+
+                if (Robot.isSimulation()) {
+                        Pose2d target = new Pose2d(new Translation2d(1, 4),
+                                        Rotation2d.fromDegrees(90));
+                        // drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
+                        driveDirectAngleKeyboard.driveToPose(() -> target,
+                                        new ProfiledPIDController(5,
+                                                        0,
+                                                        0,
+                                                        new Constraints(5, 2)),
+                                        new ProfiledPIDController(5,
+                                                        0,
+                                                        0,
+                                                        new Constraints(Units.degreesToRadians(360),
+                                                                        Units.degreesToRadians(180))));
+                        driverXbox.start()
+                                        .onTrue(Commands.runOnce(() -> drivebase
+                                                        .resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
+                        driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
+                        driverXbox.button(2)
+                                        .whileTrue(Commands.runEnd(
+                                                        () -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
+                                                        () -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
+
+                        // driverXbox.b().whileTrue(
+                        // drivebase.driveToPose(
+                        // new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0)))
+                        // );
+
+                }
+                if (DriverStation.isTest()) {
+                        drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); // Overrides drive command
+                                                                                         // above!
+
+                        driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
+                        driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
+                        driverXbox.back().whileTrue(drivebase.centerModulesCommand());
+                        driverXbox.leftBumper().onTrue(Commands.none());
+                        driverXbox.rightBumper().onTrue(Commands.none());
+                } else {
+                        // driverXbox.x().onTrue(Commands.runOnce(drivebase::addFakeVisionReading));
+                        // driverXbox.start().whileTrue(Commands.none());
+                        // driverXbox.back().whileTrue(Commands.none());
+                        // driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock,
+                        // drivebase).repeatedly());
+                        // driverXbox.rightBumper().onTrue(Commands.none());
                 }
 
-                drivebase.drive(new Translation2d(xSpeed, ySpeed), rot, false);
-            },
-                    drivebase);
         }
-    }
 
-    public SwerveSubsystem getSwerveDriveBase() {
-        return drivebase;
-    }
+        /**
+         * Use this to pass the autonomous command to the main {@link Robot} class.
+         *
+         * @return the command to run in autonomous
+         */
+        public Command getAutonomousCommand() {
+                // Pass in the selected auto from the SmartDashboard as our desired autnomous
+                // commmand
+                return autoChooser.getSelected();
+        }
 
-    public CommandXboxController getDriverXbox() {
-        return driverXbox;
-    }
+        public void setMotorBrake(boolean brake) {
+                drivebase.setMotorBrake(brake);
+        }
 
-    public SequentialCommandGroup fullShootFuelSystemCommand = new SequentialCommandGroup(
-            // m_ShooterSubsystem.moveActuatorCommand(Constants.ShooterConstants.DESIRED_POTENTIOMETER_DISTANCE),
-            m_ShooterSubsystem.shootFuelCommand(), m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly());
+        public SwerveSubsystem getSwerveDrive() {
+                return drivebase;
+        }
 
-}
+        public void killAll() {
+                CommandScheduler.getInstance().cancelAll();
+        }
+
+        public Command killAllCommand() {
+                return Commands.runOnce(() -> killAll());
+        }
+
+        public Command setIdleShooterRPMCommand() {
+                return Commands.runOnce(() -> ShooterSubsystem
+                                .setShooterMotorsRPM(Constants.ShooterConstants.IDLE_SHOOTER_RPM))
+                                .andThen(m_ShooterSubsystem.stopIndexerAndRampMotorCommand());
+        }
+
+}

src/main/java/frc/robot/subsystems/ClimberSubsystem.java

@@ -1,6 +1,12 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.ClosedLoopConfig;
+import com.revrobotics.spark.config.SparkFlexConfig;
 
 import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -8,48 +14,61 @@ import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
-public class ClimberSubsystem extends SubsystemBase{
-    private static TalonFX climberMotor = new TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID);
-    private static Servo climberRatchet = new Servo(Constants.ClimberConstants.RATCHET_PWM_PORT);
+public class ClimberSubsystem extends SubsystemBase {
+  // private static TalonFX climberMotor = new
+  // TalonFX(Constants.ClimberConstants.CLIMB_MOTOR_ID);
+  private static SparkFlex climberMotor = new SparkFlex(Constants.ClimberConstants.CLIMB_MOTOR_ID,
+      MotorType.kBrushless);
+  private static SparkClosedLoopController climberMotorPIDController;
+  public static SparkFlexConfig climberMotorConfig = new SparkFlexConfig();
 
-    public void liftRobot() {
-        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED);
-    }
-
-    public void lowerRobot() {
-        climberMotor.set(Constants.ClimberConstants.CLIMBER_SPEED * -1);
-    }
-
-    public void stopClimber() {
-        climberMotor.set(0);
-    }
-
-    public Command liftRobotCommand() {
-        return runOnce(() -> toggleRatchet(true)).andThen(() -> liftRobot());
-    }
-
-    public Command lowerRobotCommand() {
-        return runOnce(() -> toggleRatchet(false)).andThen(() -> lowerRobot());
-    }
-
-    public Command stopClimberCommand() {
-        return runOnce(() -> stopClimber());
-    }
-
-    public static void toggleRatchet(boolean toggle) {
-    if (toggle == true) {
-      climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_LOCK_ANGLE);
-    } else
-      climberRatchet.setAngle(Constants.ClimberConstants.RATCHET_UNLOCK_ANGLE);
+  public ClimberSubsystem() {
+    climberMotorConfig.closedLoop.pid(Constants.ClimberConstants.CLIMBER_PID_P,
+        Constants.ClimberConstants.CLIMBER_PID_I,
+        Constants.ClimberConstants.CLIMBER_PID_D);
+    climberMotorConfig.smartCurrentLimit(80);
+    climberMotorConfig.openLoopRampRate(0);
+    climberMotorConfig.closedLoopRampRate(0);
+    climberMotor.configure(climberMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+        com.revrobotics.PersistMode.kNoPersistParameters);
+    climberMotorPIDController = climberMotor.getClosedLoopController();
   }
 
-  public Command toggleRatchetCommand(boolean toggle) {
-    return runOnce(() -> toggleRatchet(toggle));
+  public void setClimberSpeed(double speed) {
+    climberMotor.set(speed);
+  }
+
+  public Command setClimberSpeedCommand(double speed) {
+    return runOnce(() -> setClimberSpeed(speed));
+  }
+  public void liftRobot() {
+    climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LIFTED_SETPOINT_VALUE, ControlType.kPosition);
+  }
+
+  public static void lowerRobot() {
+    climberMotorPIDController.setSetpoint(Constants.ClimberConstants.CLIMBER_LOWERED_SETPOINT_VALUE, ControlType.kPosition);
+  }
+
+
+  public void stopClimber() {
+    climberMotor.set(0);
+  }
+
+  public Command liftRobotCommand() {
+    return runOnce(() -> liftRobot());
+  }
+
+  public Command lowerRobotCommand() {
+    return runOnce(() -> lowerRobot());
+  }
+
+  public Command stopClimberCommand() {
+    return runOnce(() -> stopClimber());
   }
 
   @Override
-  public void periodic()
-  {
-    SmartDashboard.putNumber("Ratchet Position" , climberRatchet.getPosition());
+  public void periodic() {
+    SmartDashboard.putNumber("Climber Motor Encoder", climberMotor.getEncoder().getPosition());
+    SmartDashboard.putNumber("Climber motor power", climberMotor.get());
   }
 }

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -1,94 +1,214 @@
-package frc.robot.subsystems;
-
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
-import frc.robot.Constants;
-
-import com.revrobotics.spark.ClosedLoopSlot;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkClosedLoopController;
-import com.revrobotics.spark.SparkFlex;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkFlexConfig;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.spark.SparkBase.ControlType;
-
-public class IntakeSubsystem extends SubsystemBase {
-
-    private static SparkFlex intakeMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID,
-            MotorType.kBrushless);
-
-    private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID,
-            MotorType.kBrushless);
-    private static SparkClosedLoopController intakeRotatorPIDController;
-    public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
-
-    public IntakeSubsystem() {
-        intakeRotatorConfig.closedLoop.pid(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P,
-                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I,
-                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D);
-        intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
-                com.revrobotics.PersistMode.kNoPersistParameters);
-        intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
-    }
-
-    public void startIntakeMotor() {
-        intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED);
-    }
-
-    public void reverseIntakeMotor() {
-        intakeMotor.set(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_SPEED * -1);
-    }
-
-    public void stopIntakeMotor() {
-        intakeMotor.set(0);
-    }
-
-    public Command startIntakeMotorCommand() {
-        return runOnce(() -> startIntakeMotor());
-    }
-
-    public Command reverseIntakeMotorCommand() {
-        return runOnce(() -> reverseIntakeMotor());
-    }
-
-    public Command stopIntakeMotorCommand() {
-        return runOnce(() -> stopIntakeMotor());
-    }
-
-    public void deployIntake() {
-        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
-                ControlType.kPosition);
-    }
-
-    public Command deployintakeCommand() {
-        return runOnce(() -> deployIntake());
-    }
-
-    public void retractIntake() {
-        intakeRotatorPIDController.setSetpoint(0, ControlType.kPosition);
-    }
-
-    public Command retractIntakeCommand() {
-        return runOnce(() -> retractIntake());
-    }
-
-    public void assistFuelIntake() {
-        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
-                ControlType.kPosition);
-    }
-
-    public Command assistFuelIntakeCommand() {
-        return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(2)).andThen(deployintakeCommand())
-                .andThen(new WaitCommand(2));
-    }
-
-    @Override
-    public void periodic() {
-        SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition());
-    }
-}
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.Constants;
+
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkFlexConfig;
+
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.VoltsPerRadianPerSecond;
+
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase.ControlType;
+
+public class IntakeSubsystem extends SubsystemBase {
+
+    private static SparkFlex intakeWheelsMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkFlex intakeRotatorMotor = new SparkFlex(Constants.IntakeConstants.INTAKE_ROTATOR_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private final TrapezoidProfile.Constraints m_Constraints = new TrapezoidProfile.Constraints(0.3, .5);
+    private final ProfiledPIDController intakeRotatorProfiledPIDController;
+    private static TrapezoidProfile.State goalState = new TrapezoidProfile.State(
+            Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT, 0);
+
+    SysIdRoutine routine = new SysIdRoutine(new SysIdRoutine.Config(),
+            new SysIdRoutine.Mechanism(intakeRotatorMotor::setVoltage,
+            log -> log.motor("arm").voltage(Volts.of(intakeRotatorMotor.getAppliedOutput() * 12))
+                    .angularPosition(Radians.of(intakeRotatorEncoder.get() * 2 * Math.PI))
+                    .angularVelocity(RadiansPerSecond.of(0)),
+                    this, "armSysId"));
+
+    private static SparkClosedLoopController intakeRotatorPIDController;
+    public static SparkFlexConfig intakeRotatorConfig = new SparkFlexConfig();
+
+    private static SparkClosedLoopController intakeWheelsMotorPIDController;
+    public static SparkFlexConfig intakeWheelsMotorConfig = new SparkFlexConfig();
+
+    public static DutyCycleEncoder intakeRotatorEncoder = new DutyCycleEncoder(1);
+
+    private static double encoderValue = intakeRotatorEncoder.get();
+
+    public IntakeSubsystem() {
+        intakeRotatorProfiledPIDController = new ProfiledPIDController(
+                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P,
+                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I,
+                Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D,
+                m_Constraints,
+                0.02);
+        intakeRotatorProfiledPIDController.setTolerance(0.02);
+        intakeRotatorProfiledPIDController.setGoal(goalState);
+
+        intakeRotatorConfig.closedLoop
+                // Slot 0
+                .p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P)
+                .i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I)
+                .d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D)
+
+                // Slot 1
+                .p(.05, ClosedLoopSlot.kSlot1)
+                .i(0, ClosedLoopSlot.kSlot1)
+                .d(.05, ClosedLoopSlot.kSlot1)
+
+                // Slot 2
+                .p(.8, ClosedLoopSlot.kSlot2)
+                .i(.0, ClosedLoopSlot.kSlot2)
+                .d(0.8, ClosedLoopSlot.kSlot2);
+
+        intakeRotatorConfig.smartCurrentLimit(80);
+        intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+                com.revrobotics.PersistMode.kNoPersistParameters);
+
+        intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController();
+
+        intakeWheelsMotorConfig.closedLoop.pid(Constants.IntakeConstants.INTAKE_MOTOR_P,
+                Constants.IntakeConstants.INTAKE_MOTOR_I,
+                Constants.IntakeConstants.INTAKE_MOTOR_D);
+        intakeWheelsMotorConfig.smartCurrentLimit(70);
+        intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+                com.revrobotics.PersistMode.kNoPersistParameters);
+
+        intakeRotatorMotor.getEncoder().setPosition(-6);
+        intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController();
+    }
+
+    public void goToPosition(double goalPosition) {
+        goalState = new TrapezoidProfile.State(goalPosition, 0);
+    }
+
+    public Command goToPositionCommand(double goalPosition) {
+        return runOnce(() -> goToPosition(goalPosition));
+    }
+
+    public void rotateIntake(double speed) {
+        intakeRotatorMotor.set(speed);
+    }
+
+    public Command rotateIntakeCommand(double speed) {
+        return runOnce(() -> intakeRotatorMotor.set(speed));
+    }
+
+    public void startIntakeMotor(double speed) {
+        intakeWheelsMotorPIDController.setSetpoint(speed,
+                ControlType.kVelocity);
+    }
+
+    public void reverseIntakeMotor() {
+        intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM_FAST * -1,
+                ControlType.kVelocity);
+    }
+
+    public void stopIntakeMotor() {
+        intakeWheelsMotor.set(0);
+    }
+
+    public Command startIntakeMotorCommand(double speed) {
+        return runOnce(() -> startIntakeMotor(speed));
+    }
+
+    public Command reverseIntakeMotorCommand() {
+        return runOnce(() -> reverseIntakeMotor());
+    }
+
+    public Command stopIntakeMotorCommand() {
+        return runOnce(() -> stopIntakeMotor());
+    }
+
+    /*
+     * public Command assistFuelIntakeCommand(double deployedPosition, double
+     * assistPosition) {
+     * return runOnce(() -> goToPositionCommand(assistPosition).andThen(new
+     * WaitCommand(1.5))
+     * .andThen(goToPositionCommand(deployedPosition)).andThen(new
+     * WaitCommand(1.5)));
+     * }
+     */
+
+    public static void resetIntakeRotationEncoder() {
+        intakeRotatorMotor.getEncoder().setPosition(-5);
+    }
+
+    public void deployIntake() {
+        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
+                ControlType.kPosition, ClosedLoopSlot.kSlot0);
+    }
+
+    public void deployIntakeAssist() {
+        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE,
+                ControlType.kPosition, ClosedLoopSlot.kSlot2);
+    }
+
+    public Command deployIntakeCommand() {
+        return runOnce(() -> deployIntake());
+    }
+
+    public void retractIntake() {
+        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_RETRACT_ENCODER_VALUE,
+                ControlType.kPosition, ClosedLoopSlot.kSlot1);
+    }
+
+    public Command retractIntakeCommand() {
+        return runOnce(() -> retractIntake());
+    }
+
+    public void assistFuelIntake() {
+        intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE,
+                ControlType.kPosition, ClosedLoopSlot.kSlot2);
+    }
+
+    public Command assistFuelIntakeCommand() {
+        return runOnce(() -> assistFuelIntake()).andThen(new WaitCommand(.5)).andThen(deployIntakeCommand())
+                .andThen(new WaitCommand(.5));
+    }
+
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return routine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return routine.dynamic(direction);
+    }
+
+    @Override
+    public void periodic() {
+        encoderValue = intakeRotatorEncoder.get();
+        //intakeRotatorMotor.setVoltage(intakeRotatorProfiledPIDController.calculate(encoderValue, goalState));
+        SmartDashboard.putNumber("Intake Rotator Motor Encoder", intakeRotatorMotor.getEncoder().getPosition());
+        SmartDashboard.putNumber("Intake Rotator Throughbore Encoder Value", intakeRotatorEncoder.get());
+        SmartDashboard.putNumber("Voltage Deploy", intakeRotatorProfiledPIDController.calculate(encoderValue,
+                goalState));
+    }
+}

src/main/java/frc/robot/subsystems/LEDSubsystem.java

@@ -0,0 +1,195 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import static edu.wpi.first.units.Units.Percent;
+import static edu.wpi.first.units.Units.Second;
+
+import java.util.Optional;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.AddressableLEDBufferView;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.LEDPattern;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class LEDSubsystem extends SubsystemBase {
+
+  AddressableLED m_LED;
+
+  AddressableLEDBuffer m_Buffer;
+  AddressableLEDBufferView m_Left;
+  AddressableLEDBufferView m_Right;
+
+  LEDPattern hubActiveColor = LEDPattern.solid(Color.kGreen);
+  LEDPattern hubActiveBlinkPattern = hubActiveColor.blink(Second.of(0.5));
+
+  LEDPattern hubInactiveColor = LEDPattern.solid(Color.kRed);
+  LEDPattern hubInactiveBlinkPattern = hubInactiveColor.blink(Second.of(0.1));
+
+  LEDPattern allianceShiftColor = LEDPattern.gradient(LEDPattern.GradientType.kContinuous, Color.kTeal, Color.kMagenta);
+  LEDPattern allianceShiftPattern = allianceShiftColor.scrollAtRelativeSpeed(Percent.per(Second).of(100));
+
+  LEDPattern transitionColor = LEDPattern.solid(Color.kYellow);
+  LEDPattern transitionBlinkPattern = transitionColor.blink(Second.of(0.2));
+
+  LEDPattern endGameColor = LEDPattern.solid(Color.kOrangeRed);
+  LEDPattern endGameBlinkPattern = endGameColor.blink(Second.of(0.2));
+
+  LEDPattern rainbow = LEDPattern.rainbow(255, 128);
+  LEDPattern rainbowScroll = rainbow.scrollAtRelativeSpeed(Percent.per(Second).of(100));
+
+  public LEDSubsystem() {
+    m_LED = new AddressableLED(Constants.LEDConstants.LED_PWM_PORT);
+    m_Buffer = new AddressableLEDBuffer(44);    
+    m_LED.setLength(m_Buffer.getLength());
+
+    m_Left = m_Buffer.createView(0, 21);
+    m_Right = m_Buffer.createView(22, 43);
+
+    hubInactiveBlinkPattern.applyTo(m_Left);
+    hubActiveBlinkPattern.applyTo(m_Right);
+
+    m_LED.setData(m_Buffer);
+    m_LED.start();
+  }
+
+  private double matchTime;
+  private boolean isHubActive;
+
+  public void setLEDPeriod() {
+    if(DriverStation.isAutonomous())
+    {
+      allianceShiftPattern.applyTo(m_Buffer);
+    }
+
+    else
+    {
+      if (matchTime <= 140 && matchTime > 132) // transition
+      {
+        transitionBlinkPattern.applyTo(m_Left);
+      }
+
+      if(matchTime <= 132 && matchTime > 130)
+      {
+        hubInactiveBlinkPattern.applyTo(m_Buffer);
+      }
+
+      if(matchTime <= 107 && matchTime > 105 && isHubActive == false)
+      {
+        hubInactiveBlinkPattern.applyTo(m_Buffer);
+      }
+
+      if(matchTime <= 82 && matchTime > 80 && isHubActive == false)
+      {
+        hubInactiveBlinkPattern.applyTo(m_Buffer);
+      }
+
+      if(matchTime <= 57 && matchTime > 55 && isHubActive == false)
+      {
+        hubInactiveBlinkPattern.applyTo(m_Buffer);
+      }
+
+      if(matchTime <= 32 && matchTime > 30 && isHubActive == false)
+      {
+        hubInactiveBlinkPattern.applyTo(m_Buffer);
+      }
+
+      if (matchTime <= 30) // endgame
+      {
+        endGameBlinkPattern.applyTo(m_Left);
+      }
+
+
+      allianceShiftPattern.applyTo(m_Buffer);
+    }
+    
+  }
+
+  public void setLEDHubActive() {
+    if (isHubActive) {
+
+    }
+
+    else {
+
+    }
+  }
+
+  public boolean isHubActive() {
+    Optional<Alliance> alliance = DriverStation.getAlliance();
+    // If we have no alliance, we cannot be enabled, therefore no hub.
+    if (alliance.isEmpty()) {
+      return false;
+    }
+    // Hub is always enabled in autonomous.
+    if (DriverStation.isAutonomousEnabled()) {
+      return true;
+    }
+    // At this point, if we're not teleop enabled, there is no hub.
+    if (!DriverStation.isTeleopEnabled()) {
+      return false;
+    }
+
+    // We're teleop enabled, compute.
+    double matchTime = DriverStation.getMatchTime();
+    String gameData = DriverStation.getGameSpecificMessage();
+    // If we have no game data, we cannot compute, assume hub is active, as its
+    // likely early in teleop.
+    if (gameData.isEmpty()) {
+      return true;
+    }
+    boolean redInactiveFirst = false;
+    switch (gameData.charAt(0)) {
+      case 'R' -> redInactiveFirst = true;
+      case 'B' -> redInactiveFirst = false;
+      default -> {
+        // If we have invalid game data, assume hub is active.
+        return true;
+      }
+    }
+
+    // Shift was is active for blue if red won auto, or red if blue won auto.
+    boolean shift1Active = switch (alliance.get()) {
+      case Red -> !redInactiveFirst;
+      case Blue -> redInactiveFirst;
+    };
+
+    if (matchTime > 130) {
+      // Transition shift, hub is active.
+      return true;
+    } else if (matchTime > 105) {
+      // Shift 1
+      return shift1Active;
+    } else if (matchTime > 80) {
+      // Shift 2
+      return !shift1Active;
+    } else if (matchTime > 55) {
+      // Shift 3
+      return shift1Active;
+    } else if (matchTime > 30) {
+      // Shift 4
+      return !shift1Active;
+    } else {
+      // End game, hub always active.
+      return true;
+    }
+  }
+
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    matchTime = DriverStation.getMatchTime();
+    isHubActive = isHubActive();
+    setLEDPeriod();
+
+    m_LED.setData(m_Buffer);
+  }
+}

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -1,124 +1,269 @@
-package frc.robot.subsystems;
-
-import edu.wpi.first.wpilibj.DoubleSolenoid;
-import edu.wpi.first.wpilibj.PneumaticsModuleType;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
-
-import java.util.function.BooleanSupplier;
-
-import com.ctre.phoenix6.controls.Follower;
-import com.revrobotics.RelativeEncoder;
-import com.revrobotics.spark.SparkBase.ControlType;
-import com.revrobotics.spark.SparkBase.PersistMode;
-import com.revrobotics.spark.SparkBase.ResetMode;
-import com.revrobotics.spark.SparkBase;
-import com.revrobotics.spark.SparkFlex;
-import com.revrobotics.spark.SparkMax;
-import com.revrobotics.spark.SparkLowLevel.MotorType;
-import com.revrobotics.spark.config.SparkFlexConfig;
-import com.revrobotics.spark.config.SparkBaseConfig;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
-import frc.robot.LimelightHelpers;
-import edu.wpi.first.wpilibj.AnalogPotentiometer;
-
-public class ShooterSubsystem extends SubsystemBase {
-    private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
-            MotorType.kBrushless);
-
-    private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID,
-            MotorType.kBrushless);
-
-    private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID,
-            MotorType.kBrushless);
-
-    private static SparkFlex indexerMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
-            MotorType.kBrushless);
-
-    //private static SparkMax leftActuatorMotor = new SparkMax(Constants.ShooterConstants.LEFT_ACTUATOR_PWM_PORT,
-           // MotorType.kBrushless);
-
-    //private static SparkMax rightActuatorMotor = new SparkMax(Constants.ShooterConstants.RIGHT_ACTUATOR_PWM_PORT,
-            //MotorType.kBrushless);
-
-    //private static AnalogPotentiometer leftPotentiometer = new AnalogPotentiometer(0, 1, 0);
-    //private static AnalogPotentiometer rightPotentiometer = new AnalogPotentiometer(0, 1, 0);
-
-    private static double currentPotentiometerPosition; // might need second value for the right potentiometer
-
-    public void startShooterMotors() {
-        centerShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
-        leftShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
-        rightShooterMotor.set(Constants.ShooterConstants.SHOOTER_POWER);
-    }
-
-    public double getShooterMotorVelocity() {
-        return leftShooterMotor.getEncoder().getVelocity();
-    }
-
-    public void startIndexerMotor() {
-        // if (LimelightHelpers.getTX("limelight") < 1.5 &&
-        // LimelightHelpers.getTX("limelight") > -1.5) {
-        indexerMotor.set(Constants.ShooterConstants.INDEXER_MOTOR_SPEED);
-        // } else
-        // indexerMotor.set(0);
-    }
-
-    /* public Command shootFuelCommand() {
-        return run(() -> startShooterMotors())
-                .until(() -> {
-                    return (getShooterMotorVelocity() >= Constants.ShooterConstants.SHOOTER_VELOCITY);
-                })
-                .andThen(() -> startIndexerMotor());
-    } */
-
-    
-      public Command shootFuelCommand() {
-      return runOnce(() -> startShooterMotors()).andThen(new WaitCommand(2))
-      .andThen(() -> startIndexerMotor());
-      };
-     
-
-    public void stopShooters() {
-        centerShooterMotor.set(0);
-        leftShooterMotor.set(0);
-        rightShooterMotor.set(0);
-        indexerMotor.set(0);
-    }
-
-    public Command stopShooterCommand() {
-        return runOnce(() -> stopShooters());
-    }
-
-    public void moveActuator(double desiredPotentiometerPosition) {
-        if (desiredPotentiometerPosition > currentPotentiometerPosition) {
-            //TODO: Test for positive or negative power
-            //leftActuatorMotor.set(0.1);
-            //rightActuatorMotor.set(0.1);
-        } else {
-            //leftActuatorMotor.set(-0.1);
-            //rightActuatorMotor.set(-0.1);
-        }
-    }
-
-    public void stopActuator() {
-        //leftActuatorMotor.set(0);
-        //rightActuatorMotor.set(0);
-    }
-
-    public Command moveActuatorCommand(double desiredPotentiometerPosition) {
-        return run(() -> moveActuator(desiredPotentiometerPosition))
-                .until(() -> currentPotentiometerPosition == currentPotentiometerPosition)
-                .andThen(() -> stopActuator());
-    }
-
-    @Override
-    public void periodic() {
-        /* SmartDashboard.putNumber("Left Potentiometer Distance", leftPotentiometer.get());
-        SmartDashboard.putNumber("Right Potentiometer Distance", rightPotentiometer.get());
-        currentPotentiometerPosition = leftPotentiometer.get(); */
-    }
-}
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+import java.util.function.BooleanSupplier;
+
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.swerve.utility.WheelForceCalculator.Feedforwards;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
+
+public class ShooterSubsystem extends SubsystemBase {
+    private static SparkFlex centerShooterMotor = new SparkFlex(Constants.ShooterConstants.CENTER_SHOOTER_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkFlex leftShooterMotor = new SparkFlex(Constants.ShooterConstants.LEFT_SHOOTER_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkFlex rightShooterMotor = new SparkFlex(Constants.ShooterConstants.RIGHT_SHOOTER_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkFlex indexerAndRampMotor = new SparkFlex(Constants.ShooterConstants.INDEXER_MOTOR_ID,
+            MotorType.kBrushless);
+
+    private static SparkClosedLoopController centerShooterMotorPIDController;
+    public static SparkFlexConfig centerShooterMotorConfig = new SparkFlexConfig();
+
+    private static SparkClosedLoopController leftShooterMotorPIDController;
+    public static SparkFlexConfig leftShooterMotorConfig = new SparkFlexConfig();
+
+    private static SparkClosedLoopController rightShooterMotorPIDController;
+    public static SparkFlexConfig rightShooterMotorConfig = new SparkFlexConfig();
+
+    private static SparkClosedLoopController indexerAndRampMotorPIDController;
+    public static SparkFlexConfig indexerAndRampMotorConfig = new SparkFlexConfig();
+
+    public ShooterSubsystem() {
+        centerShooterMotorConfig
+                .voltageCompensation(12.0)
+                .closedLoop
+                .outputRange(-1, 0)
+                .p(Constants.ShooterConstants.SHOOTER_MOTOR_P)
+                .i(Constants.ShooterConstants.SHOOTER_MOTOR_I)
+                .d(Constants.ShooterConstants.SHOOTER_MOTOR_D)
+                .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+                .feedForward
+                .kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
+                .kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
+        centerShooterMotorConfig.smartCurrentLimit(45);
+
+        centerShooterMotor.configure(centerShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+                com.revrobotics.PersistMode.kNoPersistParameters);
+        centerShooterMotorPIDController = centerShooterMotor.getClosedLoopController();
+
+        leftShooterMotorConfig              
+                .voltageCompensation(12.0)
+                .closedLoop
+                .outputRange(-1, 0)
+                .p(Constants.ShooterConstants.SHOOTER_MOTOR_P)
+                .i(Constants.ShooterConstants.SHOOTER_MOTOR_I)
+                .d(Constants.ShooterConstants.SHOOTER_MOTOR_D)
+                .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+                .feedForward
+                .kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
+                .kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
+        leftShooterMotorConfig.smartCurrentLimit(45);
+        leftShooterMotor.configure(leftShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+                com.revrobotics.PersistMode.kNoPersistParameters);
+        leftShooterMotorPIDController = leftShooterMotor.getClosedLoopController();
+
+        rightShooterMotorConfig
+                .voltageCompensation(12.0)
+                .closedLoop
+                .outputRange(-1, 0)
+                .p(Constants.ShooterConstants.SHOOTER_MOTOR_P)
+                .i(Constants.ShooterConstants.SHOOTER_MOTOR_I)
+                .d(Constants.ShooterConstants.SHOOTER_MOTOR_D)
+                .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+                .feedForward
+                .kS(Constants.ShooterConstants.SHOOTER_MOTOR_S)
+                .kV(Constants.ShooterConstants.SHOOTER_MOTOR_V);
+        rightShooterMotorConfig.smartCurrentLimit(45);
+        rightShooterMotor.configure(rightShooterMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+                com.revrobotics.PersistMode.kNoPersistParameters);
+        rightShooterMotorPIDController = rightShooterMotor.getClosedLoopController();
+
+        indexerAndRampMotorConfig.closedLoop.pid(Constants.ShooterConstants.INDEXER_MOTOR_P,
+                0,
+                0);
+        indexerAndRampMotorConfig.smartCurrentLimit(50);
+        indexerAndRampMotor.configure(indexerAndRampMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters,
+                com.revrobotics.PersistMode.kNoPersistParameters);
+        indexerAndRampMotorPIDController = indexerAndRampMotor.getClosedLoopController();
+    }
+
+
+    public void setShooterMotorsRPM() {
+        centerShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
+        leftShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
+        rightShooterMotorPIDController.setSetpoint(Constants.ShooterConstants.SHOOTER_RPM, ControlType.kVelocity);
+    }
+
+    public static void setShooterMotorsRPM(double RPM){
+        centerShooterMotorPIDController.setSetpoint(RPM, ControlType.kVelocity);
+        leftShooterMotorPIDController.setSetpoint(RPM, ControlType.kVelocity);
+        rightShooterMotorPIDController.setSetpoint(RPM, ControlType.kVelocity);
+    }
+
+    public Command setShooterMotorsRPMCommand(double RPM) {
+        return runOnce(() -> setShooterMotorsRPM(RPM));
+    }
+
+    public Command setShooterMotorsRPMAutoCommand()
+    {
+        return runOnce(()-> setShooterMotorsRPM(Constants.ShooterConstants.AUTO_SHOOTER_RPM)).andThen(new WaitCommand(1))
+      .andThen(() -> setIndexerAndRampMotorRPM());
+    }
+
+    // test individual motor code
+    public void setLeftShooterMotorRPM() {
+        leftShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
+    }
+
+    public Command testLeftShooterCommand() {
+        return runOnce(() -> setLeftShooterMotorRPM());
+    }
+
+    public void setRightShooterMotorRPM() {
+        rightShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
+    }
+
+    public Command testRightShooterCommand() {
+        return runOnce(() -> setRightShooterMotorRPM());
+    }
+
+    public void setCenterShooterMotorRPM() {
+        centerShooterMotorPIDController.setSetpoint(-3400, ControlType.kVelocity);
+    }
+
+    public Command testCenterShooterCommand() {
+        return runOnce(() -> setCenterShooterMotorRPM());
+    }
+
+    public void stopLeftShooterMotorRPM() {
+        leftShooterMotor.set(0);
+    }
+
+    public Command stopLeftShooterCommand() {
+        return runOnce(() -> stopLeftShooterMotorRPM());
+    }
+
+    public void stopCenterShooterMotorRPM() {
+        centerShooterMotor.set(0);
+    }
+
+    public Command stopCenterShooterCommand() {
+        return runOnce(() -> stopCenterShooterMotorRPM());
+    }
+
+    public void stopRightShooterMotorRPM() {
+        rightShooterMotor.set(0);
+    }
+
+    public Command stopRightShooterCommand() {
+        return runOnce(() -> stopRightShooterMotorRPM());
+    }
+
+    public double getShooterMotorRPM() {
+        return rightShooterMotor.getEncoder().getVelocity();
+    }
+
+    public void setIndexerAndRampMotorRPM() {
+        indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM,
+                ControlType.kVelocity);
+    }
+
+
+    public void reverseIndexerAndRampMotorRPM() {
+        indexerAndRampMotorPIDController.setSetpoint(Constants.ShooterConstants.INDEXER_AND_RAMP_MOTOR_RPM * -1,
+                ControlType.kVelocity);
+    }
+
+    public Command reverseIndexerAndRampMotorRPMCommand() {
+        return runOnce(() -> reverseIndexerAndRampMotorRPM());
+    }
+
+    public Command setIndexerAndRampMotorRPMCommand() {
+        return runOnce(() -> setIndexerAndRampMotorRPM());
+    }
+
+    public Command stopIndexerAndRampMotorCommand() {
+        return runOnce(() -> indexerAndRampMotor.set(0));
+    }
+
+    /*public Command shootFuelCommand() {
+        return runOnce(() -> setShooterMotorsRPM())
+                .until(() -> {
+                    return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM);
+                })
+                .andThen(() -> setIndexerAndRampMotorRPM());
+    }*/
+
+    
+    public Command shootFuelCommand() {
+      return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(1))
+      .andThen(() -> setIndexerAndRampMotorRPM());
+      };
+
+    /*  public Command shootFuelCommand() {
+      return runOnce(() -> setShooterMotorsRPM())
+        .andThen(new WaitUntilCommand(() -> {
+            return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM);
+        }))
+        .andThen(() -> setIndexerAndRampMotorRPM());
+    };*/
+     
+
+    public void stopShooters() {
+
+        centerShooterMotor.set(0);
+        leftShooterMotor.set(0);
+        rightShooterMotor.set(0);
+        indexerAndRampMotor.set(0);
+
+    }
+
+    public Command stopShooterCommand() {
+        return runOnce(() -> stopShooters());
+    }
+
+    public void reverseShooter()
+    {
+        centerShooterMotor.set(.4);
+        leftShooterMotor.set(0.4);
+        rightShooterMotor.set(0.4);
+    }
+    public Command reverseShooterCommand()
+    {
+        return runOnce(()-> reverseShooter());
+    }
+
+    @Override
+    public void periodic() {
+
+        SmartDashboard.putString("Shooter Velocity", "Left: " 
+        + leftShooterMotor.getEncoder().getVelocity() 
+        + "  Center:" + centerShooterMotor.getEncoder().getVelocity() 
+        + "  Right: " + rightShooterMotor.getEncoder().getVelocity());
+
+    }
+}

src/main/java/frc/robot/subsystems/TargetingSubsystems.java

@@ -1,182 +1,180 @@
-package frc.robot.subsystems;
-
-import java.util.List;
-import java.util.Optional;
-
-import org.photonvision.EstimatedRobotPose;
-import org.photonvision.PhotonCamera;
-import org.photonvision.PhotonPoseEstimator;
-import org.photonvision.PhotonPoseEstimator.PoseStrategy;
-import org.photonvision.targeting.PhotonTrackedTarget;
-
-import com.pathplanner.lib.path.GoalEndState;
-import com.pathplanner.lib.path.PathConstraints;
-import com.pathplanner.lib.path.PathPlannerPath;
-import com.pathplanner.lib.path.PathPoint;
-import com.pathplanner.lib.path.RotationTarget;
-import com.pathplanner.lib.path.Waypoint;
-
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import frc.robot.Constants;
-import frc.robot.LimelightHelpers;
-import frc.robot.subsystems.swervedrive.SwerveSubsystem;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import frc.robot.RobotContainer;
-import frc.robot.Constants;
-
-public class TargetingSubsystems extends SubsystemBase {
-    
-    PhotonCamera photonVision = new PhotonCamera("Arducam_OV9281_USB_Camera");
-    Transform3d BACK_LEFT_CAMERA_OFFSETS = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0));
-    PhotonPoseEstimator photonEstimator = new PhotonPoseEstimator(
-            AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark),
-            BACK_LEFT_CAMERA_OFFSETS);
-    PIDController photonAimPIDController = new PIDController(0.3, 0, 0.001);
-
-    public TargetingSubsystems() {
-        photonAimPIDController.enableContinuousInput(-180, 180);
-    }
-
-    Pose2d currentRobotPose;
-
-    public List<Waypoint> rightClimbWaypoints;
-
-    public Command pathPlanToRightClimbPoseCommand(SwerveSubsystem swerveDrive) {
-        GoalEndState goalEndState = new GoalEndState(0, Constants.TargetingConstants.RIGHT_CLIMB_POSE.getRotation());
-        PathConstraints goToClimbConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
-        currentRobotPose = swerveDrive.getPose();
-        rightClimbWaypoints = PathPlannerPath.waypointsFromPoses(
-                currentRobotPose, Constants.TargetingConstants.RIGHT_CLIMB_POSE);
-
-        PathPlannerPath goToClimbPath = new PathPlannerPath(rightClimbWaypoints, goToClimbConstraints, null,
-                goalEndState);
-        goToClimbPath.preventFlipping = true;
-
-        return swerveDrive.getAutonomousCommand("goToClimbPath");
-    }
-
-    public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
-        return new RunCommand(() -> {
-         currentRobotPose = swerveDrive.getPose();
-
-            Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
-
-            double xError = errorFromDesiredPose.getX();
-            double yError = errorFromDesiredPose.getY();
-            double angleError = errorFromDesiredPose.getRotation().getRadians();
-
-            PIDController xController = new PIDController(1.5, 0, 0);
-            PIDController yController = new PIDController(1.5, 0, 0);
-            PIDController angleController = new PIDController(3.0, 0, 0);
-
-            angleController.enableContinuousInput(-Math.PI, Math.PI);
-
-            double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX());
-            double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY());
-            double angleSpeed = angleController.calculate(currentRobotPose.getRotation().getRadians(),
-                    desiredPose.getRotation().getRadians());
-            
-            swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, true);
-        }, swerveDrive);
-    }
-
-    Command photonAimAtClimb(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) { 
-        return new RunCommand(() -> {
-        double rot = 0.0;
-        var result = photonVision.getLatestResult();
-        if (result.hasTargets()) {
-            double yawError = result.getBestTarget().getYaw();
-            rot = photonAimPIDController.calculate(yawError, 0);
-        }
-
-        rot = MathUtil.clamp(rot, -3.0, 3.0);
-
-        swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1,
-                driverXbox.getLeftX() * -1), rot, true);
-    }, swerveDrive);
-}
-
-
-    public PhotonPoseEstimator getPhotonPoseEstimator() {
-        return photonEstimator;
-    }
-
-    // static public NetworkTable table =
-    // NetworkTableInstance.getDefault().getTable(Constants.LimeLight.LIMELIGHT_NAME);
-    // static public NetworkTableEntry ty = table.getEntry("ty");
-    // static double targetOffsetAngle_Vertical = ty.getDouble(0.0);
-
-    // how many degrees back is your limelight rotated from perfectly vertical?
-    static double limelightMountAngleDegrees = 25.0;
-
-    // distance from the center of the Limelight lens to the floor
-    static double limelightLensHeightInches = 27.5;
-
-    // distance from the target to the floor
-    static double goalHeightInches = 44;
-
-    static double angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY;
-    static double angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
-
-    // calculate distance
-    static double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)
-            / Math.tan(angleToGoalRadians);
-
-    public static double getDistanceFromAprilTag() {
-        angleToGoalDegrees = limelightMountAngleDegrees + Constants.LimeLight.LIMELIGHT_TY;
-        angleToGoalRadians = angleToGoalDegrees * (3.14159 / 180.0);
-        distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)
-                / Math.tan(angleToGoalRadians);
-        return distanceFromLimelightToGoalInches;
-    }
-
-    public void updateRobotPose(SwerveSubsystem swerveDrive) {
-         Optional<EstimatedRobotPose> result = photonEstimator.update(photonVision.getLatestResult());
-
-        if (result.isPresent()) {
-            EstimatedRobotPose estimatedPose = result.get();
-            swerveDrive.getSwerveDrive()
-                    .addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds);
-        }
-    }
-    @Override
-    public void periodic() {
-
-        /*
-         * Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value",
-         * photonVision.getLatestResult().getBestTarget().getYaw());
-         * Shuffleboard.getTab("Vision").add("Photon Vision Pitch Value",
-         * photonVision.getLatestResult().getBestTarget().getPitch());
-         * Shuffleboard.getTab("Vision").add("Limelight TX Value",
-         * LimelightHelpers.getTX("limelight"));
-         * Shuffleboard.getTab("Vision").add("Limelight April Tag ID",
-         * LimelightHelpers.getFiducialID("limelight"));
-         * Shuffleboard.getTab("Vision").addCamera("Limelight", "limelight", null);
-         * Shuffleboard.getTab("Vision").addCamera("Photon",
-         * "Arducam_OV9281_USB_Camera",
-         * "http://photonvision.local:5800");
-         */
-    } 
-}
-
+package frc.robot.subsystems;
+
+import java.lang.StackWalker.Option;
+import java.util.List;
+import java.util.Optional;
+import java.util.function.DoubleSupplier;
+
+import org.dyn4j.geometry.Rotation;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonUtils;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPoint;
+import com.pathplanner.lib.path.RotationTarget;
+import com.pathplanner.lib.path.Waypoint;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import frc.robot.Constants;
+import frc.robot.subsystems.swervedrive.SwerveSubsystem;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardComponent;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import frc.robot.RobotContainer;
+import frc.robot.Constants;
+
+public class TargetingSubsystems extends SubsystemBase {
+
+    PIDController photonAimPIDController = new PIDController(5, 0.01, 0);
+
+    public static Rotation2d hubThetaPose = new Rotation2d();
+    public static Optional<Alliance> alliance = DriverStation.getAlliance();
+
+    public TargetingSubsystems() {
+        photonAimPIDController.enableContinuousInput(-Math.PI, Math.PI);
+
+    }
+
+    Pose2d currentRobotPose;
+
+    public List<Waypoint> pathWaypoints;
+
+    public Command pathPlanToPoseCommand(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
+        GoalEndState goalEndState = new GoalEndState(0, desiredPose.getRotation());
+        PathConstraints pathConstraints = new PathConstraints(3.0, 3.0, 3.0, 6.0, 12.0);
+        currentRobotPose = swerveDrive.getPose();
+        pathWaypoints = PathPlannerPath.waypointsFromPoses(
+                currentRobotPose, desiredPose);
+
+        PathPlannerPath goToDesiredPose = new PathPlannerPath(pathWaypoints, pathConstraints, null,
+                goalEndState);
+        goToDesiredPose.preventFlipping = true;
+
+        return swerveDrive.getAutonomousCommand("goToDesiredPose");
+    }
+
+    public Command aimAndRangeToPose(Pose2d desiredPose, SwerveSubsystem swerveDrive) {
+        return new RunCommand(() -> {
+            currentRobotPose = swerveDrive.getPose();
+
+            Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
+
+            double xError = errorFromDesiredPose.getX();
+            double yError = errorFromDesiredPose.getY();
+            double angleError = errorFromDesiredPose.getRotation().getRadians();
+
+            PIDController xController = new PIDController(1.5, 0, 0);
+            PIDController yController = new PIDController(1.5, 0, 0);
+
+            double xSpeed = xController.calculate(currentRobotPose.getX(), desiredPose.getX());
+            double ySpeed = yController.calculate(currentRobotPose.getY(), desiredPose.getY());
+            double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(),
+                    desiredPose.getRotation().getRadians());
+
+            swerveDrive.drive(new Translation2d(xSpeed, ySpeed), angleSpeed, false);
+        }, swerveDrive);
+    }
+
+    public Command aimAtHubPose(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
+        return new RunCommand(() -> {
+            currentRobotPose = swerveDrive.getPose();
+
+            // Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose);
+
+            Rotation2d angleDifference = PhotonUtils.getYawToPose(currentRobotPose,
+                    Constants.TargetingConstants.allianceHubPose);
+
+            double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(), Constants.TargetingConstants.allianceHubPose.getRotation().getRadians());
+
+            angleSpeed = MathUtil.clamp(angleSpeed, -3.0, 3.0);
+
+            swerveDrive.drive(new Translation2d(driverXbox.getLeftX() * -1, -driverXbox.getLeftY() * -1), angleSpeed,
+                    true);
+        }, swerveDrive);
+    }
+
+    Command photonAimAtAprilTag(SwerveSubsystem swerveDrive, CommandXboxController driverXbox) {
+        return new RunCommand(() -> {
+            double rot = 0.0;
+            var result = Constants.TargetingConstants.RED_PHOTON_CAM.getLatestResult();
+            if (result.hasTargets()) {
+                double yawError = result.getBestTarget().getYaw();
+                rot = photonAimPIDController.calculate(yawError, 0);
+            }
+
+            rot = MathUtil.clamp(rot, -3.0, 3.0);
+
+            swerveDrive.drive(new Translation2d(driverXbox.getLeftY() * -1,
+                    driverXbox.getLeftX() * -1), rot, false);
+        }, swerveDrive);
+    }
+
+    public static void getHubPoseTheta(SwerveSubsystem swerveDrive) {
+        if (alliance.isPresent()) {
+            if (alliance.get() == Alliance.Blue) {
+                hubThetaPose = new Rotation2d(
+                        Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_BLUE - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_BLUE - swerveDrive.getPose().getX()));
+
+                Constants.TargetingConstants.allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE,
+                        Constants.TargetingConstants.HUB_Y_POSE_BLUE, hubThetaPose);
+            }
+
+            else {
+                hubThetaPose = new Rotation2d(
+                        Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_RED - swerveDrive.getPose().getY(),
+                        Constants.TargetingConstants.HUB_X_POSE_RED - swerveDrive.getPose().getX()));
+                Constants.TargetingConstants.allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_RED,
+                        Constants.TargetingConstants.HUB_Y_POSE_RED, hubThetaPose);
+            }
+        }
+
+    }
+
+
+      public static void updateShooterRPM(Pose2d currentRobotPose) {
+      double distance = PhotonUtils.getDistanceToPose(currentRobotPose,
+      Constants.TargetingConstants.allianceHubPose);
+      Constants.ShooterConstants.SHOOTER_RPM = Math.max((-293.84123 * Math.pow(distance, 3))
+      + (1360.01497 * Math.pow(distance, 2))
+      - (2391.17127 * distance)
+      - 1249.22704, -6000); 
+     }
+     
+
+    @Override
+    public void periodic() {
+        alliance = DriverStation.getAlliance();
+        SmartDashboard.putString("Target Hub Pose",
+                Constants.TargetingConstants.allianceHubPose.getX() + " " + Constants.TargetingConstants.allianceHubPose.getY() + " " + Constants.TargetingConstants.allianceHubPose.getRotation());
+
+        SmartDashboard.putString("Hub Pose", "x: " + Constants.TargetingConstants.allianceHubPose.getMeasureX() + "  y: " + Constants.TargetingConstants.allianceHubPose.getY()
+                + "  angle: " + Constants.TargetingConstants.allianceHubPose.getRotation());
+    }
+}

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

@@ -27,14 +27,19 @@ import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 import frc.robot.Constants;
+import frc.robot.subsystems.TargetingSubsystems;
 import frc.robot.subsystems.swervedrive.Vision.Cameras;
 import java.io.File;
 import java.io.IOException;
+import java.lang.reflect.Field;
 import java.util.Arrays;
 import java.util.Optional;
 import java.util.concurrent.atomic.AtomicReference;
@@ -62,13 +67,18 @@ public class SwerveSubsystem extends SubsystemBase {
     /**
      * Enable vision odometry updates while driving.
      */
-    private final boolean visionDriveTest = false;
+    private final boolean visionDriveTest = true;
+
+    
+
+    
 
     /**
      * PhotonVision class to keep an accurate odometry.
      */
     private Vision vision;
 
+    private final Field2d rebuiltField = new Field2d();
     /**
      * Initialize {@link SwerveDrive} with the directory provided.
      *
@@ -117,6 +127,10 @@ public class SwerveSubsystem extends SubsystemBase {
             swerveDrive.stopOdometryThread();
         }
         setupPathPlanner();
+        SmartDashboard.putData("Rebuilt Field", rebuiltField);
+        Constants.TargetingConstants.DRIVE_INTO_CLIMB_CONSTRAINTS = new PathConstraints(1, 4.0,
+                swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(360));
+
     }
 
     /**
@@ -148,6 +162,11 @@ public class SwerveSubsystem extends SubsystemBase {
             swerveDrive.updateOdometry();
             vision.updatePoseEstimation(swerveDrive);
         }
+
+        SmartDashboard.putString("Robot Pose ", "x: " + swerveDrive.getPose().getX() + "\ny: "
+                + swerveDrive.getPose().getY() + "\nrot: " + swerveDrive.getPose().getRotation());
+
+            rebuiltField.setRobotPose(getPose());
     }
 
     @Override
@@ -188,9 +207,9 @@ public class SwerveSubsystem extends SubsystemBase {
                     new PPHolonomicDriveController(
                             // PPHolonomicController is the built in path following controller for holonomic
                             // drive trains
-                            new PIDConstants(5.0, 0.0, 0.0),
+                            new PIDConstants(3.6, 0.0, 0.0),
                             // Translation PID constants
-                            new PIDConstants(3.8, 0.0, 0.0)
+                            new PIDConstants(3.675, 0.0, 0.00)
                     // Rotation PID constants
                     ),
                     config,
@@ -274,6 +293,48 @@ public class SwerveSubsystem extends SubsystemBase {
         );
     }
 
+    public Command driveToClimbPoseOffsetted(Pose2d blueAlliancePose, Pose2d redAlliancePose) {
+
+        Pose2d goal;
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+        if (alliance.get() == Alliance.Blue) {
+            goal = blueAlliancePose;
+        } else {
+            goal = redAlliancePose;
+        }
+        // Create the constraints to use while pathfinding
+        PathConstraints constraints = new PathConstraints(
+                swerveDrive.getMaximumChassisVelocity(), 4.0,
+                swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(720));
+
+        // Since AutoBuilder is configured, we can use it to build pathfinding commands
+        return AutoBuilder.pathfindToPose(
+                goal,
+                constraints,
+                edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec
+        );
+    }
+
+        public Command driveIntoClimbPose(Pose2d blueAlliancePose, Pose2d redAlliancePose) {
+
+        Pose2d goal;
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+        if (alliance.get() == Alliance.Blue) {
+            goal = blueAlliancePose;
+        } else {
+            goal = redAlliancePose;
+        }
+        // Create the constraints to use while pathfinding
+        
+
+        // Since AutoBuilder is configured, we can use it to build pathfinding commands
+        return AutoBuilder.pathfindToPose(
+                goal,
+                Constants.TargetingConstants.DRIVE_INTO_CLIMB_CONSTRAINTS,
+                edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec
+        );
+    }
+
     /**
      * Drive with {@link SwerveSetpointGenerator} from 254, implemented by
      * PathPlanner.

src/main/java/frc/robot/subsystems/swervedrive/Vision.java

@@ -39,60 +39,59 @@ import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 import swervelib.SwerveDrive;
 import swervelib.telemetry.SwerveDriveTelemetry;
-
+import frc.robot.Constants;
 
 /**
- * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken from
+ * Example PhotonVision class to aid in the pursuit of accurate odometry. Taken
+ * from
  * https://gitlab.com/ironclad_code/ironclad-2024/-/blob/master/src/main/java/frc/robot/vision/Vision.java?ref_type=heads
  */
-public class Vision
-{
+public class Vision {
 
   /**
    * April Tag Field Layout of the year.
    */
-  public static final AprilTagFieldLayout fieldLayout                     = AprilTagFieldLayout.loadField(
+  public static final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(
       AprilTagFields.k2026RebuiltAndymark);
   /**
-   * Ambiguity defined as a value between (0,1). Used in {@link Vision#filterPose}.
+   * Ambiguity defined as a value between (0,1). Used in
+   * {@link Vision#filterPose}.
    */
-  private final       double              maximumAmbiguity                = 0.25;
+  private final double maximumAmbiguity = 0.25;
   /**
    * Photon Vision Simulation
    */
-  public              VisionSystemSim     visionSim;
+  public VisionSystemSim visionSim;
   /**
-   * Count of times that the odom thinks we're more than 10meters away from the april tag.
+   * Count of times that the odom thinks we're more than 10meters away from the
+   * april tag.
    */
-  private             double              longDistangePoseEstimationCount = 0;
+  private double longDistangePoseEstimationCount = 0;
   /**
    * Current pose from the pose estimator using wheel odometry.
    */
-  private             Supplier<Pose2d>    currentPose;
+  private Supplier<Pose2d> currentPose;
   /**
    * Field from {@link swervelib.SwerveDrive#field}
    */
-  private             Field2d             field2d;
-
+  private Field2d field2d;
 
   /**
    * Constructor for the Vision class.
    *
-   * @param currentPose Current pose supplier, should reference {@link SwerveDrive#getPose()}
+   * @param currentPose Current pose supplier, should reference
+   *                    {@link SwerveDrive#getPose()}
    * @param field       Current field, should be {@link SwerveDrive#field}
    */
-  public Vision(Supplier<Pose2d> currentPose, Field2d field)
-  {
+  public Vision(Supplier<Pose2d> currentPose, Field2d field) {
     this.currentPose = currentPose;
     this.field2d = field;
 
-    if (Robot.isSimulation())
-    {
+    if (Robot.isSimulation()) {
       visionSim = new VisionSystemSim("Vision");
       visionSim.addAprilTags(fieldLayout);
 
-      for (Cameras c : Cameras.values())
-      {
+      for (Cameras c : Cameras.values()) {
         c.addToVisionSim(visionSim);
       }
 
@@ -104,50 +103,48 @@ public class Vision
    * Calculates a target pose relative to an AprilTag on the field.
    *
    * @param aprilTag    The ID of the AprilTag.
-   * @param robotOffset The offset {@link Transform2d} of the robot to apply to the pose for the robot to position
+   * @param robotOffset The offset {@link Transform2d} of the robot to apply to
+   *                    the pose for the robot to position
    *                    itself correctly.
    * @return The target pose of the AprilTag.
    */
-  public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset)
-  {
+  public static Pose2d getAprilTagPose(int aprilTag, Transform2d robotOffset) {
     Optional<Pose3d> aprilTagPose3d = fieldLayout.getTagPose(aprilTag);
-    if (aprilTagPose3d.isPresent())
-    {
+    if (aprilTagPose3d.isPresent()) {
       return aprilTagPose3d.get().toPose2d().transformBy(robotOffset);
-    } else
-    {
+    } else {
       throw new RuntimeException("Cannot get AprilTag " + aprilTag + " from field " + fieldLayout.toString());
     }
 
   }
 
   /**
-   * Update the pose estimation inside of {@link SwerveDrive} with all of the given poses.
+   * Update the pose estimation inside of {@link SwerveDrive} with all of the
+   * given poses.
    *
    * @param swerveDrive {@link SwerveDrive} instance.
    */
-  public void updatePoseEstimation(SwerveDrive swerveDrive)
-  {
-    if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent())
-    {
+  public void updatePoseEstimation(SwerveDrive swerveDrive) {
+    if (SwerveDriveTelemetry.isSimulation && swerveDrive.getSimulationDriveTrainPose().isPresent()) {
       /*
-       * In the maple-sim, odometry is simulated using encoder values, accounting for factors like skidding and drifting.
+       * In the maple-sim, odometry is simulated using encoder values, accounting for
+       * factors like skidding and drifting.
        * As a result, the odometry may not always be 100% accurate.
-       * However, the vision system should be able to provide a reasonably accurate pose estimation, even when odometry is incorrect.
+       * However, the vision system should be able to provide a reasonably accurate
+       * pose estimation, even when odometry is incorrect.
        * (This is why teams implement vision system to correct odometry.)
-       * Therefore, we must ensure that the actual robot pose is provided in the simulator when updating the vision simulation during the simulation.
+       * Therefore, we must ensure that the actual robot pose is provided in the
+       * simulator when updating the vision simulation during the simulation.
        */
       visionSim.update(swerveDrive.getSimulationDriveTrainPose().get());
     }
-    for (Cameras camera : Cameras.values())
-    {
+    for (Cameras camera : Cameras.values()) {
       Optional<EstimatedRobotPose> poseEst = getEstimatedGlobalPose(camera);
-      if (poseEst.isPresent())
-      {
+      if (poseEst.isPresent()) {
         var pose = poseEst.get();
         swerveDrive.addVisionMeasurement(pose.estimatedPose.toPose2d(),
-                                         pose.timestampSeconds,
-                                         camera.curStdDevs);
+            pose.timestampSeconds,
+            camera.curStdDevs);
       }
     }
 
@@ -156,24 +153,22 @@ public class Vision
   /**
    * Generates the estimated robot pose. Returns empty if:
    * <ul>
-   *  <li> No Pose Estimates could be generated</li>
-   * <li> The generated pose estimate was considered not accurate</li>
+   * <li>No Pose Estimates could be generated</li>
+   * <li>The generated pose estimate was considered not accurate</li>
    * </ul>
    *
-   * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to create the estimate
+   * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and
+   *         targets used to create the estimate
    */
-  public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera)
-  {
+  public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Cameras camera) {
     Optional<EstimatedRobotPose> poseEst = camera.getEstimatedGlobalPose();
-    if (Robot.isSimulation())
-    {
+    if (Robot.isSimulation()) {
       Field2d debugField = visionSim.getDebugField();
       // Uncomment to enable outputting of vision targets in sim.
       poseEst.ifPresentOrElse(
-          est ->
-              debugField
-                  .getObject("VisionEstimation")
-                  .setPose(est.estimatedPose.toPose2d()),
+          est -> debugField
+              .getObject("VisionEstimation")
+              .setPose(est.estimatedPose.toPose2d()),
           () -> {
             debugField.getObject("VisionEstimation").setPoses();
           });
@@ -181,46 +176,39 @@ public class Vision
     return poseEst;
   }
 
-
   /**
-   * Filter pose via the ambiguity and find best estimate between all of the camera's throwing out distances more than
+   * Filter pose via the ambiguity and find best estimate between all of the
+   * camera's throwing out distances more than
    * 10m for a short amount of time.
    *
    * @param pose Estimated robot pose.
    * @return Could be empty if there isn't a good reading.
    */
   @Deprecated(since = "2024", forRemoval = true)
-  private Optional<EstimatedRobotPose> filterPose(Optional<EstimatedRobotPose> pose)
-  {
-    if (pose.isPresent())
-    {
+  private Optional<EstimatedRobotPose> filterPose(Optional<EstimatedRobotPose> pose) {
+    if (pose.isPresent()) {
       double bestTargetAmbiguity = 1; // 1 is max ambiguity
-      for (PhotonTrackedTarget target : pose.get().targetsUsed)
-      {
+      for (PhotonTrackedTarget target : pose.get().targetsUsed) {
         double ambiguity = target.getPoseAmbiguity();
-        if (ambiguity != -1 && ambiguity < bestTargetAmbiguity)
-        {
+        if (ambiguity != -1 && ambiguity < bestTargetAmbiguity) {
           bestTargetAmbiguity = ambiguity;
         }
       }
-      //ambiguity to high dont use estimate
-      if (bestTargetAmbiguity > maximumAmbiguity)
-      {
+      // ambiguity to high dont use estimate
+      if (bestTargetAmbiguity > maximumAmbiguity) {
         return Optional.empty();
       }
 
-      //est pose is very far from recorded robot pose
-      if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1)
-      {
+      // est pose is very far from recorded robot pose
+      if (PhotonUtils.getDistanceToPose(currentPose.get(), pose.get().estimatedPose.toPose2d()) > 1) {
         longDistangePoseEstimationCount++;
 
-        //if it calculates that were 10 meter away for more than 10 times in a row its probably right
-        if (longDistangePoseEstimationCount < 10)
-        {
+        // if it calculates that were 10 meter away for more than 10 times in a row its
+        // probably right
+        if (longDistangePoseEstimationCount < 10) {
           return Optional.empty();
         }
-      } else
-      {
+      } else {
         longDistangePoseEstimationCount = 0;
       }
       return pose;
@@ -228,15 +216,13 @@ public class Vision
     return Optional.empty();
   }
 
-
   /**
    * Get distance of the robot from the AprilTag pose.
    *
    * @param id AprilTag ID
    * @return Distance
    */
-  public double getDistanceFromAprilTag(int id)
-  {
+  public double getDistanceFromAprilTag(int id) {
     Optional<Pose3d> tag = fieldLayout.getTagPose(id);
     return tag.map(pose3d -> PhotonUtils.getDistanceToPose(currentPose.get(), pose3d.toPose2d())).orElse(-1.0);
   }
@@ -248,17 +234,12 @@ public class Vision
    * @param camera Camera to check.
    * @return Tracked target.
    */
-  public PhotonTrackedTarget getTargetFromId(int id, Cameras camera)
-  {
+  public PhotonTrackedTarget getTargetFromId(int id, Cameras camera) {
     PhotonTrackedTarget target = null;
-    for (PhotonPipelineResult result : camera.resultsList)
-    {
-      if (result.hasTargets())
-      {
-        for (PhotonTrackedTarget i : result.getTargets())
-        {
-          if (i.getFiducialId() == id)
-          {
+    for (PhotonPipelineResult result : camera.resultsList) {
+      if (result.hasTargets()) {
+        for (PhotonTrackedTarget i : result.getTargets()) {
+          if (i.getFiducialId() == id) {
             return i;
           }
         }
@@ -273,54 +254,46 @@ public class Vision
    *
    * @return Vision Simulation
    */
-  public VisionSystemSim getVisionSim()
-  {
+  public VisionSystemSim getVisionSim() {
     return visionSim;
   }
 
   /**
-   * Open up the photon vision camera streams on the localhost, assumes running photon vision on localhost.
+   * Open up the photon vision camera streams on the localhost, assumes running
+   * photon vision on localhost.
    */
-  private void openSimCameraViews()
-  {
-    if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE))
-    {
-//      try
-//      {
-//        Desktop.getDesktop().browse(new URI("http://localhost:1182/"));
-//        Desktop.getDesktop().browse(new URI("http://localhost:1184/"));
-//        Desktop.getDesktop().browse(new URI("http://localhost:1186/"));
-//      } catch (IOException | URISyntaxException e)
-//      {
-//        e.printStackTrace();
-//      }
+  private void openSimCameraViews() {
+    if (Desktop.isDesktopSupported() && Desktop.getDesktop().isSupported(Desktop.Action.BROWSE)) {
+      // try
+      // {
+      // Desktop.getDesktop().browse(new URI("http://localhost:1182/"));
+      // Desktop.getDesktop().browse(new URI("http://localhost:1184/"));
+      // Desktop.getDesktop().browse(new URI("http://localhost:1186/"));
+      // } catch (IOException | URISyntaxException e)
+      // {
+      // e.printStackTrace();
+      // }
     }
   }
 
   /**
    * Update the {@link Field2d} to include tracked targets/
    */
-  public void updateVisionField()
-  {
+  public void updateVisionField() {
 
     List<PhotonTrackedTarget> targets = new ArrayList<PhotonTrackedTarget>();
-    for (Cameras c : Cameras.values())
-    {
-      if (!c.resultsList.isEmpty())
-      {
+    for (Cameras c : Cameras.values()) {
+      if (!c.resultsList.isEmpty()) {
         PhotonPipelineResult latest = c.resultsList.get(0);
-        if (latest.hasTargets())
-        {
+        if (latest.hasTargets()) {
           targets.addAll(latest.targets);
         }
       }
     }
 
     List<Pose2d> poses = new ArrayList<>();
-    for (PhotonTrackedTarget target : targets)
-    {
-      if (fieldLayout.getTagPose(target.getFiducialId()).isPresent())
-      {
+    for (PhotonTrackedTarget target : targets) {
+      if (fieldLayout.getTagPose(target.getFiducialId()).isPresent()) {
         Pose2d targetPose = fieldLayout.getTagPose(target.getFiducialId()).get().toPose2d();
         poses.add(targetPose);
       }
@@ -332,95 +305,103 @@ public class Vision
   /**
    * Camera Enum to select each camera
    */
-  enum Cameras
-  {
+  enum Cameras {
     /**
-     * Left Camera
+     * Back Left Camera
      */
-    LEFT_CAM("left",
-             new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(30)),
-             new Translation3d(Units.inchesToMeters(12.056),
-                               Units.inchesToMeters(10.981),
-                               Units.inchesToMeters(8.44)),
-             VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
+    BACK_LEFT_CAMERA("Rear Left Camera",
+        Constants.TargetingConstants.ROBOT_TO_BACK_LEFT_CAM.getRotation(),
+        Constants.TargetingConstants.ROBOT_TO_BACK_LEFT_CAM.getTranslation(),
+        VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
     /**
-     * Right Camera
+     * Back Right Camera
      */
-    RIGHT_CAM("right",
-              new Rotation3d(0, Math.toRadians(-24.094), Math.toRadians(-30)),
-              new Translation3d(Units.inchesToMeters(12.056),
-                                Units.inchesToMeters(-10.981),
-                                Units.inchesToMeters(8.44)),
-              VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
+    BACK_RIGHT_CAM("Rear Right Camera",
+        Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getRotation(),
+        Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getTranslation(),
+        VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)),
     /**
-     * Center Camera
+     * Front Left Camera
      */
-    CENTER_CAM("center",
-               new Rotation3d(0, Units.degreesToRadians(18), 0),
-               new Translation3d(Units.inchesToMeters(-4.628),
-                                 Units.inchesToMeters(-10.687),
-                                 Units.inchesToMeters(16.129)),
-               VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
+    FRONT_LEFT_CAM("Front Left Camera",
+        Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getRotation(),
+        Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getTranslation(),
+        VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));
+      
+
+    /**
+     * Front Right Camera
+     */
+    /*PURPLE_PHOTON_CAM("Front Right Camera",
+        Constants.TargetingConstants.ROBOT_TO_FRONT_RIGHT_CAM.getRotation(),
+        Constants.TargetingConstants.ROBOT_TO_FRONT_RIGHT_CAM.getTranslation(),
+        VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));*/
 
     /**
      * Latency alert to use when high latency is detected.
      */
-    public final  Alert                        latencyAlert;
+    public final Alert latencyAlert;
     /**
      * Camera instance for comms.
      */
-    public final  PhotonCamera                 camera;
+    public final PhotonCamera camera;
     /**
      * Pose estimator for camera.
      */
-    public final  PhotonPoseEstimator          poseEstimator;
+    public final PhotonPoseEstimator poseEstimator;
     /**
      * Standard Deviation for single tag readings for pose estimation.
      */
-    private final Matrix<N3, N1>               singleTagStdDevs;
+    private final Matrix<N3, N1> singleTagStdDevs;
     /**
      * Standard deviation for multi-tag readings for pose estimation.
      */
-    private final Matrix<N3, N1>               multiTagStdDevs;
+    private final Matrix<N3, N1> multiTagStdDevs;
     /**
-     * Transform of the camera rotation and translation relative to the center of the robot
+     * Transform of the camera rotation and translation relative to the center of
+     * the robot
      */
-    private final Transform3d                  robotToCamTransform;
+    private final Transform3d robotToCamTransform;
     /**
      * Current standard deviations used.
      */
-    public        Matrix<N3, N1>               curStdDevs;
+    public Matrix<N3, N1> curStdDevs;
     /**
      * Estimated robot pose.
      */
-    public        Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
+    public Optional<EstimatedRobotPose> estimatedRobotPose = Optional.empty();
 
     /**
      * Simulated camera instance which only exists during simulations.
      */
-    public        PhotonCameraSim              cameraSim;
+    public PhotonCameraSim cameraSim;
     /**
-     * Results list to be updated periodically and cached to avoid unnecessary queries.
+     * Results list to be updated periodically and cached to avoid unnecessary
+     * queries.
      */
-    public        List<PhotonPipelineResult>   resultsList       = new ArrayList<>();
+    public List<PhotonPipelineResult> resultsList = new ArrayList<>();
     /**
      * Last read from the camera timestamp to prevent lag due to slow data fetches.
      */
-    private       double                       lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds);
+    private double lastReadTimestamp = Microseconds.of(NetworkTablesJNI.now()).in(Seconds);
 
     /**
-     * Construct a Photon Camera class with help. Standard deviations are fake values, experiment and determine
+     * Construct a Photon Camera class with help. Standard deviations are fake
+     * values, experiment and determine
      * estimation noise on an actual robot.
      *
-     * @param name                  Name of the PhotonVision camera found in the PV UI.
+     * @param name                  Name of the PhotonVision camera found in the PV
+     *                              UI.
      * @param robotToCamRotation    {@link Rotation3d} of the camera.
-     * @param robotToCamTranslation {@link Translation3d} relative to the center of the robot.
-     * @param singleTagStdDevs      Single AprilTag standard deviations of estimated poses from the camera.
-     * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated poses from the camera.
+     * @param robotToCamTranslation {@link Translation3d} relative to the center of
+     *                              the robot.
+     * @param singleTagStdDevs      Single AprilTag standard deviations of estimated
+     *                              poses from the camera.
+     * @param multiTagStdDevsMatrix Multi AprilTag standard deviations of estimated
+     *                              poses from the camera.
      */
     Cameras(String name, Rotation3d robotToCamRotation, Translation3d robotToCamTranslation,
-            Matrix<N3, N1> singleTagStdDevs, Matrix<N3, N1> multiTagStdDevsMatrix)
-    {
+        Matrix<N3, N1> singleTagStdDevs, Matrix<N3, N1> multiTagStdDevsMatrix) {
       latencyAlert = new Alert("'" + name + "' Camera is experiencing high latency.", AlertType.kWarning);
 
       camera = new PhotonCamera(name);
@@ -429,21 +410,22 @@ public class Vision
       robotToCamTransform = new Transform3d(robotToCamTranslation, robotToCamRotation);
 
       poseEstimator = new PhotonPoseEstimator(Vision.fieldLayout,
-                                              PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
-                                              robotToCamTransform);
+          PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+          robotToCamTransform);
       poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
 
       this.singleTagStdDevs = singleTagStdDevs;
       this.multiTagStdDevs = multiTagStdDevsMatrix;
 
-      if (Robot.isSimulation())
-      {
+      if (Robot.isSimulation()) {
         SimCameraProperties cameraProp = new SimCameraProperties();
         // A 640 x 480 camera with a 100 degree diagonal FOV.
         cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(100));
-        // Approximate detection noise with average and standard deviation error in pixels.
+        // Approximate detection noise with average and standard deviation error in
+        // pixels.
         cameraProp.setCalibError(0.25, 0.08);
-        // Set the camera image capture framerate (Note: this is limited by robot loop rate).
+        // Set the camera image capture framerate (Note: this is limited by robot loop
+        // rate).
         cameraProp.setFPS(30);
         // The average and standard deviation in milliseconds of image data latency.
         cameraProp.setAvgLatencyMs(35);
@@ -459,35 +441,31 @@ public class Vision
      *
      * @param systemSim {@link VisionSystemSim} to use.
      */
-    public void addToVisionSim(VisionSystemSim systemSim)
-    {
-      if (Robot.isSimulation())
-      {
+    public void addToVisionSim(VisionSystemSim systemSim) {
+      if (Robot.isSimulation()) {
         systemSim.addCamera(cameraSim, robotToCamTransform);
       }
     }
 
     /**
-     * Get the result with the least ambiguity from the best tracked target within the Cache. This may not be the most
+     * Get the result with the least ambiguity from the best tracked target within
+     * the Cache. This may not be the most
      * recent result!
      *
-     * @return The result in the cache with the least ambiguous best tracked target. This is not the most recent result!
+     * @return The result in the cache with the least ambiguous best tracked target.
+     *         This is not the most recent result!
      */
-    public Optional<PhotonPipelineResult> getBestResult()
-    {
-      if (resultsList.isEmpty())
-      {
+    public Optional<PhotonPipelineResult> getBestResult() {
+      if (resultsList.isEmpty()) {
         return Optional.empty();
       }
 
-      PhotonPipelineResult bestResult       = resultsList.get(0);
-      double               amiguity         = bestResult.getBestTarget().getPoseAmbiguity();
-      double               currentAmbiguity = 0;
-      for (PhotonPipelineResult result : resultsList)
-      {
+      PhotonPipelineResult bestResult = resultsList.get(0);
+      double amiguity = bestResult.getBestTarget().getPoseAmbiguity();
+      double currentAmbiguity = 0;
+      for (PhotonPipelineResult result : resultsList) {
         currentAmbiguity = result.getBestTarget().getPoseAmbiguity();
-        if (currentAmbiguity < amiguity && currentAmbiguity > 0)
-        {
+        if (currentAmbiguity < amiguity && currentAmbiguity > 0) {
           bestResult = result;
           amiguity = currentAmbiguity;
         }
@@ -498,63 +476,63 @@ public class Vision
     /**
      * Get the latest result from the current cache.
      *
-     * @return Empty optional if nothing is found. Latest result if something is there.
+     * @return Empty optional if nothing is found. Latest result if something is
+     *         there.
      */
-    public Optional<PhotonPipelineResult> getLatestResult()
-    {
+    public Optional<PhotonPipelineResult> getLatestResult() {
       return resultsList.isEmpty() ? Optional.empty() : Optional.of(resultsList.get(0));
     }
 
     /**
-     * Get the estimated robot pose. Updates the current robot pose estimation, standard deviations, and flushes the
+     * Get the estimated robot pose. Updates the current robot pose estimation,
+     * standard deviations, and flushes the
      * cache of results.
      *
      * @return Estimated pose.
      */
-    public Optional<EstimatedRobotPose> getEstimatedGlobalPose()
-    {
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
       updateUnreadResults();
       return estimatedRobotPose;
     }
 
     /**
-     * Update the latest results, cached with a maximum refresh rate of 1req/15ms. Sorts the list by timestamp.
+     * Update the latest results, cached with a maximum refresh rate of 1req/15ms.
+     * Sorts the list by timestamp.
      */
-    private void updateUnreadResults()
-    {
+    private void updateUnreadResults() {
       double mostRecentTimestamp = resultsList.isEmpty() ? 0.0 : resultsList.get(0).getTimestampSeconds();
-      
-      for (PhotonPipelineResult result : resultsList)
-      {
+
+      for (PhotonPipelineResult result : resultsList) {
         mostRecentTimestamp = Math.max(mostRecentTimestamp, result.getTimestampSeconds());
       }
 
-        resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults();
-        resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
-          return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;
-        });
-        if (!resultsList.isEmpty())
-        {
-          updateEstimatedGlobalPose();
-        }
+      resultsList = Robot.isReal() ? camera.getAllUnreadResults() : cameraSim.getCamera().getAllUnreadResults();
+      resultsList.sort((PhotonPipelineResult a, PhotonPipelineResult b) -> {
+        return a.getTimestampSeconds() >= b.getTimestampSeconds() ? 1 : -1;
+      });
+      if (!resultsList.isEmpty()) {
+        updateEstimatedGlobalPose();
+      }
 
     }
 
     /**
-     * The latest estimated robot pose on the field from vision data. This may be empty. This should only be called once
+     * The latest estimated robot pose on the field from vision data. This may be
+     * empty. This should only be called once
      * per loop.
      *
-     * <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
+     * <p>
+     * Also includes updates for the standard deviations, which can (optionally) be
+     * retrieved with
      * {@link Cameras#updateEstimationStdDevs}
      *
-     * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets used for
-     * estimation.
+     * @return An {@link EstimatedRobotPose} with an estimated pose, estimate
+     *         timestamp, and targets used for
+     *         estimation.
      */
-    private void updateEstimatedGlobalPose()
-    {
+    private void updateEstimatedGlobalPose() {
       Optional<EstimatedRobotPose> visionEst = Optional.empty();
-      for (var change : resultsList)
-      {
+      for (var change : resultsList) {
         visionEst = poseEstimator.update(change);
         updateEstimationStdDevs(visionEst, change.getTargets());
       }
@@ -562,63 +540,54 @@ public class Vision
     }
 
     /**
-     * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard deviations based
+     * Calculates new standard deviations This algorithm is a heuristic that creates
+     * dynamic standard deviations based
      * on number of tags, estimation strategy, and distance from the tags.
      *
      * @param estimatedPose The estimated pose to guess standard deviations for.
      * @param targets       All targets in this camera frame
      */
     private void updateEstimationStdDevs(
-        Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets)
-    {
-      if (estimatedPose.isEmpty())
-      {
+        Optional<EstimatedRobotPose> estimatedPose, List<PhotonTrackedTarget> targets) {
+      if (estimatedPose.isEmpty()) {
         // No pose input. Default to single-tag std devs
         curStdDevs = singleTagStdDevs;
 
-      } else
-      {
+      } else {
         // Pose present. Start running Heuristic
-        var    estStdDevs = singleTagStdDevs;
-        int    numTags    = 0;
-        double avgDist    = 0;
+        var estStdDevs = singleTagStdDevs;
+        int numTags = 0;
+        double avgDist = 0;
 
-        // Precalculation - see how many tags we found, and calculate an average-distance metric
-        for (var tgt : targets)
-        {
+        // Precalculation - see how many tags we found, and calculate an
+        // average-distance metric
+        for (var tgt : targets) {
           var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId());
-          if (tagPose.isEmpty())
-          {
+          if (tagPose.isEmpty()) {
             continue;
           }
           numTags++;
-          avgDist +=
-              tagPose
-                  .get()
-                  .toPose2d()
-                  .getTranslation()
-                  .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
+          avgDist += tagPose
+              .get()
+              .toPose2d()
+              .getTranslation()
+              .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation());
         }
 
-        if (numTags == 0)
-        {
+        if (numTags == 0) {
           // No tags visible. Default to single-tag std devs
           curStdDevs = singleTagStdDevs;
-        } else
-        {
+        } else {
           // One or more tags visible, run the full heuristic.
           avgDist /= numTags;
           // Decrease std devs if multiple targets are visible
-          if (numTags > 1)
-          {
+          if (numTags > 1) {
             estStdDevs = multiTagStdDevs;
           }
           // Increase std devs based on (average) distance
-          if (numTags == 1 && avgDist > 4)
-          {
+          if (numTags == 1 && avgDist > 4) {
             estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
-          } else
-          {
+          } else {
             estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30));
           }
           curStdDevs = estStdDevs;

vendordeps/REVLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2026.0.2",
+    "version": "2026.0.3",
     "frcYear": "2026",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2026.0.2"
+            "version": "2026.0.3"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -34,7 +34,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibBackendDriver",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -49,7 +49,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibWpiBackendDriver",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -66,7 +66,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -83,7 +83,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -100,7 +100,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibBackendDriver",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "libName": "BackendDriver",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,
@@ -116,7 +116,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibWpiBackendDriver",
-            "version": "2026.0.2",
+            "version": "2026.0.3",
             "libName": "REVLibWpi",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,