outpost auto improved

.vscode/settings.json

@@ -57,5 +57,6 @@
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
   ],
-  "java.dependency.enableDependencyCheckup": false
+  "java.dependency.enableDependencyCheckup": false,
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
 }

src/main/deploy/pathplanner/autos/Center Back up.auto

@@ -0,0 +1,37 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intake down"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "spinup"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Center to Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "shoot close"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Left Shoot Outpost.auto

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intake down"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Outpost"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 2.0
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "trough to shot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "spinup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "spinup"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "aim"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "auto shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/MOVE B____ left to center.auto

@@ -0,0 +1,82 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "left start to center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake down"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "over bump to pile move"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "spinup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "back from center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "spinup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "aim"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "auto shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Outpost.auto

@@ -0,0 +1,31 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to outpost"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Outpost to Ranged Shot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Auto Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/left to center.auto

@@ -0,0 +1,82 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "left start to center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake down"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "over bump to pile"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "spinup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "back from center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "spinup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "aim"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "auto shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Center to Shoot.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.622028469750891,
+        "y": 4.008102016607354
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.266975088967973,
+        "y": 4.0403795966785285
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.1271055753262162,
+        "y": 4.008102016607354
+      },
+      "prevControl": {
+        "x": 4.0416370106761565,
+        "y": 4.0403795966785285
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Example Path.path

@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/Left to Outpost.path

@@ -0,0 +1,80 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.925604060913706,
+        "y": 5.503695431472081
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.107905365777289,
+        "y": 5.923303972397348
+      },
+      "isLocked": false,
+      "linkedName": "left close"
+    },
+    {
+      "anchor": {
+        "x": 0.32339086294416286,
+        "y": 5.9825177664974625
+      },
+      "prevControl": {
+        "x": 1.9265106731458264,
+        "y": 6.165424053567452
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "trough"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 0.3963599595551063,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 3.0,
+        "maxAcceleration": 4.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake Start",
+      "waypointRelativePos": 0.09706774519716882,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 178.80651057601818
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -31.15930450834445
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/My Bumpers Arent All The Way Over.path

@@ -0,0 +1,151 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 4.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.572076308784383,
+        "y": 3.3951907719609573
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.1112866015971603,
+        "y": 2.6628305235137533
+      },
+      "prevControl": {
+        "x": 2.8612866015971603,
+        "y": 2.6628305235137533
+      },
+      "nextControl": {
+        "x": 3.3612866015971603,
+        "y": 2.6628305235137533
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.839529724933453,
+        "y": 2.357009760425909
+      },
+      "prevControl": {
+        "x": 5.055196983507387,
+        "y": 2.7986728575396165
+      },
+      "nextControl": {
+        "x": 6.668464951197871,
+        "y": 1.890230700976042
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.612129547471162,
+        "y": 0.8279059449866895
+      },
+      "prevControl": {
+        "x": 6.3623142538133335,
+        "y": 0.8182976644613882
+      },
+      "nextControl": {
+        "x": 7.030621118012423,
+        "y": 0.844001774622892
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.189520851818989,
+        "y": 0.8279059449866895
+      },
+      "prevControl": {
+        "x": 8.08578683847011,
+        "y": 0.49077040160283736
+      },
+      "nextControl": {
+        "x": 8.318287488908608,
+        "y": 1.246397515527949
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.189520851818989,
+        "y": 7.111241666666667
+      },
+      "prevControl": {
+        "x": 8.36001268105017,
+        "y": 7.294087328812116
+      },
+      "nextControl": {
+        "x": 6.244366666666667,
+        "y": 5.025141666666666
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.518108333333333,
+        "y": 5.592016666666666
+      },
+      "prevControl": {
+        "x": 3.205916666666667,
+        "y": 5.644925000000001
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 45.0
+    },
+    {
+      "waypointRelativePos": 2,
+      "rotationDegrees": 45.0
+    },
+    {
+      "waypointRelativePos": 3,
+      "rotationDegrees": -135.0
+    },
+    {
+      "waypointRelativePos": 5,
+      "rotationDegrees": -135.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -133.87669728592465
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Outpost to Ranged Shot.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7089624329216013,
+        "y": 0.668228980317108
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.1043470483005366,
+        "y": 0.7169051878354196
+      },
+      "isLocked": false,
+      "linkedName": "Outpost"
+    },
+    {
+      "anchor": {
+        "x": 2.3639534883720925,
+        "y": 2.9722361359570666
+      },
+      "prevControl": {
+        "x": 2.234150268336314,
+        "y": 2.5666010733452596
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 24.304549265936608
+  },
+  "reversed": false,
+  "folder": "Right Outpost",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/Start to outpost.path

@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5922741116751276,
+        "y": 6.3692588832487305
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.956913705583757,
+        "y": 6.19430456852792
+      },
+      "isLocked": false,
+      "linkedName": "start left"
+    },
+    {
+      "anchor": {
+        "x": 0.32339086294416286,
+        "y": 5.9825177664974625
+      },
+      "prevControl": {
+        "x": 1.3270761421319812,
+        "y": 5.964101522842641
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "trough"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.6910862504511138,
+      "rotationDegrees": -179.02889973265033
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake Start",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 0.75,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 178.80651057601818
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/back from center.path

@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.68848717948718,
+        "y": 5.639692307692307
+      },
+      "isLocked": false,
+      "linkedName": "after center grab"
+    },
+    {
+      "anchor": {
+        "x": 2.979166666666668,
+        "y": 5.2327051282051285
+      },
+      "prevControl": {
+        "x": 3.0721923076923088,
+        "y": 5.721089743589744
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.11569148936170148,
+      "rotationDegrees": -34.71279786419313
+    }
+  ],
+  "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": -36.158185439808385
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/left start to center diagonal.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5922741116751276,
+        "y": 6.3692588832487305
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.9329746192893413,
+        "y": 6.3692588832487305
+      },
+      "isLocked": false,
+      "linkedName": "start left"
+    },
+    {
+      "anchor": {
+        "x": 4.577543147208122,
+        "y": 5.715482233502538
+      },
+      "prevControl": {
+        "x": 4.264467005076142,
+        "y": 5.715482233502538
+      },
+      "nextControl": {
+        "x": 5.120900364352579,
+        "y": 5.715482233502538
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
+      },
+      "prevControl": {
+        "x": 5.931137055837564,
+        "y": 5.65102538071066
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "over bump"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.8,
+      "rotationDegrees": -135.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 2.0,
+    "rotation": -129.95754893082906
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/left start to center.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5922741116751276,
+        "y": 6.3692588832487305
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.085827411167513,
+        "y": 6.277177664974619
+      },
+      "isLocked": false,
+      "linkedName": "start left"
+    },
+    {
+      "anchor": {
+        "x": 4.577543147208122,
+        "y": 5.715482233502538
+      },
+      "prevControl": {
+        "x": 3.2884060913705584,
+        "y": 5.697065989847716
+      },
+      "nextControl": {
+        "x": 5.120844928223563,
+        "y": 5.723243687517044
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
+      },
+      "prevControl": {
+        "x": 5.931137055837564,
+        "y": 5.65102538071066
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "over bump"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.8,
+      "rotationDegrees": -135.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 2.0,
+    "rotation": -129.95754893082906
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/over bump to pile move.path

@@ -0,0 +1,116 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.511248730964468,
+        "y": 7.059868020304569
+      },
+      "isLocked": false,
+      "linkedName": "over bump"
+    },
+    {
+      "anchor": {
+        "x": 7.2386903553299495,
+        "y": 7.326903553299493
+      },
+      "prevControl": {
+        "x": 6.2386903553299495,
+        "y": 7.326903553299493
+      },
+      "nextControl": {
+        "x": 8.238690355329947,
+        "y": 7.326903553299493
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.417329949238578,
+        "y": 4.831502538071066
+      },
+      "prevControl": {
+        "x": 8.475340101522843,
+        "y": 6.042370558375635
+      },
+      "nextControl": {
+        "x": 8.359319796954313,
+        "y": 3.620634517766498
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
+      },
+      "prevControl": {
+        "x": 7.758358974358975,
+        "y": 5.616435897435898
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "after center grab"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0045454545454569,
+      "rotationDegrees": -115.0
+    },
+    {
+      "waypointRelativePos": 2.348863636363685,
+      "rotationDegrees": -115.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.1243680485338854,
+      "maxWaypointRelativePos": 2.077102803738317,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake Start",
+      "waypointRelativePos": 0.7886754297269942,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -129.95754893082906
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/over bump to pile.path

@@ -0,0 +1,116 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.511248730964468,
+        "y": 7.059868020304569
+      },
+      "isLocked": false,
+      "linkedName": "over bump"
+    },
+    {
+      "anchor": {
+        "x": 7.220274111675127,
+        "y": 7.299279187817259
+      },
+      "prevControl": {
+        "x": 6.220274111675127,
+        "y": 7.299279187817259
+      },
+      "nextControl": {
+        "x": 8.220274111675131,
+        "y": 7.299279187817259
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.883258883248731,
+        "y": 4.831502538071066
+      },
+      "prevControl": {
+        "x": 7.952417879560978,
+        "y": 6.041784973535379
+      },
+      "nextControl": {
+        "x": 7.846426395939085,
+        "y": 4.186934010152284
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
+      },
+      "prevControl": {
+        "x": 7.598549873246986,
+        "y": 5.589703440094898
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "after center grab"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0045454545454569,
+      "rotationDegrees": -115.0
+    },
+    {
+      "waypointRelativePos": 2.348863636363685,
+      "rotationDegrees": -115.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.3403967538322836,
+      "maxWaypointRelativePos": 2.0,
+      "constraints": {
+        "maxVelocity": 1.0,
+        "maxAcceleration": 1.5,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0,
+        "nominalVoltage": 12.0,
+        "unlimited": false
+      }
+    }
+  ],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Intake Start",
+      "waypointRelativePos": 0.7886754297269942,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 2.0,
+    "rotation": -129.95754893082906
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/start to score left.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.5922741116751276,
+        "y": 6.3692588832487305
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.817612189966942,
+        "y": 6.260219737341258
+      },
+      "isLocked": false,
+      "linkedName": "start left"
+    },
+    {
+      "anchor": {
+        "x": 1.925604060913706,
+        "y": 5.503695431472081
+      },
+      "prevControl": {
+        "x": 2.119269541340752,
+        "y": 6.020136712610872
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "left close"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -31.15930450834445
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/trough to shot.path

@@ -0,0 +1,59 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.32339086294416286,
+        "y": 5.9825177664974625
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.6364670050761427,
+        "y": 6.03776649746193
+      },
+      "isLocked": false,
+      "linkedName": "trough"
+    },
+    {
+      "anchor": {
+        "x": 1.925604060913706,
+        "y": 5.503695431472081
+      },
+      "prevControl": {
+        "x": 1.409949238578681,
+        "y": 5.844395939086294
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "left close"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7098520389751093,
+      "rotationDegrees": 176.84552599629856
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -31.15930450834445
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 178.80651057601818
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/settings.json

@@ -1,32 +1,36 @@
 {
-  "robotWidth": 0.9,
+  "robotWidth": 0.921,
-  "robotLength": 0.9,
+  "robotLength": 0.787,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
+    "Right Outpost"
+  ],
   "autoFolders": [],
-  "defaultMaxVel": 3.0,
+  "defaultMaxVel": 2.0,
-  "defaultMaxAccel": 3.0,
+  "defaultMaxAccel": 1.5,
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 74.088,
+  "robotMass": 64.864,
-  "robotMOI": 6.883,
+  "robotMOI": 37.809,
   "robotTrackwidth": 0.546,
-  "driveWheelRadius": 0.048,
+  "driveWheelRadius": 0.051,
-  "driveGearing": 5.143,
+  "driveGearing": 6.122,
-  "maxDriveSpeed": 5.45,
+  "maxDriveSpeed": 4.66,
   "driveMotorType": "krakenX60",
-  "driveCurrentLimit": 60.0,
+  "driveCurrentLimit": 65.0,
   "wheelCOF": 1.2,
-  "flModuleX": 0.273,
+  "flModuleX": 0.238,
-  "flModuleY": 0.273,
+  "flModuleY": 0.3015,
-  "frModuleX": 0.273,
+  "frModuleX": 0.238,
-  "frModuleY": -0.273,
+  "frModuleY": -0.3015,
-  "blModuleX": -0.273,
+  "blModuleX": -0.238,
-  "blModuleY": 0.273,
+  "blModuleY": 0.3015,
-  "brModuleX": -0.273,
+  "brModuleX": -0.238,
-  "brModuleY": -0.273,
+  "brModuleY": -0.3015,
   "bumperOffsetX": 0.0,
   "bumperOffsetY": 0.0,
-  "robotFeatures": []
+  "robotFeatures": [
+    "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.55,\"y\":0.0},\"size\":{\"width\":0.921,\"length\":0.305},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}"
+  ]
 }

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

@@ -5,127 +5,537 @@
 package frc.robot;
 
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+
+import java.util.Optional;
+import java.util.OptionalDouble;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.events.EventTrigger;
+import com.pathplanner.lib.path.EventMarker;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.Timer;
 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.Commands;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.AutoConstants;
+import frc.robot.constants.CompetitionConstants;
+import frc.robot.constants.HoodConstants;
 import frc.robot.constants.OIConstants;
+import frc.robot.constants.ShooterConstants;
+import frc.robot.constants.IntakePivotConstants.IntakePivotPosition;
+import frc.robot.constants.ShooterConstants.ShooterSpeeds;
+import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Hood;
+import frc.robot.subsystems.IntakePivot;
+import frc.robot.subsystems.IntakeRoller;
 import frc.robot.subsystems.PhotonVision;
+import frc.robot.subsystems.Shooter;
+import frc.robot.subsystems.Spindexer;
 import frc.robot.utilities.Elastic;
 import frc.robot.utilities.Utilities;
 
 public class RobotContainer {
-  private PhotonVision vision;
+    private PhotonVision vision;
-  private Drivetrain drivetrain;
+    private Drivetrain drivetrain;
+    private Hood hood;
+    private Shooter shooter;
+    private IntakePivot intakePivot;
+    private IntakeRoller intakeRoller;
+    private Spindexer spindexer;
+    //private Climber climber;
 
-  private CommandXboxController driver;
+    private CommandXboxController driver;
+    private CommandXboxController secondary;
 
-  private SendableChooser<Command> autoChooser;
+    private SendableChooser<Command> autoChooser;
 
-  private Timer shiftTimer;
+    private Timer shiftTimer;
 
-  public RobotContainer() {
+    private boolean resetOdometryToVisualPose;
-    vision = new PhotonVision();
-    drivetrain = new Drivetrain();
 
-    vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
+    public RobotContainer() {
+        vision = new PhotonVision();
+        drivetrain = new Drivetrain(null);
+        hood = new Hood();
+        shooter = new Shooter();
+        intakePivot = new IntakePivot();
+        intakeRoller = new IntakeRoller();
+        spindexer = new Spindexer();
+        //climber = new Climber();
+        configureNamedCommands();
 
-    driver = new CommandXboxController(OIConstants.kDriverControllerPort);
+        resetOdometryToVisualPose = false;
          
-    shiftTimer = new Timer();
+        vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
-    shiftTimer.reset();
+        vision.addPoseEstimateConsumer((vp) -> {
+            Logger.recordOutput(
+                "Vision/" + vp.cameraName() + "/Pose", 
+                vp.visualPose()
+            );
+        });
 
-    configureBindings();
+        vision.addPoseEstimateConsumer((vp) -> {
-  }
+            if(resetOdometryToVisualPose) {
+                drivetrain.resetOdometry(vp.visualPose());
+                resetOdometryToVisualPose = false;
+            }
+        });
         
-  private void configureBindings() {
+        driver = new CommandXboxController(OIConstants.kDriverControllerPort);
-    drivetrain.setDefaultCommand(
+        secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);
-      drivetrain.drive(
-        driver::getLeftY,
-        driver::getLeftX, 
-        driver::getRightX, 
-        () -> false
-      )
-    );
 
-    driver.start().and(driver.x()).whileTrue(drivetrain.runFrontLeft(1, 0));
+        shiftTimer = new Timer();
-    driver.start().and(driver.y()).whileTrue(drivetrain.runFrontRight(1, 0));
+        shiftTimer.reset();
-    driver.start().and(driver.a()).whileTrue(drivetrain.runRearLeft(1, 0));
-    driver.start().and(driver.b()).whileTrue(drivetrain.runRearRight(1, 0));
-    driver.start().negate().and(driver.x()).whileTrue(drivetrain.runFrontLeft(0, 45));
-    driver.start().negate().and(driver.y()).whileTrue(drivetrain.runFrontRight(0, 45));
-    driver.start().negate().and(driver.a()).whileTrue(drivetrain.runRearLeft(0, 45));
-    driver.start().negate().and(driver.b()).whileTrue(drivetrain.runRearRight(0, 45));
-    driver.rightBumper().whileTrue(drivetrain.setX());
 
-    //drivetrain.setDefaultCommand(drivetrain.disableOutputs());
+        configureBindings();
+        configureShiftDisplay();
+        //testConfigureBindings();
         
-    configureShiftDisplay();
-  }
 
-  public Command getAutonomousCommand() {
+        if(AutoConstants.kAutoConfigOk) {
-    if(AutoConstants.kAutoConfigOk) {
+            autoChooser = AutoBuilder.buildAutoChooser();
-      return autoChooser.getSelected();
+            SmartDashboard.putData("Auto Chooser", autoChooser);
-    } else {
+
-      return new PrintCommand("Robot Config loading failed, autonomous disabled");
+            autoChooser.addOption(
+                "MOVE B____ right to center", 
+                new PathPlannerAuto("MOVE B____ left to center", true)
+            );
+
+            autoChooser.addOption(
+                "right to center", 
+                new PathPlannerAuto("left to center", true)
+            );
+        }
     }
-  }
 
-  private void configureShiftDisplay() {
+    /**
-    SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+     * Before you @ mention me for terrible match controls, these are <i>TEST BINDINGS</i>
+     * 
+     * The are configured in such a way to make testing of multiple systems possible without
+     * having to constantly change bindings.
+     * 
+     * Most of the configurations here won't make sense for actual competition play. See
+     * {@link #configureBindings()} for actual competition play
+     * 
+     * The intent of each binding is outlined by comments above each binding. 
+     */
+    private void testConfigureBindings() {
+        // This should just work, if it doesn't it's likely modules aren't assigned the right IDs
+        // after the electronics rebuild. For testing normal operation nothing about the Drivetrain
+        // class should need to change
+        //drivetrain.setDefaultCommand(drivetrain.drive(() -> 0, () -> 0, () -> 0, () -> true));
+        drivetrain.setDefaultCommand(
+            drivetrain.drive(
+                driver::getLeftY,
+                driver::getLeftX, 
+                driver::getRightX, 
+                () -> true
+            )
+        );
 
-    RobotModeTriggers.autonomous().onTrue(new InstantCommand(() -> {
+        driver.y().whileTrue(drivetrain.zeroHeading());
-      shiftTimer.stop();
+/* 
-      SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+        // This needs to be tested after a prolonged amount of driving around <i>aggressively</i>.
-    }));
+        // Do things like going over the bump repeatedly, spin around a bunch, etc. 
+        // If this works well over time, then this is likely all we need
+        driver.a().whileTrue(
+            drivetrain.lockRotationToHub(
+                driver::getLeftY, 
+                driver::getLeftX, 
+                false
+            ) 
+        );
 
-    RobotModeTriggers.teleop().onTrue(new InstantCommand(() -> {
+        // This can be tested as an alternative to the above, it's less dynamic, but is a simpler
-      Elastic.selectTab(OIConstants.kTeleopTab);
+        // alternative. 
-      shiftTimer.reset();
+        driver.b().whileTrue(
-      shiftTimer.start();
+            drivetrain.lockToYaw(
-    }));
+                () -> {
+                    OptionalDouble maybeYaw = vision.getBestYawForTag(Utilities.getHubCenterAprilTagID());
 
-    new Trigger(() -> shiftTimer.get() <= 10).onTrue(new InstantCommand(() -> {
+                    return maybeYaw.isEmpty() ? 0 : maybeYaw.getAsDouble();
-      SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+                }, 
-    }));
+                driver::getLeftY, 
+                driver::getLeftX
+            )
+        );*/
 
-    new Trigger(() -> shiftTimer.get() > 10 && shiftTimer.get() <= 35).onTrue(new InstantCommand(() -> {
+        // Stop everything by default other than the drivetrain
-      SmartDashboard.putStringArray(
+        shooter.setDefaultCommand(shooter.stop());
-        OIConstants.kCurrentActiveHub, 
+        intakePivot.setDefaultCommand(intakePivot.stop());
-        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
+        intakeRoller.setDefaultCommand(intakeRoller.stop());
-      );
+        hood.setDefaultCommand(hood.stop());
-    }));
+        spindexer.setDefaultCommand(spindexer.stop());
+        //climber.setDefaultCommand(climber.stop());
 
-    new Trigger(() -> shiftTimer.get() > 35 && shiftTimer.get() <= 60).onTrue(new InstantCommand(() -> {
+        // While holding POV up of the driver controller, the climber
-      SmartDashboard.putStringArray(
+        // should move such that its motor moves the climber down with the left
-        OIConstants.kCurrentActiveHub, 
+        // driver controller trigger axis, and up with the right driver controller
-        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
+        // trigger axis.
-      );
+        // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted
-    }));
+        // from the constants file for the subsystem having the problem. 
+        //driver.povUp().whileTrue(climber.manualSpeed(() -> {
+        //    return driver.getLeftTriggerAxis() * -1 + driver.getRightTriggerAxis();
+        //}));
 
-    new Trigger(() -> shiftTimer.get() > 60 && shiftTimer.get() <= 85).onTrue(new InstantCommand(() -> {
+        // While holding the right bumper of the driver controller, the intake rollers
-      SmartDashboard.putStringArray(
+        // and the spindexer and feeder should move such that all motors are moving in such a way
-        OIConstants.kCurrentActiveHub, 
+        // that it would draw balls from the floor, through the spindexer, and into the
-        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
+        // feeder. 
-      );
+        // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the 
-    }));
+        // constants file for the subsystem having the problem
+        driver.rightBumper().whileTrue(
+            spindexer.spinToShooter()  
+        );
 
-    new Trigger(() -> shiftTimer.get() > 85 && shiftTimer.get() <= 110).onTrue(new InstantCommand(() -> {
+        // While holding the left bumper of the driver controller, the intake rollers
-      SmartDashboard.putStringArray(
+        // and the spindexer and feeder should move such that all motors are moving in such a way
-        OIConstants.kCurrentActiveHub, 
+        // that it would try to eject balls through the intake.
-        Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
+        // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the 
-      );
+        // constants file for the subsystem having the problem
-    }));
+        driver.leftBumper().whileTrue(
+            intakeRoller.runIn()
+            //intakeRoller.runOut().alongWith(spindexer.spinToIntake())
+        );
 
-    new Trigger(() -> shiftTimer.get() > 110).onTrue(new InstantCommand(() -> {
+        // While holding D-Pad up on the secondary controller, the shooter should spin
-      SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+        // while holding down the secondary controllers right trigger some amount.
-    }));
+        // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the
-  }
+        // constants file for the subsystem having the problem
+        secondary.povUp().whileTrue(
+            shooter.manualSpeed(secondary::getRightTriggerAxis)
+        );
+
+        // While holding D-Pad down on the seconadry controller, the intakePivot should move
+        // such that left trigger on the secondary controller moves the pivot down, and 
+        // right trigger on the secondary controller moves the pivot up.
+        // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the
+        // constants file for the subsystem having the problem
+        secondary.povDown().whileTrue(
+            intakePivot.manualSpeed(() -> {
+                return secondary.getLeftTriggerAxis() * -1 + secondary.getRightTriggerAxis();
+            })
+        );
+
+        // While holding D-Pad left on the secondary controller, the hood should move
+        // such that the left trigger on the secondary controller moves the hood down, and
+        // right trigger on the secondary controller moves the hood up.
+        // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the 
+        // constants file for the subsystem having the problem
+        secondary.povLeft().whileTrue(
+            hood.manualSpeed(() -> {
+                return secondary.getLeftTriggerAxis() * -1 + secondary.getRightTriggerAxis();
+            })
+        );
+
+        // STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP
+        // Don't proceed unless you've verified by hand or with the above bindings, that sensing
+        // systems are providing correct values in the correct direction of rotation. 
+    
+
+        // Useful for testing PID and FF responses of the shooter
+        // You need to have graphs up of the logged data to make sure the response is correct
+        //secondary.a().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
+        secondary.b().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
+
+        // Useful for testing PID and FF responses of the intake pivot
+        // You need to have graphs up of the logged data to make sure the response is correct
+        secondary.x().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kDown));
+        secondary.y().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kUp));
+
+        secondary.povUp().whileTrue(hood.trackToAngle(() -> Math.toRadians(40.0)));
+        secondary.povDown().whileTrue(hood.trackToAngle(() -> Math.toRadians(0.0)));
+        
+
+        // TODO Some means of testing hood PIDF
+        // TODO Some means of testing climber PIDF
+    }
+  
+    private void configureBindings() {
+        drivetrain.setDefaultCommand(
+            drivetrain.drive(
+                driver::getLeftY,
+                driver::getLeftX, 
+                driver::getRightX, 
+                () -> true
+            )
+        );
+        shooter.setDefaultCommand(shooter.stop());
+        intakeRoller.setDefaultCommand(intakeRoller.stop());
+        spindexer.setDefaultCommand(spindexer.stop());
+
+        intakePivot.setDefaultCommand(intakePivot.manualSpeed(() -> secondary.getLeftY()));
+
+       // secondary.leftStick().whileTrue(intakePivot.manualSpeed(() -> secondary.getLeftY()));
+
+        driver.a().onTrue(new InstantCommand(() -> resetOdometryToVisualPose = true));
+        driver.y().whileTrue(drivetrain.zeroHeading());
+        driver.x().whileTrue(drivetrain.setX());
+
+        driver.leftTrigger().whileTrue(
+            drivetrain.lockRotationToHub(
+                driver::getLeftY, 
+                driver::getLeftX, 
+                false
+            ) 
+        );
+
+        driver.leftBumper().whileTrue(intakeRoller.runOut());
+        driver.rightBumper().whileTrue(intakeRoller.runIn());
+
+        driver.rightTrigger().whileTrue(
+            spindexer.spinToShooter(shooter::getAverageActualSpeeds, 2000).alongWith(
+                intakeRoller.runIn()/* ,
+                intakePivot.jimmy(.5)*/
+            )
+        );
+        driver.rightTrigger().whileTrue(
+            intakePivot.jimmy(.5)
+        );
+
+        secondary.leftBumper().onTrue(new InstantCommand(() -> {}, intakePivot));
+
+        driver.rightTrigger().onFalse(
+            intakePivot.manualSpeed(() -> 0.75).withTimeout(0.5)
+        );
+
+        driver.b().whileTrue(spindexer.spinToIntake());
+       /*  driver.b().whileTrue(
+            drivetrain.lockToYaw(
+                () -> {
+                    OptionalDouble maybeYaw = vision.getBestYawForTag(Utilities.getHubCenterAprilTagID());
+
+                    return maybeYaw.isEmpty() ? 0 : maybeYaw.getAsDouble();
+                }, 
+                driver::getLeftY, 
+                driver::getLeftX
+            )
+        );*/
+
+        secondary.a().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
+        secondary.x().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
+
+        //hood.setDefaultCommand(hood.trackToAngle(() -> Units.degreesToRadians(MathUtil.clamp(hoodAngle, 0, 40))));
+        //secondary.y().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(40)));
+        //40 good for feeding
+        //secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30)));
+        //30 degrees good for shooter far near outpost
+        secondary.rightBumper().whileTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
+        //10 degrees good for shooting ~33in away from hub
+
+        hood.setDefaultCommand(hood.trackToAnglePoseBased(drivetrain, shooter));
+        
+        /*hood.setDefaultCommand(hood.trackToAngle(() -> {
+            Pose2d drivetrainPose = drivetrain.getPose();
+            Pose2d hubPose = Utilities.getHubPose();
+
+            double distance = drivetrainPose.getTranslation()
+                .getDistance(hubPose.getTranslation());
+
+            Logger.recordOutput("Hood/DistanceToHub", distance);
+
+            Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
+                
+            if(currentSpeeds.isPresent()) {
+                InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
+
+                if(map != null) {
+                    return MathUtil.clamp(map.get(distance), 0, 40);
+                } else {
+                    return 0;
+                }
+            } else {
+                return 0;
+            }
+        }));*/
+
+        new Trigger(() -> MathUtil.isNear(
+            shooter.getTargetSpeeds().isEmpty() ? 0 : shooter.getTargetSpeeds().get().getSpeedRPM(), 
+            shooter.getAverageActualSpeeds(), 
+            150)).onTrue(
+            new FunctionalCommand(
+                () -> {}, 
+                () -> {
+                    driver.setRumble(RumbleType.kBothRumble, .75);
+                    secondary.setRumble(RumbleType.kBothRumble, .75);
+                }, 
+                (b) -> {
+                    driver.setRumble(RumbleType.kBothRumble, 0);
+                    secondary.setRumble(RumbleType.kBothRumble, 0);
+                }, 
+                () -> false
+            ).withTimeout(1)
+        );
+    }
+
+    private void configureNamedCommands() {
+        NamedCommands.registerCommand(
+            "Drivetrain Set X", 
+            drivetrain.setX()
+        );
+
+        NamedCommands.registerCommand(
+            "Drivetrain Face Hub", 
+            drivetrain.rotateToPose(
+                Utilities.getHubPose(),
+                false // TODO Should this be true by default?
+            )
+        );
+
+        NamedCommands.registerCommand(
+            "intake down", 
+            intakePivot.manualSpeed(()->0.75)
+                .withTimeout(1)
+        );
+
+        NamedCommands.registerCommand("spinup", 
+            shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)
+                .withTimeout(2));
+
+        NamedCommands.registerCommand("shoot close", 
+            spindexer.spinToShooter()
+                .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed))
+                .alongWith(hood.trackToAngle(() -> Units.degreesToRadians(10)))
+                .withTimeout(3).andThen(spindexer.instantaneousStop()));
+        
+       // NamedCommands.registerCommand("Intake Start", intakeRoller.runIn());
+
+        new EventTrigger("Intake Start")
+            .onTrue(
+                intakeRoller.runIn());
+
+        new EventTrigger("windup trigger")
+            .onTrue(
+                shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
+
+        NamedCommands.registerCommand("stop spindexer", spindexer.instantaneousStop());
+
+        NamedCommands.registerCommand("jimmy",
+            intakePivot.jimmy(0.2)
+        );
+
+        NamedCommands.registerCommand("shoot N jimmy", 
+            Commands.parallel(
+                intakePivot.jimmy(0.5),
+                spindexer.spinToShooter()
+                    .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed),
+                               hood.trackToAngle(() -> Units.degreesToRadians(10)))
+                
+            ).withTimeout(3).andThen(spindexer.instantaneousStop()));
+
+        NamedCommands.registerCommand("aim", 
+            hood.trackToAnglePoseBased(drivetrain, shooter)
+                .alongWith(
+                    shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), 
+                    intakePivot.jimmy(0.5),
+                    drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false))
+                .withTimeout(0.5));
+
+        NamedCommands.registerCommand("auto shoot", 
+            hood.trackToAnglePoseBased(drivetrain, shooter)
+                .alongWith(
+                    shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), 
+                    spindexer.spinToShooter(), 
+                    intakePivot.jimmy(0.5),
+                    drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false)));
+    }
+
+    public Command getAutonomousCommand() {
+        return autoChooser.getSelected();
+    }
+
+    /**
+     * The "shift display" relies on Elastic's ability to show 1 or more colors
+     * in a box on the dashboard. 
+     * 
+     * Using the RobotModeTriggers and a Timer based on the FPGA, a reasonably
+     * accurate "shift display" can be created to indicate whose hub is active
+     * and when. 
+     * 
+     * During autonomous, and the first 10 seconds of teleop, the shift display
+     * will display a gradient of both red and blue, indicating the fact that 
+     * both hubs are active.
+     * 
+     * For the rest of teleop, with the exception of the endgame, the display
+     * will present either the color red, or the color blue, based on the returned
+     * value of Utilities.whoHasFirstShift(). Because shifts change on a known cycle,
+     * we can use the known state of who has first shift, to determine the remaining three shifts
+     * that come after.
+     * 
+     * For the endgame portion of teleop, the shift display returns to the gradient
+     * of both red and blue. 
+     * 
+     * Because this relies on the RobotModeTriggers and an FPGA timer, it should be 
+     * <i>reasonably</i> accurate, it's unlikely to be perfect relative to field time
+     * but it will be very very very (likely unnoticably) close. 
+     */
+    private void configureShiftDisplay() {
+        SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+        
+        RobotModeTriggers.autonomous().onTrue(new InstantCommand(() -> {
+            shiftTimer.stop();
+            SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+        }));
+
+        RobotModeTriggers.teleop().onTrue(new InstantCommand(() -> {
+            Elastic.selectTab(OIConstants.kTeleopTab);
+            shiftTimer.reset();
+            shiftTimer.start();
+        }));
+
+        new Trigger(() -> shiftTimer.get() <= 10).onTrue(new InstantCommand(() -> {
+            SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+        }));
+
+        new Trigger(() -> shiftTimer.get() > 10 && shiftTimer.get() <= 35).onTrue(new InstantCommand(() -> {
+            SmartDashboard.putStringArray(
+                OIConstants.kCurrentActiveHub, 
+                Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
+            );
+        }));
+
+        new Trigger(() -> shiftTimer.get() > 35 && shiftTimer.get() <= 60).onTrue(new InstantCommand(() -> {
+            SmartDashboard.putStringArray(
+                OIConstants.kCurrentActiveHub, 
+                Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
+            );
+        }));
+
+        new Trigger(() -> shiftTimer.get() > 60 && shiftTimer.get() <= 85).onTrue(new InstantCommand(() -> {
+            SmartDashboard.putStringArray(
+                OIConstants.kCurrentActiveHub, 
+                Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
+            );
+        }));
+
+        new Trigger(() -> shiftTimer.get() > 85 && shiftTimer.get() <= 110).onTrue(new InstantCommand(() -> {
+            SmartDashboard.putStringArray(
+                OIConstants.kCurrentActiveHub, 
+                Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
+            );
+        }));
+
+        new Trigger(() -> shiftTimer.get() > 110).onTrue(new InstantCommand(() -> {
+            SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
+        }));
+    }
 }

