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

@@ -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/left to center.auto

@@ -4,28 +4,67 @@
     "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": "intake down"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "left start to center"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "over bump to pile"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "back from center"
+            "name": "aim"
           }
         },
         {

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.050842639593909,
-        "y": 5.715482233502538
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.130030456852792,
-        "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,20 +30,16 @@
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 0.10454545454545605,
-      "rotationDegrees": -115.0
-    },
-    {
-      "waypointRelativePos": 0.7931818181818296,
-      "rotationDegrees": -115.0
+      "waypointRelativePos": 0.11569148936170148,
+      "rotationDegrees": -34.71279786419313
     }
   ],
   "constraintZones": [],
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.0,
-    "maxAcceleration": 1.5,
+    "maxVelocity": 4.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": -123.34070734647689
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
   },
-  "useDefaultConstraints": true
+  "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.050842639593909,
-        "y": 5.715482233502538
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
       },
       "prevControl": {
-        "x": 5.525979695431473,
-        "y": 5.807563451776649
+        "x": 5.931137055837564,
+        "y": 5.65102538071066
       },
       "nextControl": null,
       "isLocked": false,
@@ -62,31 +46,24 @@
   ],
   "rotationTargets": [
     {
-      "waypointRelativePos": 1.62272727272727,
+      "waypointRelativePos": 0.8,
       "rotationDegrees": -135.0
     }
   ],
   "constraintZones": [],
   "pointTowardsZones": [],
-  "eventMarkers": [
-    {
-      "name": "Intake Start",
-      "waypointRelativePos": 2.1,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
+  "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": 0,
-    "rotation": -123.34070734647689
+    "velocity": 2.0,
+    "rotation": -129.95754893082906
   },
   "reversed": false,
   "folder": null,

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

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.050842639593909,
-        "y": 5.715482233502538
+        "x": 6.041634517766498,
+        "y": 6.3692588832487305
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.520456852791878,
-        "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,40 +36,44 @@
         "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.050842639593909,
-        "y": 5.715482233502538
+        "x": 6.742194543297748,
+        "y": 5.589703440094898
       },
       "prevControl": {
-        "x": 6.99789847715736,
-        "y": 5.183713197969544
+        "x": 7.598549873246986,
+        "y": 5.589703440094898
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "after center grab"
     }
   ],
   "rotationTargets": [
     {
       "waypointRelativePos": 1.0045454545454569,
       "rotationDegrees": -115.0
+    },
+    {
+      "waypointRelativePos": 2.348863636363685,
+      "rotationDegrees": -115.0
     }
   ],
   "constraintZones": [
     {
       "name": "Constraints Zone",
-      "minWaypointRelativePos": 1.1243680485338854,
+      "minWaypointRelativePos": 1.3403967538322836,
       "maxWaypointRelativePos": 2.0,
       "constraints": {
         "maxVelocity": 1.0,
@@ -91,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": -120.34324888423971
+    "velocity": 4.0,
+    "rotation": -45.365518355574764
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
-    "velocity": 0,
-    "rotation": -123.34070734647689
+    "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

@@ -14,6 +14,7 @@ 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;
 
@@ -113,6 +114,16 @@ public class RobotContainer {
         if(AutoConstants.kAutoConfigOk) {
             autoChooser = AutoBuilder.buildAutoChooser();
             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)
+            );
         }
     }
 
@@ -274,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());
@@ -290,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)
@@ -391,7 +409,7 @@ public class RobotContainer {
 
         NamedCommands.registerCommand("spinup", 
             shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)
-                .withTimeout(3));
+                .withTimeout(2));
 
         NamedCommands.registerCommand("shoot close", 
             spindexer.spinToShooter()
@@ -405,6 +423,10 @@ public class RobotContainer {
             .onTrue(
                 intakeRoller.runIn());
 
+        new EventTrigger("windup trigger")
+            .onTrue(
+                shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
+
         NamedCommands.registerCommand("stop spindexer", spindexer.instantaneousStop());
 
         NamedCommands.registerCommand("jimmy",
@@ -420,7 +442,21 @@ public class RobotContainer {
                 
             ).withTimeout(3).andThen(spindexer.instantaneousStop()));
 
-        NamedCommands.registerCommand("auto shoot", hood.trackToAnglePoseBased(drivetrain, shooter));
+        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/DrivetrainConstants.java

@@ -46,7 +46,7 @@ public class DrivetrainConstants {
     // 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/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/constants/ShooterConstants.java

@@ -43,14 +43,14 @@ public class ShooterConstants {
     public static final boolean kLeftShooterMotorInverted = true;
     public static final boolean kRightShooterMotorInverted = false;
 
-    public static final double kLeftP = 0.1;//0.01;//0.001;
+    public static final double kLeftP = 0.75;//0.01;//0.001;
     public static final double kLeftI = 0;
     public static final double kLeftD = 0;//0.1;//1.8;
     public static final double kLeftS = 0;
     public static final double kLeftV = 0.00129;
     public static final double kLeftA = 0;
 
-    public static final double kRightP = 0.1;//0.001;//0.001;
+    public static final double kRightP = 0.75;//0.001;//0.001;
     public static final double kRightI = 0;
     public static final double kRightD = 0;//0.1;
     public static final double kRightS = 0;

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(