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/Left Shoot Outpost.auto

@@ -11,39 +11,41 @@
           }
         },
         {
-          "type": "named",
+          "type": "parallel",
           "data": {
-            "name": "spinup"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Start to Outpost"
+                }
+              }
+            ]
           }
         },
         {
-          "type": "path",
+          "type": "wait",
           "data": {
-            "pathName": "start to score left"
+            "waitTime": 2.0
           }
         },
         {
-          "type": "named",
+          "type": "parallel",
           "data": {
-            "name": "shoot close"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Left to Outpost"
-          }
-        },
-        {
-          "type": "named",
-          "data": {
-            "name": "stop spindexer"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "trough to shot"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "trough to shot"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "spinup"
+                }
+              }
+            ]
           }
         },
         {
@@ -55,7 +57,13 @@
         {
           "type": "named",
           "data": {
-            "name": "shoot N jimmy"
+            "name": "aim"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "auto shoot"
           }
         }
       ]

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

@@ -5,15 +5,22 @@
     "data": {
       "commands": [
         {
-          "type": "named",
+          "type": "parallel",
           "data": {
-            "name": "intake down"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "left start to center"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "left start to center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake down"
+                }
+              }
+            ]
           }
         },
         {
@@ -54,6 +61,12 @@
             ]
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "aim"
+          }
+        },
         {
           "type": "named",
           "data": {

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

@@ -5,15 +5,22 @@
     "data": {
       "commands": [
         {
-          "type": "named",
+          "type": "parallel",
           "data": {
-            "name": "intake down"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "left start to center"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "left start to center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "intake down"
+                }
+              }
+            ]
           }
         },
         {
@@ -54,6 +61,12 @@
             ]
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "aim"
+          }
+        },
         {
           "type": "named",
           "data": {

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

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.2515736040609142,
-        "y": 4.914375634517767
+        "x": 1.925604060913706,
+        "y": 5.503695431472081
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.4338749089244973,
-        "y": 5.333984175443034
+        "x": 1.107905365777289,
+        "y": 5.923303972397348
       },
       "isLocked": false,
       "linkedName": "left close"
     },
     {
       "anchor": {
-        "x": 0.77458883248731,
-        "y": 5.927269035532995
+        "x": 0.32339086294416286,
+        "y": 5.9825177664974625
       },
       "prevControl": {
-        "x": 2.3777086426889733,
-        "y": 6.110175322602984
+        "x": 1.9265106731458264,
+        "y": 6.165424053567452
       },
       "nextControl": null,
       "isLocked": false,

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

@@ -69,7 +69,7 @@
       },
       "prevControl": {
         "x": 8.08578683847011,
-        "y": 0.4907704016028374
+        "y": 0.49077040160283736
       },
       "nextControl": {
         "x": 8.318287488908608,

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

@@ -3,35 +3,35 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.6619856887266913,
-        "y": 2.2583184257605784
+        "x": 3.5922741116751276,
+        "y": 6.3692588832487305
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.4937567084046877,
-        "y": 1.3172450805011864
+        "x": 2.956913705583757,
+        "y": 6.19430456852792
       },
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "start left"
     },
     {
       "anchor": {
-        "x": 0.7089624329216013,
-        "y": 0.668228980317108
+        "x": 0.32339086294416286,
+        "y": 5.9825177664974625
       },
       "prevControl": {
-        "x": 1.6987119856944104,
-        "y": 1.0414132379199703
+        "x": 1.3270761421319812,
+        "y": 5.964101522842641
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "Outpost"
+      "linkedName": "trough"
     }
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 0.7234468937875753,
-      "rotationDegrees": 180.0
+      "waypointRelativePos": 0.6910862504511138,
+      "rotationDegrees": -179.02889973265033
     }
   ],
   "constraintZones": [],
@@ -39,14 +39,14 @@
   "eventMarkers": [
     {
       "name": "Intake Start",
-      "waypointRelativePos": 0.4500000000000002,
+      "waypointRelativePos": 0,
       "endWaypointRelativePos": null,
       "command": null
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.5,
+    "maxVelocity": 0.75,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -54,13 +54,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": 178.80651057601818
   },
   "reversed": false,
-  "folder": "Right Outpost",
+  "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": -90.0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.428375634517765,
-        "y": 5.715482233502538
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.507563451776648,
-        "y": 5.7523147208121825
+        "x": 3.68848717948718,
+        "y": 5.639692307692307
       },
       "isLocked": false,