src/main/java/frc/robot/constants/AutoConstants.java

@@ -10,10 +10,11 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.path.PathConstraints;
 
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
 
 // TODO This is all hold over from 2025, does any of it need to change?
 public class AutoConstants {
-    public static final double kMaxSpeedMetersPerSecond = 5;
+    public static final double kMaxSpeedMetersPerSecond = 4;
     public static final double kMaxAccelerationMetersPerSecondSquared = 4;
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI;

src/main/java/frc/robot/constants/ClimberConstants.java

@@ -0,0 +1,61 @@
+package frc.robot.constants;
+
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+public class ClimberConstants {
+    // TODO Real values
+    public enum ClimberPositions {
+        kStow(0),
+        kClimbOffGround(0),
+        kUp(0);
+
+        private double positionMeters;
+
+        private ClimberPositions(double positionMeters) {
+            this.positionMeters = positionMeters;
+        }
+
+        public double getPositionMeters() {
+            return positionMeters;
+        }
+    }
+
+    public static final int kMotorCANID = 0;
+
+    public static final double kConversionFactor = 0;
+
+    public static final double kP = 0;
+    public static final double kI = 0;
+    public static final double kD = 0;
+    public static final double kS = 0;
+    public static final double kV = 0;
+    public static final double kA = 0;
+
+    public static final boolean kMotorInverted = false;
+
+    public static final int kCurrentLimit = 40;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig();
+
+    static {
+        kMotorConfig
+            .inverted(kMotorInverted)
+            .smartCurrentLimit(kCurrentLimit)
+            .idleMode(kIdleMode);
+        kMotorConfig.encoder
+            .positionConversionFactor(kConversionFactor)
+            .velocityConversionFactor(kConversionFactor / 60);
+        kMotorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1)
+            .feedForward
+                .sva(kS, kV, kA);
+    }
+}

