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

@@ -69,7 +69,7 @@
       },
       "prevControl": {
         "x": 8.08578683847011,
-        "y": 0.49077040160283747
+        "y": 0.49077040160283736
       },
       "nextControl": {
         "x": 8.318287488908608,
@@ -130,8 +130,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/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

@@ -2,10 +2,12 @@
   "robotWidth": 0.921,
   "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,

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

@@ -5,19 +5,31 @@
 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;
@@ -42,7 +54,7 @@ import frc.robot.utilities.Elastic;
 import frc.robot.utilities.Utilities;
 
 public class RobotContainer {
-    //private PhotonVision vision;
+    private PhotonVision vision;
     private Drivetrain drivetrain;
     private Hood hood;
     private Shooter shooter;
@@ -58,8 +70,10 @@ public class RobotContainer {
 
     private Timer shiftTimer;
 
+    private boolean resetOdometryToVisualPose;
+
     public RobotContainer() {
-        //vision = new PhotonVision();
+        vision = new PhotonVision();
         drivetrain = new Drivetrain(null);
         hood = new Hood();
         shooter = new Shooter();
@@ -67,29 +81,49 @@ public class RobotContainer {
         intakeRoller = new IntakeRoller();
         spindexer = new Spindexer();
         //climber = new Climber();
+        configureNamedCommands();
 
-        /*
+        resetOdometryToVisualPose = false;
+         
         vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
         vision.addPoseEstimateConsumer((vp) -> {
             Logger.recordOutput(
                 "Vision/" + vp.cameraName() + "/Pose", 
                 vp.visualPose()
             );
-        });*/
+        });
 
+        vision.addPoseEstimateConsumer((vp) -> {
+            if(resetOdometryToVisualPose) {
+                drivetrain.resetOdometry(vp.visualPose());
+                resetOdometryToVisualPose = false;
+            }
+        });
+        
         driver = new CommandXboxController(OIConstants.kDriverControllerPort);
         secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
         shiftTimer = new Timer();
         shiftTimer.reset();
 
-        //configureBindings();
+        configureBindings();
-        testConfigureBindings();
         configureShiftDisplay();
+        //testConfigureBindings();
+        
 
         if(AutoConstants.kAutoConfigOk) {
             autoChooser = AutoBuilder.buildAutoChooser();
-            configureNamedCommands();
+            SmartDashboard.putData("Auto Chooser", autoChooser);
+
+            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)
+            );
         }
     }
 
@@ -108,8 +142,8 @@ public class RobotContainer {
         // 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(() -> 0, () -> 0, () -> 0, () -> true));
-        /*drivetrain.setDefaultCommand(
+        drivetrain.setDefaultCommand(
             drivetrain.drive(
                 driver::getLeftY,
                 driver::getLeftX, 
@@ -118,6 +152,8 @@ public class RobotContainer {
             )
         );
 
+        driver.y().whileTrue(drivetrain.zeroHeading());
+/* 
         // 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
@@ -168,8 +204,7 @@ public class RobotContainer {
         // 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(
-            //intakeRoller.runIn().alongWith(spindexer.spinToShooter())
+            spindexer.spinToShooter()  
-            spindexer.spinToShooter()
         );
 
         // While holding the left bumper of the driver controller, the intake rollers
@@ -178,7 +213,8 @@ public class RobotContainer {
         // 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.runOut().alongWith(spindexer.spinToIntake())
+            intakeRoller.runIn()
+            //intakeRoller.runOut().alongWith(spindexer.spinToIntake())
         );
 
         // While holding D-Pad up on the secondary controller, the shooter should spin
@@ -218,7 +254,7 @@ public class RobotContainer {
 
         // 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.a().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
         secondary.b().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
 
         // Useful for testing PID and FF responses of the intake pivot
@@ -226,6 +262,10 @@ public class RobotContainer {
         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
     }
@@ -239,8 +279,19 @@ public class RobotContainer {
                 () -> true
             )
         );
+        shooter.setDefaultCommand(shooter.stop());
+        intakeRoller.setDefaultCommand(intakeRoller.stop());
+        spindexer.setDefaultCommand(spindexer.stop());
 
-        driver.a().whileTrue(
+        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, 
@@ -248,8 +299,27 @@ public class RobotContainer {
             ) 
         );
 
-        /*
+        driver.leftBumper().whileTrue(intakeRoller.runOut());
-        driver.b().whileTrue(
+        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());
@@ -261,32 +331,60 @@ public class RobotContainer {
             )
         );*/
 
-        shooter.setDefaultCommand(
+        secondary.a().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
-            shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)
+        secondary.x().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
-        );
 