-      "linkedName": "over bump"
+      "linkedName": "after center grab"
     },
     {
       "anchor": {
-        "x": 3.002954314720812,
-        "y": 5.310324873096447
+        "x": 2.979166666666668,
+        "y": 5.2327051282051285
       },
       "prevControl": {
-        "x": 4.052680203045686,
-        "y": 5.9825177664974625
+        "x": 3.0721923076923088,
+        "y": 5.721089743589744
       },
       "nextControl": null,
       "isLocked": false,
@@ -30,12 +30,8 @@
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 0.10454545454545605,
-      "rotationDegrees": -115.0
-    },
-    {
-      "waypointRelativePos": 0.7931818181818296,
-      "rotationDegrees": -115.0
+      "waypointRelativePos": 0.11569148936170148,
+      "rotationDegrees": -34.71279786419313
     }
   ],
   "constraintZones": [],
@@ -43,7 +39,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 2.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -56,8 +52,8 @@
   "reversed": false,
   "folder": null,
   "idealStartingState": {
-    "velocity": 0,
-    "rotation": -129.95754893082906
+    "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

@@ -3,57 +3,41 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.573857868020305,
+        "x": 3.5922741116751276,
         "y": 6.3692588832487305
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.9108730964467004,
-        "y": 6.498172588832488
+        "x": 3.085827411167513,
+        "y": 6.277177664974619
       },
       "isLocked": false,
       "linkedName": "start left"
     },
-    {
-      "anchor": {
-        "x": 2.5885888324873094,
-        "y": 5.936477157360406
-      },
-      "prevControl": {
-        "x": 2.3675939086294413,
-        "y": 6.28638578680203
-      },
-      "nextControl": {
-        "x": 2.8493791599284664,
-        "y": 5.523559138911907
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
     {
       "anchor": {
         "x": 4.577543147208122,
         "y": 5.715482233502538
       },
       "prevControl": {
-        "x": 3.565840773084476,
-        "y": 5.768729726877464
+        "x": 3.2884060913705584,
+        "y": 5.697065989847716
       },
       "nextControl": {
-        "x": 5.102406091370558,
-        "y": 5.687857868020306
+        "x": 5.120844928223563,
+        "y": 5.723243687517044
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.428375634517765,
-        "y": 5.715482233502538
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
       },
       "prevControl": {
-        "x": 5.903512690355329,
-        "y": 5.807563451776649
+        "x": 5.931137055837564,
+        "y": 5.65102538071066
       },
       "nextControl": null,
       "isLocked": false,
@@ -62,7 +46,7 @@
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 1.62272727272727,
+      "waypointRelativePos": 0.8,
       "rotationDegrees": -135.0
     }
   ],
@@ -70,15 +54,15 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 2.0,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 1.0,
+    "velocity": 2.0,
     "rotation": -129.95754893082906
   },
   "reversed": false,

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

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.428375634517765,
-        "y": 5.715482233502538
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.897989847715735,
-        "y": 6.406091370558377
+        "x": 6.511248730964468,
+        "y": 7.059868020304569
       },
       "isLocked": false,
       "linkedName": "over bump"