src/main/java/frc/robot/constants/CompetitionConstants.java

@@ -2,6 +2,10 @@ package frc.robot.constants;
 
 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.Transform3d;
+import edu.wpi.first.math.util.Units;
 
 public class CompetitionConstants {
     // THIS SHOULD BE FALSE DURING COMPETITION PLAY
@@ -10,4 +14,29 @@ public class CompetitionConstants {
     public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(
         AprilTagFields.kDefaultField
     );
+
+    public static final double kHubGoalHeightMeters = Units.inchesToMeters(72);
+
+    // TODO Real Values
+    public static final Transform3d kRobotToShooter = new Transform3d();
+
+    public static final Pose2d kBlueHubLocation = new Pose2d(
+        Units.inchesToMeters(182.11), 
+        Units.inchesToMeters(158.84), 
+        Rotation2d.fromDegrees(0)
+    );
+
+    // TODO The origination value produced by the April Tag Field Layout object may
+    // influence what this value should actually be. See AprilTagFieldLayout.getOrigin
+    // For now, the X axis position (forward/backward) is calculated as though the blue
+    // alliance wall right hand side is the originiation point, so, the distance from
+    // the blue alliance wall, to the blue alliance hub center point, plus
+    // the distance between the center of the blue alliance hub and the center of
+    // the red alliance hub
+    public static final Pose2d kRedHubLocation = new Pose2d(
+        Units.inchesToMeters(182.11 + 143.5 * 2),
+        Units.inchesToMeters(158.84),
+        Rotation2d.fromDegrees(0)
+    );
+
 }

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -1,12 +1,15 @@
 package frc.robot.constants;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 
 public class DrivetrainConstants {
-    // TODO Hold over from 2025, adjust?
+    public static final double kMaxSpeedMetersPerSecond = 4.663;
-    public static final double kMaxSpeedMetersPerSecond = 4.125;
     public static final double kMaxAngularSpeed = 2 * Math.PI;
 
     public static final double kTrackWidth = Units.inchesToMeters(23.75);
@@ -17,15 +20,15 @@ public class DrivetrainConstants {
     public static final double kRearLeftMagEncoderOffset = 3.761;
     public static final double kRearRightMagEncoderOffset = 2.573;
 
-    public static final int kFrontLeftDrivingCANID = 0;
+    public static final int kFrontLeftDrivingCANID = 4;
     public static final int kFrontRightDrivingCANID = 3;
     public static final int kRearLeftDrivingCANID = 1;
     public static final int kRearRightDrivingCANID = 2;
 
-    public static final int kFrontLeftTurningCANID = 8;
+    public static final int kFrontLeftTurningCANID = 7; // 8
-    public static final int kFrontRightTurningCANID = 9;
+    public static final int kFrontRightTurningCANID = 21; //9
-    public static final int kRearLeftTurningCANID = 7;
+    public static final int kRearLeftTurningCANID = 6; //7
-    public static final int kRearRightTurningCANID = 6;
+    public static final int kRearRightTurningCANID = 8; //6
 
     public static final int kFrontLeftAnalogInPort = 3;
     public static final int kFrontRightAnalogInPort = 2;
@@ -35,9 +38,15 @@ public class DrivetrainConstants {
     public static final boolean kGyroReversed = true;
 
     // TODO Hold over from 2025, adjust?
-    public static final double kHeadingP = .1;
+    public static final double kHeadingP = .65;
     public static final double kXTranslationP = .5;
     public static final double kYTranslationP = .5;
+    public static final double kYawPIDTolerance = Units.degreesToRadians(1);
+
+    // TODO How much do we trust gyro and encoders vs vision estimates.
+    // NOTE: Bigger values indicate LESS trust. Generally all three values for a given matrix should be the same
+    public static final Matrix<N3, N1> kSensorFusionOdometryStdDevs = VecBuilder.fill(0.1, 0.1, 0.1);
+    public static final Matrix<N3, N1> kVisionOdometryStdDevs = VecBuilder.fill(0.3, 0.3, 0.3);
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
     public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(

src/main/java/frc/robot/constants/HoodConstants.java

@@ -1,27 +1,54 @@
 package frc.robot.constants;
 
+import java.io.BufferedReader;
+import java.io.File;
+import java.io.FileReader;
+import java.io.IOException;
+import java.nio.file.Path;
+import java.util.Map;
+
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Filesystem;
+import frc.robot.constants.ShooterConstants.ShooterSpeeds;
+
 public class HoodConstants {
     // TODO Real Values
-    public static final int kMotorCANID = 0;
+    public static final int kMotorCANID = 12;
 
-    public static final double kP = 0;
+    public static final double kConversionFactor = (1.0/3.0)*(8.0/147.0)*2*Math.PI;
+
+    public static final double kP = 1.75;
     public static final double kI = 0;
     public static final double kD = 0;
-    public static final double kS = 0;
+    public static final double kS = 0.435;
     public static final double kV = 0;
     public static final double kA = 0;
-    public static final double kStartupAngle = 0;
+    public static final double kStartupAngle = 0.0;
+    public static final double kMaxManualSpeedMultiplier = 0.1;
+    public static final double kTolerance = Math.toRadians(0.5);
+
+    public static final double kAmpsToTriggerPositionReset = 10;
+
+    // TODO This is just barely longer than the default frame time for output current information
+    // Should this be longer?
+    public static final double kTimeAboveThresholdToReset = .25;
 
     public static final int kCurrentLimit = 15;
 
-    public static final boolean kInverted = false;
+    public static final boolean kInverted = true;
+    public static final boolean kUseInterpolatorForAngle = false;
 
     public static final IdleMode kIdleMode = IdleMode.kBrake;
 
+    public static final Map<ShooterSpeeds, InterpolatingDoubleTreeMap> kHoodInterpolators = Map.of(
+        ShooterSpeeds.kHubSpeed, new InterpolatingDoubleTreeMap()
+    );
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
 
     public static final SparkMaxConfig kConfig = new SparkMaxConfig();
@@ -31,13 +58,35 @@ public class HoodConstants {
             .idleMode(kIdleMode)
             .inverted(kInverted)
             .smartCurrentLimit(kCurrentLimit);
+        kConfig.encoder
+            .positionConversionFactor(kConversionFactor)
+            .velocityConversionFactor(kConversionFactor / 60);
         kConfig.closedLoop
-            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
             .pid(kP, kI, kD)
             .outputRange(-1, 1)
-            .positionWrappingEnabled(true)
+            .allowedClosedLoopError(kTolerance, ClosedLoopSlot.kSlot0)
-            .positionWrappingInputRange(0, Math.PI * 2)
             .feedForward
                 .sva(kS, kV, kA);
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 40)), 
+            Double.valueOf(Units.degreesToRadians(9.5)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 60)), 
+            Double.valueOf(Units.degreesToRadians(12.5)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 80)),
+            Double.valueOf(Units.degreesToRadians(16.25)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 100)), 
+            Double.valueOf(Units.degreesToRadians(20.5)));
+
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 120)), 
+            Double.valueOf(Units.degreesToRadians(23.5)));
     }
 }