-        hood.setDefaultCommand(hood.trackToAngle(() -> {
+        //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()
-                .plus(CompetitionConstants.kRobotToShooter.getTranslation().toTranslation2d())
                 .getDistance(hubPose.getTranslation());
+
+            Logger.recordOutput("Hood/DistanceToHub", distance);
+
+            Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
                 
-            if(HoodConstants.kUseInterpolatorForAngle) {
+            if(currentSpeeds.isPresent()) {
-                return HoodConstants.kDistanceToAngle.get(distance);
+                InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
+
+                if(map != null) {
+                    return MathUtil.clamp(map.get(distance), 0, 40);
+                } else {
+                    return 0;
+                }
             } else {
-                // TODO The average actual speeds isn't <i>really</i> the exit velocity of the ball
+                return 0;
-                // on a hooded shooter, based on documentation, it's more like 30-50% depending on
-                // hood material, surface friction, etc.
-                return Utilities.shotAngle(
-                    shooter.getAverageActualSpeeds(), 
-                    distance, 
-                    CompetitionConstants.kHubGoalHeightMeters - ShooterConstants.kShooterHeightMeters, 
-                    false
-                );
             }
-        }));
+        }));*/
+
+        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() {
@@ -302,14 +400,67 @@ public class RobotContainer {
                 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() {
-        if(AutoConstants.kAutoConfigOk) {
+        return autoChooser.getSelected();
-            return autoChooser.getSelected();
-        } else {
-            return new PrintCommand("Robot Config loading failed, autonomous disabled");
-        }
     }
 
     /**

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

@@ -24,8 +24,6 @@ public class AutoConstants {
     public static final double kPXYController = 3.5;
     public static final double kPThetaController = 5;
 
-    public static final double kYawPIDTolerance = Units.degreesToRadians(2);
-
     public static final double kAlignPXYController = 2;
     public static final double kAlignPThetaController = 5;
 

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

@@ -38,14 +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.9, 0.9, 0.9);
+    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

@@ -5,28 +5,33 @@ 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 = 12;
 
-    public static final double kConversionFactor = 3.0*147.0/8.0;
+    public static final double kConversionFactor = (1.0/3.0)*(8.0/147.0)*2*Math.PI;
 
-    public static final double kP = 0;
+    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 = 1;
+    public static final double kMaxManualSpeedMultiplier = 0.1;
+    public static final double kTolerance = Math.toRadians(0.5);
 
     public static final double kAmpsToTriggerPositionReset = 10;
 
@@ -36,14 +41,14 @@ public class HoodConstants {
 
     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;
 
-    // TODO This needs to be filled in from some source
+    public static final Map<ShooterSpeeds, InterpolatingDoubleTreeMap> kHoodInterpolators = Map.of(
-    public static final InterpolatingDoubleTreeMap kDistanceToAngle = new InterpolatingDoubleTreeMap();
+        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();
@@ -60,30 +65,28 @@ public class HoodConstants {
             .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
             .pid(kP, kI, kD)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(kTolerance, ClosedLoopSlot.kSlot0)
             .feedForward
                 .sva(kS, kV, kA);
 
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
+            Double.valueOf(Units.inchesToMeters(22.2 + 40)), 
+            Double.valueOf(Units.degreesToRadians(9.5)));
 
-        File interpolatorFile = Path.of(
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
-            Filesystem.getDeployDirectory().getAbsolutePath().toString(), 
+            Double.valueOf(Units.inchesToMeters(22.2 + 60)), 
-            "interpolatorData.csv"
+            Double.valueOf(Units.degreesToRadians(12.5)));
-        ).toFile();
 
-        if(interpolatorFile.exists()) {
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
-            try (BufferedReader reader = new BufferedReader(new FileReader(interpolatorFile))) {
+            Double.valueOf(Units.inchesToMeters(22.2 + 80)),
-                reader.lines().forEach((s) -> {
+            Double.valueOf(Units.degreesToRadians(16.25)));
-                    if(s.trim() != "") { //Empty or whitespace line protection
-                        String[] lineSplit = s.split(",");
 
-                        kDistanceToAngle.put(
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
-                            Double.valueOf(lineSplit[0].replace("\"", "")), 
+            Double.valueOf(Units.inchesToMeters(22.2 + 100)), 
-                            Double.valueOf(lineSplit[1].replace("\"", ""))
+            Double.valueOf(Units.degreesToRadians(20.5)));
-                        );
+
-                    }
+        kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
-                });
+            Double.valueOf(Units.inchesToMeters(22.2 + 120)), 
-            } catch (IOException e) {
+            Double.valueOf(Units.degreesToRadians(23.5)));
-                // This condition is never reached because of the if exists line above
-            }
-        }
     }
 }

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

@@ -51,7 +51,7 @@ public class IntakePivotConstants {
         KLeftMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors);
+            .inverted(false);
         KLeftMotorConfig.encoder
             .positionConversionFactor(kConversionFactor)
             .velocityConversionFactor(kConversionFactor / 60);
@@ -67,7 +67,7 @@ public class IntakePivotConstants {
         kRightMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors)
+            .inverted(true)
-            .follow(kLeftMotorCANID);
+            ;//.follow(kLeftMotorCANID);
     }
 }

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

@@ -5,24 +5,32 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 public class IntakeRollerConstants {
     // TODO Real values
-    public static final int kMotorCANID = 20;
+    public static final int kRightMotorCANID = 20;
+    public static final int kLeftMotorCANID = 1;
 
-    public static final int kCurrentLimit = 40;
+    public static final int kCurrentLimit = 65;
 
-    public static final boolean kInvertMotors = true;
+    public static final boolean kInvertLeftMotor = false;
+    public static final boolean kInvertRightMotor = true;
 
-    public static final double kSpeed = .6;
+    public static final double kSpeed = 1;
 
     public static final IdleMode kIdleMode = IdleMode.kCoast;
 
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
 
     public static final SparkMaxConfig leftMotorConfig = new SparkMaxConfig();
+    public static final SparkMaxConfig rightMotorConfig = new SparkMaxConfig();
 
     static {
         leftMotorConfig
             .idleMode(kIdleMode)
             .smartCurrentLimit(kCurrentLimit)
-            .inverted(kInvertMotors);
+            .inverted(kInvertLeftMotor);
+        rightMotorConfig
+            .idleMode(kIdleMode)
+            .smartCurrentLimit(kCurrentLimit)
+            .inverted(kInvertRightMotor)
+        ;
     }
 }

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;
@@ -33,7 +35,7 @@ public class ModuleConstants {
     }
 
     // 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);
@@ -44,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;
@@ -63,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?
@@ -85,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;
@@ -106,6 +112,8 @@ public class ModuleConstants {
         kDriveSlot0Config.kV = kDriveV;
         kDriveSlot0Config.kA = kDriveA;
 
+        kDriveClosedLoopRampConfig.withVoltageClosedLoopRampPeriod(kClosedLoopRampRate);
+
         turningConfig
             .idleMode(kTurnIdleMode)
             .smartCurrentLimit(kTurnMotorCurrentLimit)
@@ -118,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,24 +1,34 @@
 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),
+        kHubSpeed(3000.0),
-        kFeedSpeed(0.35);
+        kFeedSpeed(5000.0),
+        kIdleSpeed(750.0);
 
         private double speedMPS;
+        private double speedRPM;
 
-        private ShooterSpeeds(double speedMPS) {
+        private ShooterSpeeds(double speedRPM) {
-            this.speedMPS = speedMPS;
+            this.speedMPS = speedRPM * kWheelDiameter*Math.PI;
+            this.speedRPM = speedRPM;
         }
 
         public double getSpeedMPS() {
-            return speedMPS;
+            return speedMPS * kWheelDiameter*Math.PI;
+        }
+
+        public double getSpeedRPM(){
+            return speedRPM;
         }
     }
 
@@ -33,18 +43,18 @@ public class ShooterConstants {
     public static final boolean kLeftShooterMotorInverted = true;
     public static final boolean kRightShooterMotorInverted = false;
 
-    public static final double kLeftP = 0.1;
+    public static final double kLeftP = 0.75;//0.01;//0.001;
     public static final double kLeftI = 0;
-    public static final double kLeftD = 0;
+    public static final double kLeftD = 0;//0.1;//1.8;
     public static final double kLeftS = 0;
-    public static final double kLeftV = 0.21;
+    public static final double kLeftV = 0.00129;
     public static final double kLeftA = 0;
 
-    public static final double kRightP = 0.1;
+    public static final double kRightP = 0.75;//0.001;//0.001;
     public static final double kRightI = 0;
-    public static final double kRightD = 0;
+    public static final double kRightD = 0;//0.1;
     public static final double kRightS = 0;
-    public static final double kRightV = 0.21;
+    public static final double kRightV = 0.00125;
     public static final double kRightA = 0;
 
     public static final double kMaxManualSpeedMultiplier = 1;
@@ -52,7 +62,7 @@ public class ShooterConstants {
     public static final double kShooterHeightMeters = 0;
 
     // TODO Is this value sane?
-    public static final int kCurrentLimit = 50;
+    public static final int kCurrentLimit = 60;
 
     public static final IdleMode kShooterIdleMode = IdleMode.kCoast;
 
@@ -68,27 +78,32 @@ public class ShooterConstants {
             .smartCurrentLimit(kCurrentLimit)
             .inverted(kLeftShooterMotorInverted);
         kLeftMotorConfig.absoluteEncoder
-            .positionConversionFactor(kWheelDiameter * Math.PI)
+            .positionConversionFactor(1)
-            .velocityConversionFactor(kWheelDiameter * Math.PI / 60);
+            .velocityConversionFactor(60)
+            .averageDepth(8); // VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
         kLeftMotorConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-            .pid(kLeftP, kLeftI, kLeftD)
+            .pid(kLeftP, kLeftI, kLeftD, ClosedLoopSlot.kSlot0)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
             .feedForward
-                .sva(kLeftS, kLeftV, kLeftA);
+                .sva(kLeftS, kLeftV, kLeftA, ClosedLoopSlot.kSlot0);
 
         kRightMotorConfig
             .idleMode(kShooterIdleMode)
             .smartCurrentLimit(kCurrentLimit)
             .inverted(kRightShooterMotorInverted);
         kRightMotorConfig.absoluteEncoder
-            .positionConversionFactor(kWheelDiameter * Math.PI)
+            .positionConversionFactor(1)
-            .velocityConversionFactor(kWheelDiameter * Math.PI / 60);
+            .velocityConversionFactor(60)
+            .averageDepth(8)// VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
+            .inverted(true);
         kRightMotorConfig.closedLoop
             .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
             .pid(kRightP, kRightI, kRightD)
             .outputRange(-1, 1)
+            .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
             .feedForward
-                .sva(kRightS, kRightV, kRightA);
+                .sva(kRightS, kRightV, kRightA, ClosedLoopSlot.kSlot0);
     }
 }

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

@@ -12,14 +12,14 @@ public class SpindexerConstants {
     public static final int kSpindexerMotorCANID = 0;
     public static final int kFeederMotorCANID = 4;
 
-    public static final int kSpindexerStatorCurrentLimit = 110;
+    public static final int kSpindexerStatorCurrentLimit = 95;
-    public static final int kSpindexerSupplyCurrentLimit = 60;
+    public static final int kSpindexerSupplyCurrentLimit = 50;
-    public static final int kFeederCurrentLimit = 40;
+    public static final int kFeederCurrentLimit = 30;
 
     public static final double kSpindexerSpeed = 1;
     public static final double kFeederSpeed = 1;
 
-    public static final boolean kFeederMotorInverted = true;
+    public static final boolean kFeederMotorInverted = false;
 
     public static final InvertedValue kSpindexerInversionState = InvertedValue.Clockwise_Positive;
     public static final NeutralModeValue kSpindexerIdleMode = NeutralModeValue.Coast;

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

@@ -50,9 +50,9 @@ public class Climber extends SubsystemBase {
     }
 
     public Command maintainPosition(ClimberPositions position) {
-        targetPosition = position;
-
         return run(() -> {
+            targetPosition = position;
+
             controller.setSetpoint(
                 position.getPositionMeters(), 
                 ControlType.kPosition
@@ -61,9 +61,9 @@ public class Climber extends SubsystemBase {
     }
 
     public Command manualSpeed(DoubleSupplier speed) {
-        targetPosition = null;
-
         return run(() -> {
+            targetPosition = null;
+
             motor.set(speed.getAsDouble());
         });
     }

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

@@ -82,12 +82,12 @@ public class Drivetrain extends SubsystemBase {
         gyro = new AHRS(NavXComType.kMXP_SPI);
 
         yawRotationController = new PIDController(
-            AutoConstants.kPThetaController, 
+            DrivetrainConstants.kHeadingP, 
             0, 
             0
         );
         yawRotationController.enableContinuousInput(-Math.PI, Math.PI);
-        yawRotationController.setTolerance(AutoConstants.kYawPIDTolerance);
+        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(
@@ -144,6 +144,7 @@ public class Drivetrain extends SubsystemBase {
         Logger.recordOutput("Drivetrain/Pose", getPose());
         Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue());
         Logger.recordOutput("Drivetrain/Heading", getHeadingDegrees());
+        Logger.recordOutput("Drivetrain/Velocity", getCurrentChassisSpeeds());
     }
 
     /**
@@ -261,10 +262,17 @@ public class Drivetrain extends SubsystemBase {
                         targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
                     }
 
-                    return yawRotationController.calculate(
+                    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;
                 }, 
                 () -> true
             )
@@ -323,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()
@@ -368,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)   
         );
 

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

@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import java.util.Optional;
 import java.util.function.DoubleSupplier;
 
 import org.littletonrobotics.junction.Logger;
@@ -12,12 +13,18 @@ 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;
@@ -26,10 +33,10 @@ public class Hood extends SubsystemBase {
 
     private SparkClosedLoopController controller;
 
-    private Trigger resetTrigger;
+    //private Trigger resetTrigger;
-    private Trigger timerTrigger;
+    //private Trigger timerTrigger;
 
-    private Timer resetTimer;
+    //private Timer resetTimer;
 
     private double currentTargetDegrees;
 
@@ -43,10 +50,11 @@ public class Hood extends SubsystemBase {
         );   
 
         encoder = motor.getEncoder();
+        encoder.setPosition(HoodConstants.kStartupAngle);
 
         controller = motor.getClosedLoopController();
 
-        resetTimer = new Timer();
+        /*resetTimer = new Timer();
         resetTimer.reset();
 
         resetTrigger = new Trigger(() -> (motor.getOutputCurrent() > HoodConstants.kAmpsToTriggerPositionReset));
@@ -60,17 +68,59 @@ public class Hood extends SubsystemBase {
         timerTrigger.onTrue(new InstantCommand(() -> {
             encoder.setPosition(0);
             resetTimer.reset();
-        }));
+        }));*/
 
         currentTargetDegrees = HoodConstants.kStartupAngle;
     }
 
     @Override
     public void periodic() {
+        SmartDashboard.putNumber(
+            "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", currentTargetDegrees);
+        Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees));
-        Logger.recordOutput("Hood/CurrentAngle", encoder.getPosition());
+        Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition()));
         Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