@@ -48,16 +48,16 @@
     },
     {
       "anchor": {
-        "x": 6.428375634517765,
-        "y": 5.715482233502538
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
       },
       "prevControl": {
-        "x": 7.375431472081217,
-        "y": 5.183713197969544
+        "x": 7.758358974358975,
+        "y": 5.616435897435898
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "over bump"
+      "linkedName": "after center grab"
     }
   ],
   "rotationTargets": [
@@ -74,7 +74,7 @@
     {
       "name": "Constraints Zone",
       "minWaypointRelativePos": 1.1243680485338854,
-      "maxWaypointRelativePos": 2.0,
+      "maxWaypointRelativePos": 2.077102803738317,
       "constraints": {
         "maxVelocity": 1.0,
         "maxAcceleration": 1.5,
@@ -95,16 +95,16 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.5,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 2.0,
-    "rotation": -129.95754893082906
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
   },
   "reversed": false,
   "folder": null,
@@ -112,5 +112,5 @@
     "velocity": 0,
     "rotation": -129.95754893082906
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.428375634517765,
-        "y": 5.715482233502538
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.897989847715735,
-        "y": 6.406091370558377
+        "x": 6.511248730964468,
+        "y": 7.059868020304569
       },
       "isLocked": false,
       "linkedName": "over bump"
     },
     {
       "anchor": {
-        "x": 7.017695431472081,
-        "y": 7.216406091370559
+        "x": 7.220274111675127,
+        "y": 7.299279187817259
       },
       "prevControl": {
-        "x": 6.017695431472081,
-        "y": 7.216406091370559
+        "x": 6.220274111675127,
+        "y": 7.299279187817259
       },
       "nextControl": {
-        "x": 8.017695431472081,
-        "y": 7.216406091370559
+        "x": 8.220274111675131,
+        "y": 7.299279187817259
       },
       "isLocked": false,
       "linkedName": null
@@ -36,28 +36,28 @@
         "y": 4.831502538071066
       },
       "prevControl": {
-        "x": 7.941269035532995,
-        "y": 6.042370558375635
+        "x": 7.952417879560978,
+        "y": 6.041784973535379
       },
       "nextControl": {
-        "x": 7.825248730964466,
-        "y": 3.620634517766498
+        "x": 7.846426395939085,
+        "y": 4.186934010152284
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 6.428375634517765,
-        "y": 5.715482233502538
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
       },
       "prevControl": {
-        "x": 7.375431472081217,
-        "y": 5.183713197969544
+        "x": 7.598549873246986,
+        "y": 5.589703440094898
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "over bump"
+      "linkedName": "after center grab"
     }
   ],
   "rotationTargets": [
@@ -73,7 +73,7 @@
   "constraintZones": [
     {
       "name": "Constraints Zone",
-      "minWaypointRelativePos": 1.1243680485338854,
+      "minWaypointRelativePos": 1.3403967538322836,
       "maxWaypointRelativePos": 2.0,
       "constraints": {
         "maxVelocity": 1.0,
@@ -95,22 +95,22 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.5,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
-    "rotation": -129.95754893082906
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
-    "velocity": 0,
+    "velocity": 2.0,
     "rotation": -129.95754893082906
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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

@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.573857868020305,
+        "x": 3.5922741116751276,
         "y": 6.3692588832487305
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.7991959463121194,
+        "x": 2.817612189966942,
         "y": 6.260219737341258
       },
       "isLocked": false,
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 3.2515736040609142,
-        "y": 4.914375634517767
+        "x": 1.925604060913706,
+        "y": 5.503695431472081
       },
       "prevControl": {
-        "x": 3.44523908448796,
-        "y": 5.430816915656558
+        "x": 2.119269541340752,
+        "y": 6.020136712610872
       },
       "nextControl": null,
       "isLocked": false,

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

@@ -3,38 +3,43 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 0.77458883248731,
-        "y": 5.927269035532995
+        "x": 0.32339086294416286,
+        "y": 5.9825177664974625
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.7745888324873098,
-        "y": 5.927269035532995
+        "x": 0.6364670050761427,
+        "y": 6.03776649746193
       },
       "isLocked": false,
       "linkedName": "trough"
     },
     {
       "anchor": {
-        "x": 3.2515736040609142,
-        "y": 4.914375634517767
+        "x": 1.925604060913706,
+        "y": 5.503695431472081
       },
       "prevControl": {
-        "x": 2.6254213197969554,
-        "y": 5.420822335025381
+        "x": 1.409949238578681,
+        "y": 5.844395939086294
       },
       "nextControl": null,
       "isLocked": false,
       "linkedName": "left close"
     }
   ],
-  "rotationTargets": [],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.7098520389751093,
+      "rotationDegrees": 176.84552599629856
+    }
+  ],
   "constraintZones": [],
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.5,
+    "maxVelocity": 4.0,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -50,5 +55,5 @@
     "velocity": 0,
     "rotation": 178.80651057601818
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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

@@ -285,6 +285,8 @@ public class RobotContainer {
 
         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());
@@ -301,11 +303,16 @@ public class RobotContainer {
         driver.rightBumper().whileTrue(intakeRoller.runIn());
 
         driver.rightTrigger().whileTrue(
-            spindexer.spinToShooter().alongWith(
-                intakeRoller.runIn(),
-                intakePivot.jimmy(.5)
+            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)
@@ -435,8 +442,21 @@ public class RobotContainer {
                 
             ).withTimeout(3).andThen(spindexer.instantaneousStop()));
 
-        NamedCommands.registerCommand("auto shoot", hood.trackToAnglePoseBased(drivetrain, shooter)
-            .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), spindexer.spinToShooter(), intakePivot.jimmy(0.5)));
+        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() {

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

@@ -56,7 +56,7 @@ public class ModuleConstants {
 
     // TODO Hold over from 2025, adjust?
     public static final int kDriveMotorStatorCurrentLimit = 90;
-    public static final int kDriveMotorSupplyCurrentLimit = 55;
+    public static final int kDriveMotorSupplyCurrentLimit = 40;
 
     // TODO Hold over from 2025, adjust?
     public static final InvertedValue kDriveInversionState = InvertedValue.Clockwise_Positive;

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

@@ -32,8 +32,8 @@ public class IntakeRoller extends SubsystemBase {
 
     public Command runIn() {
         return run(() -> {
-            leftMotor.set(IntakeRollerConstants.kSpeed*0.8);
-            rightMotor.set(IntakeRollerConstants.kSpeed*0.8);
+            leftMotor.set(IntakeRollerConstants.kSpeed*0.9);
+            rightMotor.set(IntakeRollerConstants.kSpeed*0.9);
         });
     }
 

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(