src/main/java/frc/robot/constants/IntakePivotConstants.java

@@ -7,8 +7,8 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 public class IntakePivotConstants {
     // TODO Real values
     public enum IntakePivotPosition {
-        kUp(0),
+        kUp(Math.toRadians(116.0)),
-        kDown(0);
+        kDown(Math.toRadians(0.0));
         private double positionRadians;
 
         private IntakePivotPosition(double positionRadians) {
@@ -20,23 +20,27 @@ public class IntakePivotConstants {
         }
     }
 
-    public static final int kLeftMotorCANID = 0;
+    public static final int kLeftMotorCANID = 16;
-    public static final int kRightMotorCANID = 1;
+    public static final int kRightMotorCANID = 9;
 
-    public static final double kConversionFactor = 0;
+    public static final double kConversionFactor = 60.0/11.0*60.0/18.0*38.0/16.0;
+
+    // Ultra conservative multiplier to prevent 1/8" lexan destruction, modify at your own peril
+    public static final double kMaxManualSpeedMultiplier = .3;
 
     public static final double kP = 0;
     public static final double kI = 0;
     public static final double kD = 0;
     public static final double kS = 0;
-    public static final double kV = 0;
+    public static final double kV = 5.26;
-    public static final double kA = 0;
+    public static final double kA = 0.05;
+    public static final double kG = 0.25;
 
     public static final boolean kInvertMotors = false;
 
     public static final int kCurrentLimit = 30;
 
-    public static final IdleMode kIdleMode = IdleMode.kBrake;
+    public static final IdleMode kIdleMode = IdleMode.kCoast;
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
     
@@ -47,23 +51,23 @@ public class IntakePivotConstants {
         KLeftMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors);
+            .inverted(false);
-        KLeftMotorConfig.absoluteEncoder
+        KLeftMotorConfig.encoder
             .positionConversionFactor(kConversionFactor)
             .velocityConversionFactor(kConversionFactor / 60);
         KLeftMotorConfig.closedLoop
-            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
             .pid(kP, kI, kD)
             .outputRange(-1, 1)
             .positionWrappingEnabled(true)
             .positionWrappingInputRange(0, 2 * Math.PI)
             .feedForward
-                .sva(kS, kV, kA);
+                .svag(kS, kV, kA, kG);
 
         kRightMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors)
+            .inverted(true)
-            .follow(kLeftMotorCANID);
+            ;//.follow(kLeftMotorCANID);
     }
 }

src/main/java/frc/robot/constants/IntakeRollerConstants.java

@@ -5,12 +5,15 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 public class IntakeRollerConstants {
     // TODO Real values
-    public static final int kLeftMotorCANID = 0;
+    public static final int kRightMotorCANID = 20;
-    public static final int kRightMotorCANID = 0;
+    public static final int kLeftMotorCANID = 1;
 
-    public static final int kCurrentLimit = 30;
+    public static final int kCurrentLimit = 65;
 
-    public static final boolean kInvertMotors = false;
+    public static final boolean kInvertLeftMotor = false;
+    public static final boolean kInvertRightMotor = true;
+
+    public static final double kSpeed = 1;
 
     public static final IdleMode kIdleMode = IdleMode.kCoast;
 
@@ -23,12 +26,11 @@ public class IntakeRollerConstants {
         leftMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors);
+            .inverted(kInvertLeftMotor);
-
         rightMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors)
+            .inverted(kInvertRightMotor)
-            .follow(kLeftMotorCANID);
+        ;
     }
 }

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -1,12 +1,14 @@
 package frc.robot.constants;
 
 import com.ctre.phoenix6.configs.AudioConfigs;