+        Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage());
+    }
+
+    public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) {
+        return 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;
+            }
+        });
     }
 
     public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {
@@ -94,7 +144,7 @@ public class Hood extends SubsystemBase {
      * 
      * @return A complete Command structure that performs the specified action
      */
-    public Command automatedRezero() {
+    /*public Command automatedRezero() {
         return manualSpeed(() -> -1)
             .until(timerTrigger)
             .andThen(
@@ -110,11 +160,11 @@ public class Hood extends SubsystemBase {
      * 
      * @return A complete Command structure that performs the specified action
      */
-    public Command automatedRezeroNoTimer() {
+    /*public Command automatedRezeroNoTimer() {
         return manualSpeed(() -> -1)
             .until(() -> motor.getOutputCurrent() >= HoodConstants.kAmpsToTriggerPositionReset)
             .andThen(new InstantCommand(() -> encoder.setPosition(0)));
-    }
+    }*/
 
     public Command manualSpeed(DoubleSupplier speed) {
         currentTargetDegrees = 0;

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

@@ -14,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;
@@ -60,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 {
@@ -72,13 +73,40 @@ public class IntakePivot extends SubsystemBase {
     }
 
     public Command manualSpeed(DoubleSupplier speed) {
-        currentTargetPosition = null;
-
         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 manualSpeed(() -> 0);
     }

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

@@ -10,33 +10,44 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.IntakeRollerConstants;
 
 public class IntakeRoller extends SubsystemBase {
-    private SparkMax motor;
+    private SparkMax leftMotor;
+    private SparkMax rightMotor;
 
     public IntakeRoller() {
-        motor = new SparkMax(IntakeRollerConstants.kMotorCANID, MotorType.kBrushless);
+        leftMotor = new SparkMax(IntakeRollerConstants.kLeftMotorCANID, MotorType.kBrushless);
+        rightMotor = new SparkMax(IntakeRollerConstants.kRightMotorCANID, MotorType.kBrushless);
 
-        motor.configure(
+        leftMotor.configure(
             IntakeRollerConstants.leftMotorConfig, 
             ResetMode.kResetSafeParameters, 
             PersistMode.kPersistParameters
         );
+
+        rightMotor.configure(
+            IntakeRollerConstants.rightMotorConfig,
+            ResetMode.kResetSafeParameters,
+            PersistMode.kPersistParameters
+        );
     }
 
     public Command runIn() {
         return run(() -> {
-            motor.set(IntakeRollerConstants.kSpeed);
+            leftMotor.set(IntakeRollerConstants.kSpeed*0.9);
+            rightMotor.set(IntakeRollerConstants.kSpeed*0.9);
         });
     }
 
     public Command runOut() {
         return run(() -> {
-            motor.set(-IntakeRollerConstants.kSpeed);
+            leftMotor.set(-IntakeRollerConstants.kSpeed);
+            rightMotor.set(-IntakeRollerConstants.kSpeed);
         });
     }
 
     public Command stop() {
         return run(() -> {
-            motor.set(0);
+            leftMotor.set(0);
+            rightMotor.set(0);
         });
     }
     

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

@@ -13,6 +13,7 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 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.wpilibj2.command.SubsystemBase;

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

@@ -7,12 +7,16 @@ 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;
@@ -25,10 +29,14 @@ public class Shooter extends SubsystemBase {
     private AbsoluteEncoder leftEncoder;
     private AbsoluteEncoder rightEncoder;
 
+    private RelativeEncoder rightRelative;
+
     private SparkClosedLoopController leftClosedLoopController;
     private SparkClosedLoopController rightClosedLoopController;
 
     private ShooterSpeeds targetSpeeds;
+    private SimpleMotorFeedforward shooterFFLeft;
+    private SimpleMotorFeedforward shooterFFRight;
 
     public Shooter() {
         leftMotor = new SparkMax(ShooterConstants.kLeftShooterMotorCANID, MotorType.kBrushless);
@@ -54,17 +62,51 @@ public class Shooter extends SubsystemBase {
 
         // 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/TargetMPS", 
+            "ShooterTargetRPM", 
-            targetSpeeds == null ? 0 : targetSpeeds.getSpeedMPS()
+            targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM());
+
+        SmartDashboard.putNumber(
+            "ShooterLeftSideRPM", 
+            leftEncoder.getVelocity()
         );
 
-        Logger.recordOutput("Shooter/LeftRollers/CurrentMPS", leftEncoder.getVelocity());
+        SmartDashboard.putNumber(
-        Logger.recordOutput("Shooter/RightRollers/CurrentMPS", rightEncoder.getVelocity());
+            "ShooterRightSideRPM", 
+            rightEncoder.getVelocity()
+        );
+
+        SmartDashboard.putBoolean(
+            "ShooterLeftSideAtSetpoint", 
+            leftClosedLoopController.isAtSetpoint()
+        );
+
+        SmartDashboard.putBoolean(
+            "ShooterRightSideAtSetpoint", 
+            rightClosedLoopController.isAtSetpoint()
+        );
+
+        Logger.recordOutput(
+            "Shooter/TargetRPM", 
+            targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM()
+        );
+
+        Logger.recordOutput("Shooter/LeftRollers/CurrentRPM", leftEncoder.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/LeftRollers/AtSetpoint", leftClosedLoopController.isAtSetpoint());
@@ -72,30 +114,34 @@ public class Shooter extends SubsystemBase {
     }
 
     public Command maintainSpeed(ShooterSpeeds speeds) {
-        targetSpeeds = speeds;
-
         return run(() -> {
+            targetSpeeds = speeds;
+
             if(targetSpeeds == null) {
                 leftMotor.disable();
                 rightMotor.disable();
             } else {
                 leftClosedLoopController.setSetpoint(
-                    targetSpeeds.getSpeedMPS(), 
+                    targetSpeeds.getSpeedRPM(), 
-                    ControlType.kVelocity
+                    ControlType.kVelocity,
+                    ClosedLoopSlot.kSlot0,
+                    shooterFFLeft.calculate(targetSpeeds.getSpeedRPM())
                 );
 
                 rightClosedLoopController.setSetpoint(
-                    -targetSpeeds.getSpeedMPS(), 
+                    targetSpeeds.getSpeedRPM(), 
-                    ControlType.kVelocity
+                    ControlType.kVelocity,
+                    ClosedLoopSlot.kSlot0,
+                    shooterFFRight.calculate(targetSpeeds.getSpeedRPM())
                 );
             }
         });
     }
 
     public Command manualSpeed(DoubleSupplier speed) {
-        targetSpeeds = null;
-
         return run(() -> {
+            targetSpeeds = null;
+            
             leftMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
             rightMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
         });

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;
@@ -44,6 +46,22 @@ public class Spindexer extends SubsystemBase {
         });
     }
 
+    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(
@@ -59,5 +77,12 @@ public class Spindexer extends SubsystemBase {
             feederMotor.set(0);
         });
     }
+
+    public Command instantaneousStop() {
+        return runOnce(() -> {
+            spindexerMotor.setControl(spindexerMotorOutput.withOutput(0));
+            feederMotor.set(0);
+        });
+    }
     
 }

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

@@ -109,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,