+import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.FeedbackConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
@@ -14,8 +16,26 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 import edu.wpi.first.math.util.Units;
 
 public class ModuleConstants {
+    public enum ModuleName {
+        kFrontLeft("FrontLeft"),
+        kFrontRight("FrontRight"),
+        kRearLeft("RearLeft"),
+        kRearRight("RearRight");
+
+        private String loggableName;
+
+        private ModuleName(String loggableName) {
+            this.loggableName = loggableName;
+        }
+
+        public String getLoggableName() {
+            return "Drivetrain/Modules/" + loggableName;
+        }
+        
+    }
+
     // DRIVING MOTOR CONFIG (Kraken)
-    public static final double kDrivingMotorReduction = (14.0 * 28.0 * 15.0) / (50 * 16 * 45);
+    public static final double kDrivingMotorReduction = (50 * 16 * 45)/(14.0 * 28.0 * 15.0);
 
     public static final double kDrivingMotorFeedSpeedRPS = KrakenMotorConstants.kFreeSpeedRPM / 60;
     public static final double kWheelDiameterMeters = Units.inchesToMeters(4);
@@ -26,16 +46,17 @@ public class ModuleConstants {
     public static final double kDrivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRPS;
     
     // TODO Hold over from 2025, adjust?
-    public static final double kDriveP = .04;
+    public static final double kDriveP = .06;
     public static final double kDriveI = 0;
     public static final double kDriveD = 0;
     public static final double kDriveS = 0;
     public static final double kDriveV = kDrivingVelocityFeedForward;
     public static final double kDriveA = 0;
+    public static final double kClosedLoopRampRate = .01;
 
     // TODO Hold over from 2025, adjust?
-    public static final int kDriveMotorStatorCurrentLimit = 100;
+    public static final int kDriveMotorStatorCurrentLimit = 90;
-    public static final int kDriveMotorSupplyCurrentLimit = 65;
+    public static final int kDriveMotorSupplyCurrentLimit = 40;
 
     // TODO Hold over from 2025, adjust?
     public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;
@@ -45,10 +66,12 @@ public class ModuleConstants {
     public static final double kTurningMotorReduction = 150.0/7.0;
     public static final double kTurningFactor = 2 * Math.PI / kTurningMotorReduction;
     // TODO Adjust? Let over from 2025
-    public static final double kTurnP = 1;
+    public static final double kTurnP = 12;
     public static final double kTurnI = 0;
     public static final double kTurnD = 0;
 
+    public static final double kTurnTolerance = Math.toRadians(0.25);
+
     public static final boolean kIsEncoderInverted = false;
 
     // TODO How sensitive is too sensitive?
@@ -67,6 +90,7 @@ public class ModuleConstants {
     public static final MotorOutputConfigs kDriveMotorConfig = new MotorOutputConfigs();
     public static final AudioConfigs kAudioConfig = new AudioConfigs();
     public static final Slot0Configs kDriveSlot0Config = new Slot0Configs();
+    public static final ClosedLoopRampsConfigs kDriveClosedLoopRampConfig = new ClosedLoopRampsConfigs();
 
     static {
         kDriveFeedConfig.SensorToMechanismRatio = kDrivingMotorReduction;
@@ -88,6 +112,8 @@ public class ModuleConstants {
         kDriveSlot0Config.kV = kDriveV;
         kDriveSlot0Config.kA = kDriveA;
 
+        kDriveClosedLoopRampConfig.withVoltageClosedLoopRampPeriod(kClosedLoopRampRate);
+
         turningConfig
             .idleMode(kTurnIdleMode)
             .smartCurrentLimit(kTurnMotorCurrentLimit)
@@ -100,6 +126,7 @@ public class ModuleConstants {
             .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
             .pid(kTurnP, kTurnI, kTurnD)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(kTurnTolerance, ClosedLoopSlot.kSlot0)
             .positionWrappingEnabled(true)
             .positionWrappingInputRange(0, 2 * Math.PI);
             

src/main/java/frc/robot/constants/PhotonConstants.java

@@ -2,16 +2,36 @@ package frc.robot.constants;
 
 import java.util.List;
 
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.util.Units;
 import frc.robot.utilities.PhotonVisionConfig;
 
 public class PhotonConstants {
-    public static final String kCamera1Name = "pv1";
+    public static final String kCamera1Name = "CameraPV1";
-    public static final String kCamera2Name = "pv2";
+    public static final String kCamera2Name = "CameraPV2";
 
     // TODO Need actual values for all of this
-    public static final Transform3d kCamera1RobotToCam = new Transform3d();
+    public static final Transform3d kCamera1RobotToCam = new Transform3d(
-    public static final Transform3d kCamera2RobotToCam = new Transform3d();
+        Units.inchesToMeters(1.5),
+        Units.inchesToMeters(-8.5),
+        Units.inchesToMeters(28.5),
+        new Rotation3d(
+            Units.degreesToRadians(0), 
+            Units.degreesToRadians(-24.0), 
+            Units.degreesToRadians(30.0)
+        )
+    );
+    public static final Transform3d kCamera2RobotToCam = new Transform3d(
+        Units.inchesToMeters(1.5),
+        Units.inchesToMeters(-10.5),
+        Units.inchesToMeters(28.5),
+        new Rotation3d(
+            Units.degreesToRadians(0.0), 
+            Units.degreesToRadians(-24.0), 
+            Units.degreesToRadians(-10.0)
+        )
+    );
 
     public static final double kCamera1HeightMeters = 0;
     public static final double kCamera1PitchRadians = 0;

src/main/java/frc/robot/constants/ShooterConstants.java

@@ -1,114 +1,109 @@
 package frc.robot.constants;
 
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.FeedbackSensor;
 import com.revrobotics.spark.config.SparkMaxConfig;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.util.Units;
 
+
 public class ShooterConstants {
     public enum ShooterSpeeds {
-        kHubSpeed(0, 0),
+        kHubSpeed(3000.0),
-        kFeedSpeed(0, 0);
+        kFeedSpeed(5000.0),
+        kIdleSpeed(750.0);
 
-        private double frontRollerMPS;
+        private double speedMPS;
-        private double rearRollerMPS;
+        private double speedRPM;
 
-        private ShooterSpeeds(double frontRollerMPS, double rearRollerMPS) {
+        private ShooterSpeeds(double speedRPM) {
-            this.frontRollerMPS = frontRollerMPS;
+            this.speedMPS = speedRPM * kWheelDiameter*Math.PI;
-            this.rearRollerMPS = rearRollerMPS;
+            this.speedRPM = speedRPM;
         }
 
-        public double getFrontRollerMPS() {
+        public double getSpeedMPS() {
-            return frontRollerMPS;
+            return speedMPS * kWheelDiameter*Math.PI;
         }
 
-        public double getRearRollerMPS() {
+        public double getSpeedRPM(){
-            return rearRollerMPS;
+            return speedRPM;
         }
     }
 
     // TODO Conversion factor?
 
-    public static final double kWheelDiameter = Units.inchesToMeters(6);
+    public static final double kWheelDiameter = Units.inchesToMeters(4);
 
     // TODO Real values
-    public static final int kFrontShooterMotor1CANID = 0;
+    public static final int kLeftShooterMotorCANID = 2;
-    public static final int kFrontShooterMotor2CANID = 0;
+    public static final int kRightShooterMotorCANID = 5;
-    public static final int kRearShooterMotor1CANID = 0;
-    public static final int kRearShooterMotor2CANID = 0;
 
-    public static final boolean kFrontShooterMotor1Inverted = false;
+    public static final boolean kLeftShooterMotorInverted = true;
-    public static final boolean kFrontShooterMotor2Inverted = false;
+    public static final boolean kRightShooterMotorInverted = false;
-    public static final boolean kRearShooterMotor1Inverted = false;
-    public static final boolean kRearShooterMotor2Inverted = false;
 
-    public static final double kFrontP = 0;
+    public static final double kLeftP = 0.75;//0.01;//0.001;
-    public static final double kFrontI = 0;
+    public static final double kLeftI = 0;
-    public static final double kFrontD = 0;
+    public static final double kLeftD = 0;//0.1;//1.8;
-    public static final double kFrontS = 0;
+    public static final double kLeftS = 0;
-    public static final double kFrontV = 0;
+    public static final double kLeftV = 0.00129;
-    public static final double kFrontA = 0;
+    public static final double kLeftA = 0;
 
-    public static final double kRearP = 0;
+    public static final double kRightP = 0.75;//0.001;//0.001;
-    public static final double kRearI = 0;
+    public static final double kRightI = 0;
-    public static final double kRearD = 0;
+    public static final double kRightD = 0;//0.1;
-    public static final double kRearS = 0;
+    public static final double kRightS = 0;
-    public static final double kRearV = 0;
+    public static final double kRightV = 0.00125;
-    public static final double kRearA = 0;
+    public static final double kRightA = 0;
+
+    public static final double kMaxManualSpeedMultiplier = 1;
+
+    public static final double kShooterHeightMeters = 0;
 
     // TODO Is this value sane?
-    public static final int kCurrentLimit = 30;
+    public static final int kCurrentLimit = 60;
 
     public static final IdleMode kShooterIdleMode = IdleMode.kCoast;
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
 
-    public static final SparkMaxConfig kFrontMotor1Config = new SparkMaxConfig();
+    public static final SparkMaxConfig kLeftMotorConfig = new SparkMaxConfig();
-    public static final SparkMaxConfig kFrontMotor2Config = new SparkMaxConfig();
+    public static final SparkMaxConfig kRightMotorConfig = new SparkMaxConfig();
-    public static final SparkMaxConfig kRearMotor1Config = new SparkMaxConfig();
-    public static final SparkMaxConfig kRearMotor2Config = new SparkMaxConfig();
 
 
     static {
-        kFrontMotor1Config
+        kLeftMotorConfig
             .idleMode(kShooterIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kFrontShooterMotor1Inverted);
+            .inverted(kLeftShooterMotorInverted);
-        kFrontMotor1Config.absoluteEncoder
+        kLeftMotorConfig.absoluteEncoder
-            .positionConversionFactor(kWheelDiameter * Math.PI)
+            .positionConversionFactor(1)
-            .velocityConversionFactor(kWheelDiameter * Math.PI / 60);
+            .velocityConversionFactor(60)
-        kFrontMotor1Config.closedLoop
+            .averageDepth(8); // VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
+        kLeftMotorConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-            .pid(kFrontP, kFrontI, kFrontD)
+            .pid(kLeftP, kLeftI, kLeftD, ClosedLoopSlot.kSlot0)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
             .feedForward
-                .sva(kFrontS, kFrontV, kFrontA);
+                .sva(kLeftS, kLeftV, kLeftA, ClosedLoopSlot.kSlot0);
 
-        kFrontMotor2Config
+        kRightMotorConfig
             .idleMode(kShooterIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kFrontShooterMotor2Inverted)
+            .inverted(kRightShooterMotorInverted);
-            .follow(kFrontShooterMotor1CANID);
+        kRightMotorConfig.absoluteEncoder
-
+            .positionConversionFactor(1)
-        kRearMotor1Config
+            .velocityConversionFactor(60)
-            .idleMode(kShooterIdleMode)
+            .averageDepth(8)// VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
-            .smartCurrentLimit(kCurrentLimit)
+            .inverted(true);
-            .inverted(kRearShooterMotor1Inverted);
+        kRightMotorConfig.closedLoop
-        kRearMotor1Config.absoluteEncoder
-            .positionConversionFactor(kWheelDiameter * Math.PI)
-            .velocityConversionFactor(kWheelDiameter * Math.PI / 60);
-        kRearMotor1Config.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-            .pid(kRearP, kRearI, kRearD)
+            .pid(kRightP, kRightI, kRightD)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
             .feedForward
-                .sva(kRearS, kRearV, kRearA);
+                .sva(kRightS, kRightV, kRightA, ClosedLoopSlot.kSlot0);
-
-        kRearMotor2Config
-            .idleMode(kShooterIdleMode)
-            .smartCurrentLimit(kCurrentLimit)
-            .inverted(kRearShooterMotor2Inverted)
-            .follow(kRearShooterMotor1CANID);
     }
 }

src/main/java/frc/robot/constants/SpindexerConstants.java

@@ -10,16 +10,19 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 public class SpindexerConstants {
     // TODO Real values
     public static final int kSpindexerMotorCANID = 0;
-    public static final int kFeederMotorCANID = 0;
+    public static final int kFeederMotorCANID = 4;
 
-    public static final int kSpindexerStatorCurrentLimit = 80;
+    public static final int kSpindexerStatorCurrentLimit = 95;
-    public static final int kSpindexerSupplyCurrentLimit = 30;
+    public static final int kSpindexerSupplyCurrentLimit = 50;
     public static final int kFeederCurrentLimit = 30;
 
+    public static final double kSpindexerSpeed = 1;
+    public static final double kFeederSpeed = 1;
+
     public static final boolean kFeederMotorInverted = false;
 
     public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive;
-    public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Brake;
+    public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Coast;
 
     public static final IdleMode kFeederIdleMode = IdleMode.kBrake;
 

src/main/java/frc/robot/interfaces/IAprilTagProvider.java

@@ -1,39 +0,0 @@
-package frc.robot.interfaces;
-
-import java.util.OptionalDouble;
-
-/**
- * An interface which ensures a class can provide common AprilTag oriented
- * information from various sources in a consistent way.
- */
-public interface IAprilTagProvider {
-    /**
-     * A method to get the tags currently in the camera's field of view
-     * @return
-     */
-    public int[] getVisibleTagIDs();
-
-    /**
-     * A method to get the distance from <i>the camera</i> to the AprilTag specified
-     * 
-     * @param id The ID of the AprilTag to give a distance to
-     * @return The distance, in meters, to the target, or OptionalDouble.empty() if the tag is not present in the camera's view
-     */
-    public OptionalDouble getTagDistanceFromCameraByID(int id);
-
-    /**
-     * A method to get the pitch from the center of the image of a particular AprilTag
-     * 
-     * @param id The ID of the AprilTag to get the pitch of
-     * @return The pitch, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
-     */
-    public OptionalDouble getTagPitchByID(int id);
-
-    /**
-     * A method to get the yaw from the center of the image of a particular AprilTag
-     * 
-     * @param id The ID of the AprilTag to get the yaw of
-     * @return The yaw, in degrees, of the target, or OptionalDouble.empty() if the tag is not present in the camera's view
-     */
-    public OptionalDouble getTagYawByID(int id);
-}

src/main/java/frc/robot/interfaces/IVisualPoseProvider.java

@@ -1,27 +0,0 @@
-package frc.robot.interfaces;
-
-import java.util.Optional;
-
-import edu.wpi.first.math.geometry.Pose2d;
-
-/**
- * An interface which ensures a class' ability to provide visual pose information
- * in a consistent way
- */
-public interface IVisualPoseProvider {
-    /**
-     * A record that can contain the two elements necessary for a WPILIB
-     * pose estimator to use the information from a vision system as part of a full
-     * robot pose estimation
-     */
-    public record VisualPose(Pose2d visualPose, double timestamp) {}
-
-    /**
-     * Return a VisualPose or null if an empty Optional if none is available.
-     * Implementation should provide an empty response if it's unable to provide
-     * a reliable pose, or any pose at all.
-     * 
-     * @return An Optional containing a VisualPose, or empty if no VisualPose can reliably be provided
-     */
-    public Optional<VisualPose> getVisualPose();
-}

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

@@ -0,0 +1,74 @@
+package frc.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ClimberConstants;
+import frc.robot.constants.ClimberConstants.ClimberPositions;
+
+public class Climber extends SubsystemBase {
+    private SparkMax motor;
+
+    private RelativeEncoder encoder;
+
+    private SparkClosedLoopController controller;
+
+    private ClimberPositions targetPosition;
+
+    public Climber() {
+        motor = new SparkMax(ClimberConstants.kMotorCANID, MotorType.kBrushless);
+
+        motor.configure(
+            ClimberConstants.kMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        encoder = motor.getEncoder();
+
+        controller = motor.getClosedLoopController();
+
+        targetPosition = null;
+    }
+
+    @Override
+    public void periodic() {
+        Logger.recordOutput("Climber/TargetPositionMeters", targetPosition == null ? -1 : targetPosition.getPositionMeters());
+        Logger.recordOutput("Climber/CurrentPositionMeters", encoder.getPosition());
+        Logger.recordOutput("Climber/AtSetpoint", controller.isAtSetpoint());
+    }
+
+    public Command maintainPosition(ClimberPositions position) {
+        return run(() -> {
+            targetPosition = position;
+
+            controller.setSetpoint(
+                position.getPositionMeters(), 
+                ControlType.kPosition
+            );
+        });
+    }
+
+    public Command manualSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            targetPosition = null;
+
+            motor.set(speed.getAsDouble());
+        });
+    }
+
+    public Command stop() {
+        return manualSpeed(() -> 0);
+    }
+}

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

@@ -1,9 +1,10 @@
 package frc.robot.subsystems;
 
+import java.util.List;
 import java.util.Optional;
-import java.util.OptionalDouble;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 
 import org.littletonrobotics.junction.Logger;
 
@@ -28,8 +29,10 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
-import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
+import frc.robot.constants.ModuleConstants.ModuleName;
 import frc.robot.utilities.SwerveModule;
+import frc.robot.utilities.Utilities;
+import frc.robot.utilities.VisualPose;
 
 public class Drivetrain extends SubsystemBase {
     private SwerveModule frontLeft;
@@ -41,12 +44,11 @@ public class Drivetrain extends SubsystemBase {
 
     private SwerveDrivePoseEstimator estimator;
 
-    private PhotonVision camera1;
+    private PIDController yawRotationController;
-    private PhotonVision camera2;
 
-    public Drivetrain() {
+    public Drivetrain(Pose2d startupPose) {
         frontLeft = new SwerveModule(
-            "FrontLeft", 
+            ModuleName.kFrontLeft, 
             DrivetrainConstants.kFrontLeftDrivingCANID, 
             DrivetrainConstants.kFrontLeftTurningCANID, 
             DrivetrainConstants.kFrontLeftAnalogInPort, 
@@ -54,7 +56,7 @@ public class Drivetrain extends SubsystemBase {
         );
 
         frontRight = new SwerveModule(
-            "FrontRight", 
+            ModuleName.kFrontRight, 
             DrivetrainConstants.kFrontRightDrivingCANID, 
             DrivetrainConstants.kFrontRightTurningCANID, 
             DrivetrainConstants.kFrontRightAnalogInPort, 
@@ -62,7 +64,7 @@ public class Drivetrain extends SubsystemBase {
         );
 
         rearLeft = new SwerveModule(
-            "RearLeft", 
+            ModuleName.kRearLeft, 
             DrivetrainConstants.kRearLeftDrivingCANID, 
             DrivetrainConstants.kRearLeftTurningCANID, 
             DrivetrainConstants.kRearLeftAnalogInPort, 
@@ -70,7 +72,7 @@ public class Drivetrain extends SubsystemBase {
         );
 
         rearRight = new SwerveModule(
-            "RearRight", 
+            ModuleName.kRearRight, 
             DrivetrainConstants.kRearRightDrivingCANID, 
             DrivetrainConstants.kRearRightTurningCANID, 
             DrivetrainConstants.kRearRightAnalogInPort, 
@@ -79,6 +81,14 @@ public class Drivetrain extends SubsystemBase {
 
         gyro = new AHRS(NavXComType.kMXP_SPI);
 
+        yawRotationController = new PIDController(
+            DrivetrainConstants.kHeadingP, 
+            0, 
+            0
+        );
+        yawRotationController.enableContinuousInput(-Math.PI, Math.PI);
+        yawRotationController.setTolerance(DrivetrainConstants.kYawPIDTolerance);
+
         // TODO 2025 used non-standard deviations for encoder/gyro inputs and vision, will need to be tuned for 2026 in the future
         estimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.kDriveKinematics, 
@@ -89,7 +99,9 @@ public class Drivetrain extends SubsystemBase {
                 rearLeft.getPosition(),
                 rearRight.getPosition()
             },
-            new Pose2d()
+            startupPose != null ? startupPose : new Pose2d(),
+            DrivetrainConstants.kSensorFusionOdometryStdDevs,
+            DrivetrainConstants.kVisionOdometryStdDevs
         );
 
         if(AutoConstants.kAutoConfigOk) {
@@ -131,35 +143,36 @@ public class Drivetrain extends SubsystemBase {
 
         Logger.recordOutput("Drivetrain/Pose", getPose());
         Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue());
-        Logger.recordOutput("Drivetrain/Heading", getHeading());
+        Logger.recordOutput("Drivetrain/Heading", getHeadingDegrees());
+        Logger.recordOutput("Drivetrain/Velocity", getCurrentChassisSpeeds());
     }
 
-    public Command runFrontLeft(double staticSpeed, double staticAngleDegrees) {
+    /**
+     * Can be used to run an individual module on the drive base a static speed while maintaining a static angle.
+     * 
+     * Good for diagnosing issues with swerve module configuration. Essentially useless otherwise.
+     * 
+     * @param name The ModuleName enumeration that indicates which module you want to control
+     * @param staticSpeed The static speed in Meters Per Second to spin the drive wheel at
+     * @param staticAngleDegrees The static angle in degrees that you want the wheel to face
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command runIndividualModule(ModuleName name, double staticSpeed, double staticAngleDegrees) {
+        SwerveModule module = List.of(
+            frontLeft,
+            frontRight,
+            rearLeft,
+            rearRight
+        ).stream()
+        .filter((m) -> m.getModuleName() == name)
+        .findFirst()
+        .get();
+
         return run(() -> {
-            frontLeft.setDesiredState(new SwerveModuleState(
+            module.setDesiredState(new SwerveModuleState(
                 staticSpeed, 
-                Rotation2d.fromDegrees(staticAngleDegrees)));
+                Rotation2d.fromDegrees(staticAngleDegrees)
-        });
+            ));
-    }
-    public Command runFrontRight(double staticSpeed, double staticAngleDegrees) {
-        return run(() -> {
-            frontRight.setDesiredState(new SwerveModuleState(
-                staticSpeed, 
-                Rotation2d.fromDegrees(staticAngleDegrees)));
-        });
-    }
-    public Command runRearLeft(double staticSpeed, double staticAngleDegrees) {
-        return run(() -> {
-            rearLeft.setDesiredState(new SwerveModuleState(
-                staticSpeed, 
-                Rotation2d.fromDegrees(staticAngleDegrees)));
-        });
-    }
-    public Command runRearRight(double staticSpeed, double staticAngleDegrees) {
-        return run(() -> {
-            rearRight.setDesiredState(new SwerveModuleState(
-                staticSpeed, 
-                Rotation2d.fromDegrees(staticAngleDegrees)));
         });
     }
 
@@ -172,32 +185,121 @@ public class Drivetrain extends SubsystemBase {
         });  
     }
 
-    // TODO check both cameras
+    /**
-    /*public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
+     * Rotates the robot to a face a given Pose2d position on the field
-        if (camera1 == null) {
+     * 
-            return new PrintCommand("Camera 1 not available");
+     * Note that this Command does not provide a means of timeout. If you are
-        }
+     * using this in an auto context, this Command should be decorated with 
+     * withTimeout(<some_value>). Otherwise, you will be waiting for the PID
+     * Controller doing the work to report that it is at the desired setpoint. 
+     * 
+     * @param targetPose The Pose2d object to rotate the robot towards
+     * @param rotate180 When false, the front of the robot faces the specified pose, when true
+     *   the back of the robot faces the specified pose
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command rotateToPose(Pose2d targetPose, boolean rotate180) {
+        return lockRotationToSuppliedPose(() -> targetPose, () -> 0, () -> 0, rotate180)
+            .until(yawRotationController::atSetpoint);
+    }
 
-        // TODO The process variable is different here than what these constants are used for, may need to use something different
+    /**
-        PIDController controller = new PIDController(
+     * Locks the robots rotation to face the Alliance Hub on the field. 
-            AutoConstants.kPThetaController, 
+     * 
-            0, 
+     * This method is innately aware of which hub to face based on the assigned alliance color.
-            0
+     * 
+     * This method is <i>NOT</i> for autonomous, see rotateToPose
+     * 
+     * This method provides a field oriented mechanism of driving the robot, such that the robot
+     * is always facing the point on the field that is the center of the alliance hub. This
+     * method assumes that the robots estimated pose is reasonably accurate. 
+     * 
+     * @param xSpeed The X (forward/backward) translational speed of the robot
+     * @param ySpeed The Y (left/right) translational speed of the robot
+     * @param rotate180 When false, the front of the robot faces the hub, when true, the back
+     *   of the robot faces the hub
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
+        return lockRotationToSuppliedPose(
+            Utilities::getHubPose, 
+            xSpeed, 
+            ySpeed, 
+            rotate180
         );
+    }
 
-        return runOnce(controller::reset).andThen(
+    /**
+     * Locks the robots rotation to face a particular pose on the field
+     * 
+     * This method is <i>NOT</i> for autonomous, see rotateToPose
+     * 
+     * This method provides a field oriented mechanism of driving the robot, such that the robot
+     * is always facing the point on the field that is the Pose2d object being supplied. This 
+     * method assumes that the robots estimated pose is reasonably accurate.
+     * 
+     * @param poseSupplier A Supplier object, lambda, or method reference which consistently produces a Pose2d object to point towards
+     * @param xSpeed The X (forward/backward) translational speed of the robot
+     * @param ySpeed The Y (left/right) translational speed of the robot
+     * @param rotate180 When false, the front of the robot faces the supplied pose, when true, the back
+     *   of the robot faces the supplied pose
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command lockRotationToSuppliedPose(Supplier<Pose2d> poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
+        return runOnce(yawRotationController::reset).andThen(
             drive(
                 xSpeed,
                 ySpeed, 
                 () -> {
-                    OptionalDouble tagYaw = camera1.getTagYawByID(tagID);
+                    Pose2d faceTowards = poseSupplier.get();
 
-                    return (tagYaw.isEmpty() ? 0 : controller.calculate(tagYaw.getAsDouble(), 0));
+                    Rotation2d targetRotation = new Rotation2d(
+                        faceTowards.getX() - getPose().getX(), 
+                        faceTowards.getY() - getPose().getY()
+                    );
+
+                    if(rotate180) {
+                        targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
+                    }
+
+                    Logger.recordOutput("/HubAutoAlign/CurrentHeader", getHeading().getRadians());
+                    Logger.recordOutput("/HubAutoAlign/Setpoint", targetRotation.getRadians());
+
+                    double outputPower = -yawRotationController.calculate(
+                        getHeading().getRadians(),
+                        targetRotation.getRadians()
+                    );
+
+                    Logger.recordOutput("/HubAutoAlign/OutputPower", outputPower);
+
+                    return outputPower;
                 }, 
-                () -> false
+                () -> true
-           )
+            )
         );
-    }*/
+    }
+
+    /**
+     * A method to lock to a particular source of an external "yaw". The intent is for this yaw to be sourced from
+     * {@link frc.robot.subsystems.PhotonVision#getBestYawForTag(int)} which generates a "yaw" for a particular tag as referenced
+     * from the center point of the cameras image frame. The objective being to "0 the source" using a PID Controller, or in
+     * other terms, to center the provided tag in the camera's image frame.
+     * 
+     * @param yaw The "yaw" of the tag source relative to the center of the image frame
+     * @param xSpeed The X (forward/backward) translational speed of the robot
+     * @param ySpeed The Y (left/right) translational speed of the robot
+     * @return A complete Command structure that performs the specified action
+     */
+    public Command lockToYaw(DoubleSupplier yaw, DoubleSupplier xSpeed, DoubleSupplier ySpeed) {
+        return runOnce(yawRotationController::reset).andThen(
+            drive(
+                xSpeed,
+                ySpeed, 
+                () -> yawRotationController.calculate(yaw.getAsDouble(), 0),
+                () -> true
+            )
+        );
+    }
 
     public Command drivePathPlannerPath(PathPlannerPath path) {
         if(AutoConstants.kAutoConfigOk) {
@@ -229,7 +331,18 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
+    public Command zeroHeading() {
+        return run(() -> {
+            gyro.reset();
+            estimator.resetRotation(new Rotation2d(0));
+        });
+    }
+
     public void consumeVisualPose(VisualPose pose) {
+        if(Math.abs(pose.visualPose().minus(getPose()).getTranslation().getNorm()) > 1) {
+            return;
+        }
+        
         estimator.addVisionMeasurement(
             pose.visualPose(), 
             pose.timestamp()
@@ -274,7 +387,8 @@ public class Drivetrain extends SubsystemBase {
         SwerveModuleState[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
             fieldRelative ?
                 ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered, 
-                    estimator.getEstimatedPosition().getRotation()) :
+                    //estimator.getEstimatedPosition().getRotation()) :
+                    Rotation2d.fromDegrees(getGyroValue())) :
                 new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered)   
         );
 
@@ -314,7 +428,11 @@ public class Drivetrain extends SubsystemBase {
         return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
     }
 
-    public double getHeading() {
+    public Rotation2d getHeading() {
+        return estimator.getEstimatedPosition().getRotation();
+    }
+
+    public double getHeadingDegrees() {
         return estimator.getEstimatedPosition().getRotation().getDegrees();
     }
 }

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

@@ -1,60 +1,184 @@
 package frc.robot.subsystems;
 
+import java.util.Optional;
 import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
 
-import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.HoodConstants;
+import frc.robot.constants.ShooterConstants.ShooterSpeeds;
+import frc.robot.utilities.Utilities;
 
 public class Hood extends SubsystemBase {
     private SparkMax motor;
 
-    private AbsoluteEncoder encoder;
+    private RelativeEncoder encoder;
 
     private SparkClosedLoopController controller;
 
-    private double currentTargetRadians;
+    //private Trigger resetTrigger;
+    //private Trigger timerTrigger;
+
+    //private Timer resetTimer;
+
+    private double currentTargetDegrees;
 
     public Hood() {
         motor = new SparkMax(HoodConstants.kMotorCANID, MotorType.kBrushless);
 
-        encoder = motor.getAbsoluteEncoder();
+        motor.configure(
+            HoodConstants.kConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );   
+
+        encoder = motor.getEncoder();
+        encoder.setPosition(HoodConstants.kStartupAngle);
 
         controller = motor.getClosedLoopController();
 
-        currentTargetRadians = HoodConstants.kStartupAngle;
+        /*resetTimer = new Timer();
+        resetTimer.reset();
+
+        resetTrigger = new Trigger(() -> (motor.getOutputCurrent() > HoodConstants.kAmpsToTriggerPositionReset));
+        resetTrigger.onTrue(new InstantCommand(resetTimer::start));
+        resetTrigger.onFalse(new InstantCommand(() -> {
+            resetTimer.stop();
+            resetTimer.reset();
+        }));
+
+        timerTrigger = new Trigger(() -> resetTimer.hasElapsed(HoodConstants.kTimeAboveThresholdToReset));
+        timerTrigger.onTrue(new InstantCommand(() -> {
+            encoder.setPosition(0);
+            resetTimer.reset();
+        }));*/
+
+        currentTargetDegrees = HoodConstants.kStartupAngle;
     }
 
     @Override
     public void periodic() {
-        Logger.recordOutput("Hood/CurrentTarget", currentTargetRadians);
+        SmartDashboard.putNumber(
-        Logger.recordOutput("Hood/CurrentAngle", encoder.getPosition());
+            "HoodTargetDegrees", 
+            Math.toDegrees(currentTargetDegrees)
+        );
+
+        SmartDashboard.putNumber(
+            "HoodCurrentAngle", 
+            Math.toDegrees(encoder.getPosition())
+        );
+
+        SmartDashboard.putBoolean(
+            "HoodAtSetpoint", 
+            controller.isAtSetpoint()
+        );
+        
+        Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent());
+        Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees));
+        Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition()));
         Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
+        Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage());
     }
 
-    public Command trackToAngle(DoubleSupplier radianAngleSupplier) {
+    public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) {
-        return run(() -> {
+        return trackToAngle(() -> {
-            currentTargetRadians = radianAngleSupplier.getAsDouble();
+            Pose2d drivetrainPose = drivetrain.getPose();
+            Pose2d hubPose = Utilities.getHubPose();
 
-            controller.setSetpoint(currentTargetRadians, ControlType.kPosition);
+            double distance = drivetrainPose.getTranslation()
+                .getDistance(hubPose.getTranslation());
+
+            Logger.recordOutput("Hood/DistanceToHub", distance);
+
+            Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
+                
+            if(currentSpeeds.isPresent()) {
+                InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
+
+                if(map != null) {
+                    return MathUtil.clamp(map.get(distance), 0, 40);
+                } else {
+                    return 0;
+                }
+            } else {
+                return 0;
+            }
+        });
+    }
+
+    public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {
+        return run(() -> {
+            currentTargetDegrees = degreeAngleSupplier.getAsDouble();
+
+            controller.setSetpoint(currentTargetDegrees, ControlType.kPosition);
+        });
+    }
+
+    /**
+     * An automated form of resetting the hood position sensing.
+     * 
+     * Run down at the full manual speed (note that this is affected by the 
+     * kMaxManualSpeedMultiplier constant) until the timer trigger becomes true
+     * (i.e. the output current has been above the threshold (kAmpsToTriggerPositionReset) 
+     * for reset for the amount of specified by kTimeAboveThresholdToReset). Once
+     * that returns true, the motor is stopped until the timer trigger switches to false
+     * (i.e. it has reset the position automatically, because that's how it's configured, 
+     * and resets the timer to 0, which makes the timer trigger false)
+     * 
+     * @return A complete Command structure that performs the specified action
+     */
+    /*public Command automatedRezero() {
+        return manualSpeed(() -> -1)
+            .until(timerTrigger)
+            .andThen(
+                stop().until(timerTrigger.negate())
+        );
+    }
+
+    /**
+     * An alternate form of {@link #automatedRezero()} that doesn't rely on the triggers
+     * to reset the hood position to zero. Note that this method doesn't have any time limiting
+     * factor to it, as soon as the current goes above the threshold specified by 
+     * kAmpsToTriggerPositionReset the encoder position will be set to zero
+     * 
+     * @return A complete Command structure that performs the specified action
+     */
+    /*public Command automatedRezeroNoTimer() {
+        return manualSpeed(() -> -1)
+            .until(() -> motor.getOutputCurrent() >= HoodConstants.kAmpsToTriggerPositionReset)
+            .andThen(new InstantCommand(() -> encoder.setPosition(0)));
+    }*/
+
+    public Command manualSpeed(DoubleSupplier speed) {
+        currentTargetDegrees = 0;
+
+        return run(() -> {
+            motor.set(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier);
         });
     }
 
     public Command stop() {
-        return run(() -> {
+        return manualSpeed(() -> 0);
-            motor.disable();
-        });
     }
 
-    public double getTargetRadians() {
+    public double getTargetDegrees() {
-        return currentTargetRadians;
+        return currentTargetDegrees;
     }
 }

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

@@ -1,11 +1,12 @@
 package frc.robot.subsystems;
 
 import java.util.Optional;
+import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
 
-import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
@@ -13,6 +14,7 @@ import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.IntakePivotConstants;
 import frc.robot.constants.IntakePivotConstants.IntakePivotPosition;
@@ -21,7 +23,7 @@ public class IntakePivot extends SubsystemBase {
     private SparkMax leftMotor;
     private SparkMax rightMotor;
 
-    private AbsoluteEncoder encoder;
+    private RelativeEncoder encoder;
 
     private SparkClosedLoopController controller;
 
@@ -45,7 +47,8 @@ public class IntakePivot extends SubsystemBase {
 
         controller = leftMotor.getClosedLoopController();
 
-        encoder = leftMotor.getAbsoluteEncoder();
+        encoder = leftMotor.getEncoder();
+        encoder.setPosition(IntakePivotConstants.IntakePivotPosition.kUp.getPositionRadians());
     }
 
     @Override
@@ -58,9 +61,9 @@ public class IntakePivot extends SubsystemBase {
     }
 
     public Command maintainPosition(IntakePivotPosition position) {
-        currentTargetPosition = position;
-
         return run(() -> {
+            currentTargetPosition = position;
+
             if(currentTargetPosition == null) {
                 leftMotor.disable();
             } else {
@@ -69,8 +72,43 @@ public class IntakePivot extends SubsystemBase {
         });
     }
 
+    public Command manualSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            currentTargetPosition = null;
+            
+            leftMotor.set(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
+            rightMotor.set(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
+        });
+    }
+
+    /**
+     * Repeatedly moves the intake up and down. AKA "Jimmying" the intake
+     * 
+     * @param time How long the intake will go both ways for (seconds)
+     * @return Command that repeatedly Jimmys the intake
+     */
+    public Command jimmy(double time){
+        return Commands.repeatingSequence(
+            manualSpeed(() -> -0.75).withTimeout(time),
+            manualSpeed(() -> 0.75).withTimeout(time)
+        );
+    }
+
+    /**
+     * Repeatedly moves the intake up and down. AKA "Jimmying" the intake
+     * 
+     * @param time How long the intake will go both ways for (seconds)
+     * @return Command that repeatedly Jimmys the intake
+     */
+    public Command jimmy(DoubleSupplier time) {
+        return Commands.repeatingSequence(
+            manualSpeed(() -> -0.75).withTimeout(time.getAsDouble()),
+            manualSpeed(() -> 0.75).withTimeout(time.getAsDouble())
+        );
+    }
+
     public Command stop() {
-        return maintainPosition(null);
+        return manualSpeed(() -> 0);
     }
 
     public Optional<IntakePivotPosition> getCurrentTargetPosition() {

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

@@ -32,19 +32,22 @@ public class IntakeRoller extends SubsystemBase {
 
     public Command runIn() {
         return run(() -> {
-            leftMotor.set(1);
+            leftMotor.set(IntakeRollerConstants.kSpeed*0.9);
+            rightMotor.set(IntakeRollerConstants.kSpeed*0.9);
         });
     }
 
     public Command runOut() {
         return run(() -> {
-            leftMotor.set(-1);
+            leftMotor.set(-IntakeRollerConstants.kSpeed);
+            rightMotor.set(-IntakeRollerConstants.kSpeed);
         });
     }
 
     public Command stop() {
         return run(() -> {
             leftMotor.set(0);
+            rightMotor.set(0);
         });
     }
     

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

@@ -1,33 +1,49 @@
 package frc.robot.subsystems;
 
-import java.io.IOException;
 import java.util.ArrayList;
-import java.util.Comparator;
 import java.util.List;
 import java.util.Optional;
 import java.util.OptionalDouble;
 import java.util.function.Consumer;
-import java.util.stream.Stream;
 
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
-import org.photonvision.PhotonUtils;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.CompetitionConstants;
 import frc.robot.constants.PhotonConstants;
-import frc.robot.interfaces.IAprilTagProvider;
-import frc.robot.interfaces.IVisualPoseProvider;
-import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
 import frc.robot.utilities.PhotonVisionConfig;
+import frc.robot.utilities.VisualPose;
 
+/**
+ * This "Subsystem" is not actually a Subsystem. The intent is for this to be treated as
+ * a "resource", that is, something that is not inherently a physical mechanism to be controlled.
+ * 
+ * A "resource" in this instance should be thought of as something that can be safely shared
+ * by other Subsystems generally without collision if more that one Subsystem requires the 
+ * "resource" at any given time.
+ * 
+ * Resources should <i>NOT</i> produce Commands, they should not have a default Command.
+ * Resources do not have behaviors, and because Commands are in of themselves behaviors,
+ * this class should not have Commands. 
+ * 
+ * Part of the thinking behind creating the PhotonVision components this way is to rely
+ * on the CommandScheduler to call periodic. If this weren't the case, some other subsystem
+ * would have to manage calling for periodic updates, while still sharing the resource with 
+ * other subsystems <i>somehow</i>. 
+ * 
+ * This class is dynamic, by adding or removing PhotonVisionConfig objects to the "configs"
+ * List in the PhotonConstants file, you change what is set up internally in this class. 
+ * 1 config means 1 camera, 1 estimator, 1 stored pipeline result, 2 configs means 2 cameras,
+ * 2 estimators, etc. etc. 
+ */
 public class PhotonVision extends SubsystemBase {
     private PhotonCamera[] cameras;
     private PhotonPoseEstimator[] estimators;
@@ -60,139 +76,131 @@ public class PhotonVision extends SubsystemBase {
 
             if(!results.isEmpty()) {
                 latestResults.set(i, results.get(results.size() - 1));
-            }
 
-            Optional<EstimatedRobotPose> pose = estimators[i].update(latestResults.get(i));
+                Optional<EstimatedRobotPose> pose = estimators[i].update(latestResults.get(i));
 
-            if(!pose.isEmpty()) {
+                if(!pose.isEmpty()) {
-                VisualPose visualPose = new VisualPose(
+                    VisualPose visualPose = new VisualPose(
-                    pose.get().estimatedPose.toPose2d(), 
+                        cameras[i].getName(),
-                    pose.get().timestampSeconds
+                        pose.get().estimatedPose.toPose2d(), 
-                );
+                        pose.get().timestampSeconds
+                    );
 
-                for(Consumer<VisualPose> consumer: poseEstimateConsumers) {
+                    for(Consumer<VisualPose> consumer: poseEstimateConsumers) {
-                    consumer.accept(visualPose);
+                        consumer.accept(visualPose);
+                    }
                 }
             }
         }
     }
 
-    public void testMethod(int targetID) {
+    /**
-        Optional<PhotonTrackedTarget> target = latestResults.stream()
+     * Returns the best 3D pose for a given AprilTag ID as seen by the cameras on the robot.
-            .filter((p) -> p != null)
+     * 
-            .map(PhotonPipelineResult::getTargets)
+     * All cameras fields of view are observed, if no camera can see the given tag ID, this
-            .map(List::stream)
+     * method will return Optional.empty().
-            .reduce(Stream::concat)
+     * 
-            .get()
+     * Note that this method has no minimum confidence threshold for a tag. This means that 
-            .filter((p) -> p.getFiducialId() == targetID)
+     * if one camera thinks it sees the tag, even with very low confidence, it'll still return
-            .max(
+     * some sort of pose. 
-                Comparator.comparingDouble((ptt) -> {
+     * 
-                    return (double)ptt.getDetectedObjectConfidence();
+     * @param tagID The ID of the tag to look for in the latest results from all cameras
-                })
+     * @return An Optional object containing a Pose3d object, or Optional.empty() if
-            );
+     *   the tag is not present anywhere in the robots field of view. 
+     */
+    public Optional<Pose3d> getBestPoseForTag(int tagID) {
+        PhotonVisionConfig config = null;
+        Transform3d bestCameraToTarget = null;
+        float bestConfidence = -1;
 
-    }
+        for(int cameraIndex = 0; cameraIndex < latestResults.size(); cameraIndex++) {
+            if(latestResults.get(cameraIndex) == null) {
+                continue;
+            }
 
-    public void addPoseEstimateConsumer(Consumer<VisualPose> consumer) {
+            for(PhotonTrackedTarget target: latestResults.get(cameraIndex).getTargets()) {
-        poseEstimateConsumers.add(consumer);
+                if(target.getFiducialId() != tagID || bestConfidence > target.getDetectedObjectConfidence()) {
-    }
+                    continue;
+                }
 
-    /*public Trigger tagPrescenseTrigger(int targetTag) {
+                config = PhotonConstants.configs.get(cameraIndex);
-        return new Trigger(() -> {
+                bestCameraToTarget = target.bestCameraToTarget;
-            return List.of(latestResults).stream()
+                bestConfidence = target.getDetectedObjectConfidence();
-                .filter((p) -> p != null)
-                .anyMatch((p) -> {
-                    return p.getTargets().stream().map(PhotonTrackedTarget::getFiducialId).anyMatch((i) -> {
-                        return i == targetTag;
-                    });
-                });
-            });
-    }*/
-/* 
-    @Override
-    public OptionalDouble getTagDistanceFromCameraByID(int id) {
-        if (latestResult == null) {
-            return OptionalDouble.empty();
-        }
-
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-        
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            PhotonUtils.calculateDistanceToTargetMeters(
-                cameraHeightMeters, 
-                CompetitionConstants.kTagLayout.getTagPose(id).get().getZ(),
-                cameraPitchRadians, 
-                Units.degreesToRadians(desiredTarget.get().getPitch()))
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagPitchByID(int id) {
-        if(latestResult == null) {
-            OptionalDouble.empty();
-        }
-        
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            desiredTarget.get().getPitch()
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagYawByID(int id) {
-        if(latestResult == null) {
-            OptionalDouble.empty();
-        }
-        
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            desiredTarget.get().getYaw()
-        );
-    }
-
-    private Optional<PhotonTrackedTarget> getTargetFromList(List<PhotonTrackedTarget> targets, int id) {
-        for (PhotonTrackedTarget target : targets) {
-            if (target.getFiducialId() == id) {
-                return Optional.of(target);
             }
         }
 
-        return Optional.empty();
+        if(bestCameraToTarget == null) {
-    }
+            return Optional.empty();
-
-    @Override
-    public int[] getVisibleTagIDs() {
-        if(latestResult == null) {
-            return new int[] {};
         }
 
-        return latestResult.getTargets().stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray();
+        // This is based on what PhotonVision does for multitag Pose estimation
+        // See PhotonPoseEstimator.multiTagOnCoprocStrategy
+        // TODO This doesn't currently account for the offset of the tag relative to say the hub
+        // unclear if that offset amount will be important or not
+        return Optional.of(Pose3d.kZero
+            .plus(bestCameraToTarget.inverse())
+            .relativeTo(CompetitionConstants.kTagLayout.getOrigin())
+            .plus(config.robotToCamera().inverse()));
+    }
+
+    /**
+     * Returns the best yaw for a given AprilTag ID as seen by the cameras on the robot.
+     * 
+     * All cameras fields of view are observed, if no camera can see the given tag ID, this
+     * method will return OptionalDouble.empty()
+     * 
+     * Note that this method has no minimum confidence threshold for a tag. This means that
+     * if one camera thinks it sees the tag, even with very low confidence, it'll still
+     * return some sort of yaw value.
+     * 
+     * Note that the yaw value here is the yaw of the observed tag relative to the center
+     * of the cameras image frame. 
+     * 
+     * @param tagID The ID of the tag to look for in the latest results from all cameras
+     * @return An OptionalDouble object containing a Double representing the described yaw
+     *   of the AprilTag specified, or OptionalDouble.empty() if the tag is not present
+     *   anywhere in the robots field of view
+     */
+    public OptionalDouble getBestYawForTag(int tagID) {
+        double bestTagYaw = -1;
+        float bestConfidence = -1;
+
+        for(PhotonPipelineResult result: latestResults) {
+            if(result == null) {
+                continue;
+            }
+
+            for(PhotonTrackedTarget target: result.getTargets()) {
+                if(target.getFiducialId() != tagID || bestConfidence > target.getDetectedObjectClassID()) {
+                    continue;
+                }
+
+                bestTagYaw = target.getYaw();
+                bestConfidence = target.getDetectedObjectConfidence();
+            }
+        }
+
+        if(bestConfidence == -1) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(bestTagYaw);
+    }
+
+    /**
+     * Add a Consumer of VisualPose records to the PhotonVision resource. 
+     * 
+     * Each consumer will receive a VisualPose object when any camera produces a new
+     * VisualPose. 
+     * 
+     * The number of Poses produced in a given 20ms cycle is the same number as how many
+     * cameras there are on the robot, assuming those cameras see enough tags to generate a pose, 
+     * as currently all cameras configuration will generate a Pose2d
+     * 
+     * @param consumer The lambda, functional reference, or Consumer implementing object 
+     *  that will consume Poses produced by the PhotonVision resource.
+     */
+    public void addPoseEstimateConsumer(Consumer<VisualPose> consumer) {
+        poseEstimateConsumers.add(consumer);
     }
-    */
 }

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

@@ -1,119 +1,158 @@
 package frc.robot.subsystems;
 
 import java.util.Optional;
+import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
 
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
 import com.revrobotics.ResetMode;
+import com.revrobotics.spark.ClosedLoopSlot;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+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.ShooterConstants;
 import frc.robot.constants.ShooterConstants.ShooterSpeeds;
 
 public class Shooter extends SubsystemBase {
-    private SparkMax frontMotor1;
+    private SparkMax leftMotor;
-    private SparkMax frontMotor2;
+    private SparkMax rightMotor;
-    private SparkMax rearMotor1;
-    private SparkMax rearMotor2;
 
-    private AbsoluteEncoder frontEncoder;
+    private AbsoluteEncoder leftEncoder;
-    private AbsoluteEncoder rearEncoder;
+    private AbsoluteEncoder rightEncoder;
 
-    private SparkClosedLoopController frontClosedLoopController;
+    private RelativeEncoder rightRelative;
-    private SparkClosedLoopController rearClosedLoopController;
+
+    private SparkClosedLoopController leftClosedLoopController;
+    private SparkClosedLoopController rightClosedLoopController;
 
     private ShooterSpeeds targetSpeeds;
+    private SimpleMotorFeedforward shooterFFLeft;
+    private SimpleMotorFeedforward shooterFFRight;
 
     public Shooter() {
-        frontMotor1 = new SparkMax(ShooterConstants.kFrontShooterMotor1CANID, MotorType.kBrushless);
+        leftMotor = new SparkMax(ShooterConstants.kLeftShooterMotorCANID, MotorType.kBrushless);
-        frontMotor2 = new SparkMax(ShooterConstants.kFrontShooterMotor2CANID, MotorType.kBrushless);
+        rightMotor = new SparkMax(ShooterConstants.kRightShooterMotorCANID, MotorType.kBrushless);
-        rearMotor1 = new SparkMax(ShooterConstants.kRearShooterMotor1CANID, MotorType.kBrushless);
-        rearMotor2 = new SparkMax(ShooterConstants.kRearShooterMotor2CANID, MotorType.kBrushless);
 
-        frontMotor1.configure(
+        leftMotor.configure(
-            ShooterConstants.kFrontMotor1Config, 
+            ShooterConstants.kLeftMotorConfig, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
 
-        rearMotor1.configure(
+        rightMotor.configure(
-            ShooterConstants.kRearMotor1Config, 
+            ShooterConstants.kRightMotorConfig, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
 
-        frontMotor2.configure(
+        leftEncoder = leftMotor.getAbsoluteEncoder();
-            ShooterConstants.kFrontMotor2Config, 
+        rightEncoder = rightMotor.getAbsoluteEncoder();
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
 
-        rearMotor2.configure(
+        leftClosedLoopController = leftMotor.getClosedLoopController();
-            ShooterConstants.kRearMotor2Config, 
+        rightClosedLoopController = rightMotor.getClosedLoopController();
-            ResetMode.kResetSafeParameters, 
-            PersistMode.kPersistParameters
-        );
-
-        frontEncoder = frontMotor1.getAbsoluteEncoder();
-        rearEncoder = rearMotor1.getAbsoluteEncoder();
-
-        frontClosedLoopController = frontMotor1.getClosedLoopController();
-        rearClosedLoopController = rearMotor1.getClosedLoopController();
 
         // TODO Set this to the initial startup speed
         targetSpeeds = null;
+
+        rightRelative = rightMotor.getEncoder();
+
+        shooterFFLeft = new SimpleMotorFeedforward(ShooterConstants.kLeftS, ShooterConstants.kLeftV, ShooterConstants.kLeftA);
+        shooterFFRight = new SimpleMotorFeedforward(ShooterConstants.kRightS, ShooterConstants.kRightV, ShooterConstants.kRightA);
     }
 
     @Override
     public void periodic() {
-        Logger.recordOutput(
+        SmartDashboard.putNumber(
-            "Shooter/FrontRollers/TargetMPS", 
+            "ShooterTargetRPM", 
-            targetSpeeds == null ? 0 : targetSpeeds.getFrontRollerMPS()
+            targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM());
+
+        SmartDashboard.putNumber(
+            "ShooterLeftSideRPM", 
+            leftEncoder.getVelocity()
+        );
+
+        SmartDashboard.putNumber(
+            "ShooterRightSideRPM", 
+            rightEncoder.getVelocity()
+        );
+
+        SmartDashboard.putBoolean(
+            "ShooterLeftSideAtSetpoint", 
+            leftClosedLoopController.isAtSetpoint()
+        );
+
+        SmartDashboard.putBoolean(
+            "ShooterRightSideAtSetpoint", 
+            rightClosedLoopController.isAtSetpoint()
         );
 
         Logger.recordOutput(
-            "Shooter/RearRollers/TargetMPS", 
+            "Shooter/TargetRPM", 
-            targetSpeeds == null ? 0 : targetSpeeds.getRearRollerMPS()
+            targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM()
         );
 
-        Logger.recordOutput("Shooter/FrontRollers/CurrentMPS", frontEncoder.getVelocity());
+        Logger.recordOutput("Shooter/LeftRollers/CurrentRPM", leftEncoder.getVelocity());
-        Logger.recordOutput("Shooter/RearRollers/CurrentMPS", rearEncoder.getVelocity());
+        Logger.recordOutput("Shooter/RightRollers/CurrentRPM", rightEncoder.getVelocity());
+
+        Logger.recordOutput("Shooter/RightRollers/rightmotor", rightRelative.getVelocity());
+
+        Logger.recordOutput("Shooter/LeftRollers/OutputVoltage", leftMotor.getAppliedOutput() * leftMotor.getBusVoltage());
+        Logger.recordOutput("Shooter/RightRollers/OutputVoltage", rightMotor.getAppliedOutput() * rightMotor.getBusVoltage());
 
         // TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance?
-        Logger.recordOutput("Shooter/FrontRollers/AtSetpoint", frontClosedLoopController.isAtSetpoint());
+        Logger.recordOutput("Shooter/LeftRollers/AtSetpoint", leftClosedLoopController.isAtSetpoint());
-        Logger.recordOutput("Shooter/RearRollers/AtSetpoint", rearClosedLoopController.isAtSetpoint());
+        Logger.recordOutput("Shooter/RightRollers/AtSetpoint", rightClosedLoopController.isAtSetpoint());
     }
 
     public Command maintainSpeed(ShooterSpeeds speeds) {
-        targetSpeeds = speeds;
-
         return run(() -> {
+            targetSpeeds = speeds;
+
             if(targetSpeeds == null) {
-                frontMotor1.disable();
+                leftMotor.disable();
-                rearMotor1.disable();
+                rightMotor.disable();
             } else {
-                frontClosedLoopController.setSetpoint(
+                leftClosedLoopController.setSetpoint(
-                    targetSpeeds.getFrontRollerMPS(), 
+                    targetSpeeds.getSpeedRPM(), 
-                    ControlType.kVelocity
+                    ControlType.kVelocity,
+                    ClosedLoopSlot.kSlot0,
+                    shooterFFLeft.calculate(targetSpeeds.getSpeedRPM())
                 );
 
-                rearClosedLoopController.setSetpoint(
+                rightClosedLoopController.setSetpoint(
-                    targetSpeeds.getRearRollerMPS(), 
+                    targetSpeeds.getSpeedRPM(), 
-                    ControlType.kVelocity
+                    ControlType.kVelocity,
+                    ClosedLoopSlot.kSlot0,
+                    shooterFFRight.calculate(targetSpeeds.getSpeedRPM())
                 );
             }
         });
     }
 
+    public Command manualSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            targetSpeeds = null;
+            
+            leftMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
+            rightMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
+        });
+    }
+
     public Command stop() {
-        return maintainSpeed(null);
+        return manualSpeed(() -> 0);
+    }
+
+    public double getAverageActualSpeeds() {
+        return (leftEncoder.getVelocity() + rightEncoder.getVelocity()) / 2;
     }
 
     public Optional<ShooterSpeeds> getTargetSpeeds() {

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

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.ctre.phoenix6.controls.DutyCycleOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.revrobotics.PersistMode;
@@ -37,15 +39,35 @@ public class Spindexer extends SubsystemBase {
 
     public Command spinToShooter() {
         return run(() -> {
-            spindexerMotor.setControl(spindexerMotorOutput.withOutput(1));
+            spindexerMotor.setControl(
-            feederMotor.set(1);
+                spindexerMotorOutput.withOutput(SpindexerConstants.kSpindexerSpeed)
+            );
+            feederMotor.set(SpindexerConstants.kFeederSpeed);
+        });
+    }
+
+    public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM) {
+        return run(() -> {
+            if(shooterSpeedRPM.getAsDouble() < cutOffRPM) {
+                spindexerMotor.setControl(
+                    spindexerMotorOutput.withOutput(0)
+                );
+                feederMotor.set(0);
+            } else {
+                spindexerMotor.setControl(
+                    spindexerMotorOutput.withOutput(SpindexerConstants.kSpindexerSpeed)
+                );
+                feederMotor.set(SpindexerConstants.kFeederSpeed);
+            }
         });
     }
 
     public Command spinToIntake() {
         return run(() -> {
-            spindexerMotor.setControl(spindexerMotorOutput.withOutput(-1));
+            spindexerMotor.setControl(
-            feederMotor.set(-1);
+                spindexerMotorOutput.withOutput(-SpindexerConstants.kSpindexerSpeed)
+            );
+            feederMotor.set(-SpindexerConstants.kFeederSpeed);
         });
     }
 
@@ -56,4 +78,11 @@ public class Spindexer extends SubsystemBase {
         });
     }
 
+    public Command instantaneousStop() {
+        return runOnce(() -> {
+            spindexerMotor.setControl(spindexerMotorOutput.withOutput(0));
+            feederMotor.set(0);
+        });
+    }
+    
 }

src/main/java/frc/robot/utilities/SparkMAXTester.java

@@ -8,13 +8,27 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
+/**
+ * A simple subsystem that can be used to test a single SparkMax and associated NEO motor
+ */
 public class SparkMAXTester extends SubsystemBase {
     private SparkMax spark;
 
+    /**
+     * Constructor
+     * 
+     * @param deviceID The CAN ID of the SparkMAX that needs testing
+     */
     public SparkMAXTester(int deviceID) {
         spark = new SparkMax(deviceID, MotorType.kBrushless);
     }
 
+    /**
+     * Sets the speed of the motor
+     * 
+     * @param speed A method or lambda which returns a double between -1 and 1
+     * @return A Command object that runs indefinitely to control motor speed
+     */
     public Command setSpeed(DoubleSupplier speed) {
         return run(() -> {
             spark.set(speed.getAsDouble());

src/main/java/frc/robot/utilities/SwerveModule.java

@@ -17,6 +17,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.AnalogEncoder;
 import frc.robot.constants.ModuleConstants;
+import frc.robot.constants.ModuleConstants.ModuleName;
 
 /*
  * This thread
@@ -30,6 +31,8 @@ import frc.robot.constants.ModuleConstants;
  * the controller closed loop controller. 
  */
 public class SwerveModule {
+    private ModuleName moduleName;
+
     private TalonFX drive;
     private SparkMax turning;
 
@@ -41,8 +44,6 @@ public class SwerveModule {
 
     private VelocityVoltage driveVelocityRequest;
 
-    private String moduleName;
-
     private SwerveModuleState lastTargetState;
     private SwerveModuleState lastTargetStateOptimized;
 
@@ -59,7 +60,7 @@ public class SwerveModule {
      * @param drivingCANID The CAN ID of the Kraken used to drive the module wheel
      * @param turningCANID The CAN ID of the Spark MAX used to turn the module wheel
      */
-    public SwerveModule(String moduleName, int drivingCANID, int turningCANID) {
+    public SwerveModule(ModuleName moduleName, int drivingCANID, int turningCANID) {
         this(moduleName, drivingCANID, turningCANID, -1, -1);
     }
 
@@ -73,7 +74,7 @@ public class SwerveModule {
      * @param analogEncoderID The Analog In port ID for the Thrify Absolute Encoder
      * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module 
      */
-    public SwerveModule(String moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
+    public SwerveModule(ModuleName moduleName, int drivingCANID, int turningCANID, int analogEncoderID, double analogEncoderOffset) {
         this(moduleName, drivingCANID, turningCANID, analogEncoderID, analogEncoderOffset, false);
     }
 
@@ -88,7 +89,7 @@ public class SwerveModule {
      * @param analogEncoderOffset The angular offset for the absolute encoder to achieve 0 position on the module 
      * @param turningEncoderAutoRezeroEnabled Should the turning encoder in the NEO automatically rezero from the absolute encoder
      */
-    public SwerveModule(String moduleName, int drivingCANID, int turningCANID, 
+    public SwerveModule(ModuleName moduleName, int drivingCANID, int turningCANID, 
         int analogEncoderID, double analogEncoderOffset, boolean turningEncoderAutoRezeroEnabled) {
         isAbsoluteEncoderDisabled = (analogEncoderID == -1) || (analogEncoderOffset < 0);
 
@@ -108,6 +109,7 @@ public class SwerveModule {
         drive.getConfigurator().apply(ModuleConstants.kDriveMotorConfig);
         drive.getConfigurator().apply(ModuleConstants.kAudioConfig);
         drive.getConfigurator().apply(ModuleConstants.kDriveSlot0Config);
+        drive.getConfigurator().apply(ModuleConstants.kDriveClosedLoopRampConfig);
 
         turning.configure(
             ModuleConstants.turningConfig,
@@ -130,21 +132,23 @@ public class SwerveModule {
 
         this.turningEncoderAutoRezeroEnabled = turningEncoderAutoRezeroEnabled;
 
-        this.moduleName = "Drivetrain/Modules/" + moduleName;
+        this.moduleName = moduleName;
     }
 
     public void periodic() {
         
         if(!isAbsoluteEncoderDisabled) {
-            Logger.recordOutput(moduleName + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
+            Logger.recordOutput(moduleName.getLoggableName() + "/AbsoluteEncoder/Position", turningAbsoluteEncoder.get());
         }
         
-        Logger.recordOutput(moduleName + "/ModuleTargetState", lastTargetState);
+        Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetState", lastTargetState);
-        Logger.recordOutput(moduleName + "/ModuleTargetStateOptimized", lastTargetStateOptimized);
+        Logger.recordOutput(moduleName.getLoggableName() + "/ModuleTargetStateOptimized", lastTargetStateOptimized);
-        Logger.recordOutput(moduleName + "/SwerveModuleState", getState());
+        Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModuleState", getState());
-        Logger.recordOutput(moduleName + "/SwerveModulePosition", getPosition());
+        Logger.recordOutput(moduleName.getLoggableName() + "/SwerveModulePosition", getPosition());
-        Logger.recordOutput(moduleName + "/RelativeEncoderPosition", getTurningEncoderPosition());
+        Logger.recordOutput(moduleName.getLoggableName() + "/RelativeEncoderPosition", getTurningEncoderPosition());
 
+        // TODO Re-enable this? Was turned off when there was drivetrain issues
+        // Now that there aren't, do we try this again?
         /*
         if(!isAbsoluteEncoderDisabled && turningEncoderAutoRezeroEnabled) {
             if(Math.abs(getState().angle.getRadians() - lastTargetState.angle.getRadians()) <= ModuleConstants.kAutoResetPositionDeadband) {
@@ -153,6 +157,10 @@ public class SwerveModule {
         }*/
     }
 
+    public ModuleName getModuleName() {
+        return moduleName;
+    }
+
     public SwerveModuleState getState() {
         return new SwerveModuleState(
             drive.getVelocity().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters, 
@@ -174,9 +182,7 @@ public class SwerveModule {
 
     public void setDesiredState(SwerveModuleState desiredState) {
         lastTargetState = new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
-        // TODO is this really necessary, the offset is managed by the Absolute Encoder
+        
-        // and its "source of truth" behavior in relation to the relative encoder
-        // Probably doesn't *hurt* that it's here, but it may not be needed
         desiredState.optimize(new Rotation2d(getTurningEncoderPosition()));
 
         lastTargetStateOptimized = desiredState;

src/main/java/frc/robot/utilities/Utilities.java

@@ -1,11 +1,22 @@
 package frc.robot.utilities;
 
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import frc.robot.constants.CompetitionConstants;
 
 public class Utilities {
     public static final double kG = -9.81;
     
+    /**
+     * Returns the Alliance enumeration that indicates who will have the first
+     * shift. Returns null if the data is not available. 
+     * 
+     * @return The Alliance that will have the first shift, or null if game specific data
+     *  is not present
+     */
     public static Alliance whoHasFirstShift() {
         String gameData = DriverStation.getGameSpecificMessage();
 
@@ -23,6 +34,40 @@ public class Utilities {
         return null;
     }
 
+    /**
+     * Returns the pose of the hub for the given alliance assigned to our robot.
+     * If no alliance is assigned (which is unlikely) this method returns
+     * the Blue hub pose, which is the closet to the field origin
+     * 
+     * @return The Pose2d object which represents the appropriate pose of the Hub
+     */
+    public static Pose2d getHubPose() {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+
+        if(alliance.isEmpty() || alliance.get() == Alliance.Blue) {
+            return CompetitionConstants.kBlueHubLocation;
+        } else {
+            return CompetitionConstants.kRedHubLocation;
+        }
+    }
+
+    /**
+     * Returns the AprilTag ID of the tag that is in the center of the hub
+     * for the robot's assigned alliance. If no alliance is assigned (which is unlikely)
+     * the Blue hub's center tag is returned. 
+     * 
+     * @return The tag ID that is in the center of the assigned alliance's hub
+     */
+    public static int getHubCenterAprilTagID() {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+
+        if(alliance.isEmpty() || alliance.get() == Alliance.Blue) {
+            return 26;
+        } else {
+            return 10;
+        }
+    }
+
     /**
      * A ChatGPT possible hallucination related to calcuating whether a shot is possible
      * for a given speed and change in X and Y position
@@ -30,8 +75,6 @@ public class Utilities {
      * Note that X in this scenario is the physical distance from the shooter exit to 
      * target. Y is the change in height from the shooter exit to the target height
      * 
-     * TODO Review ChatGPT's math more thoroughly, preferably with someone with fresher math skills
-     * 
      * @param targetVMPS The target velocity of the shooter in Meters per Second
      * @param deltaXM The "as the crow flies" distance between the shooter exit and the target
      * @param deltaYM The height difference between the shooter exit and the target
@@ -52,8 +95,6 @@ public class Utilities {
      * Setting softerShot to true changes the angle of attack to a soft, long range shot. False
      * makes the shot more of a lob
      * 
-     * TODO Review ChatGPT's math more thoroughly, preferably with someone with fresher math skills
-     * 
      * @param targetVMPS The target velocity of the shooter in Meters per Second
      * @param deltaXM The "as the crow flies" distance between the shooter exit and the target
      * @param deltaYM The height difference between the shooter exit and the target

src/main/java/frc/robot/utilities/VisualPose.java

@@ -0,0 +1,9 @@
+package frc.robot.utilities;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+/**
+ * A record class which represents the source of a visual pose, the pose itself
+ * and the timestamp the pose was generated. 
+ */
+public record VisualPose(String cameraName, Pose2d visualPose, double timestamp) {}