after portland before UNH

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

@@ -0,0 +1,69 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "intake down"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "left start to center"
+          }
+        },
+        {
+          "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": "auto shoot"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -17,15 +17,41 @@
           }
         },
         {
-          "type": "path",
+          "type": "parallel",
           "data": {
-            "pathName": "over bump to pile"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "over bump to pile"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "spinup"
+                }
+              }
+            ]
           }
         },
         {
-          "type": "path",
+          "type": "parallel",
           "data": {
-            "pathName": "back from center"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "back from center"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "spinup"
+                }
+              }
+            ]
           }
         },
         {

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

@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.050842639593909,
+        "x": 6.428375634517765,
         "y": 5.715482233502538
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.130030456852792,
+        "x": 5.507563451776648,
         "y": 5.7523147208121825
       },
       "isLocked": false,
@@ -42,8 +42,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 2.0,
+    "maxVelocity": 4.0,
-    "maxAcceleration": 1.5,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -57,7 +57,7 @@
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -123.34070734647689
+    "rotation": -129.95754893082906
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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

@@ -48,11 +48,11 @@
     },
     {
       "anchor": {
-        "x": 6.050842639593909,
+        "x": 6.428375634517765,
         "y": 5.715482233502538
       },
       "prevControl": {
-        "x": 5.525979695431473,
+        "x": 5.903512690355329,
         "y": 5.807563451776649
       },
       "nextControl": null,
@@ -68,14 +68,7 @@
   ],
   "constraintZones": [],
   "pointTowardsZones": [],
-  "eventMarkers": [
+  "eventMarkers": [],
-    {
-      "name": "Intake Start",
-      "waypointRelativePos": 2.1,
-      "endWaypointRelativePos": null,
-      "command": null
-    }
-  ],
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 2.0,
@@ -85,8 +78,8 @@
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
+    "velocity": 1.0,
-    "rotation": -123.34070734647689
+    "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.428375634517765,
+        "y": 5.715482233502538
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.897989847715735,
+        "y": 6.406091370558377
+      },
+      "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.428375634517765,
+        "y": 5.715482233502538
+      },
+      "prevControl": {
+        "x": 7.375431472081217,
+        "y": 5.183713197969544
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "over bump"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0045454545454569,
+      "rotationDegrees": -115.0
+    },
+    {
+      "waypointRelativePos": 2.348863636363685,
+      "rotationDegrees": -115.0
+    }
+  ],
+  "constraintZones": [
+    {
+      "name": "Constraints Zone",
+      "minWaypointRelativePos": 1.1243680485338854,
+      "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": 2.0,
+    "maxAcceleration": 1.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": -129.95754893082906
+  },
+  "useDefaultConstraints": true
+}

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

@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.050842639593909,
+        "x": 6.428375634517765,
         "y": 5.715482233502538
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.520456852791878,
+        "x": 6.897989847715735,
         "y": 6.406091370558377
       },
       "isLocked": false,
@@ -48,22 +48,26 @@
     },
     {
       "anchor": {
-        "x": 6.050842639593909,
+        "x": 6.428375634517765,
         "y": 5.715482233502538
       },
       "prevControl": {
-        "x": 6.99789847715736,
+        "x": 7.375431472081217,
         "y": 5.183713197969544
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": null
+      "linkedName": "over bump"
     }
   ],
   "rotationTargets": [
     {
       "waypointRelativePos": 1.0045454545454569,
       "rotationDegrees": -115.0
+    },
+    {
+      "waypointRelativePos": 2.348863636363685,
+      "rotationDegrees": -115.0
     }
   ],
   "constraintZones": [
@@ -100,13 +104,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -120.34324888423971
+    "rotation": -129.95754893082906
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -123.34070734647689
+    "rotation": -129.95754893082906
   },
   "useDefaultConstraints": true
 }

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)
+            );
         }
     }
 
@@ -391,7 +402,7 @@ public class RobotContainer {
 
         NamedCommands.registerCommand("spinup", 
             shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)
-                .withTimeout(3));
+                .withTimeout(2));
 
         NamedCommands.registerCommand("shoot close", 
             spindexer.spinToShooter()
@@ -405,6 +416,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 +435,8 @@ public class RobotContainer {
                 
             ).withTimeout(3).andThen(spindexer.instantaneousStop()));
 
-        NamedCommands.registerCommand("auto shoot", hood.trackToAnglePoseBased(drivetrain, shooter));
+        NamedCommands.registerCommand("auto shoot", hood.trackToAnglePoseBased(drivetrain, shooter)
+            .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), spindexer.spinToShooter(), intakePivot.jimmy(0.5)));
     }
 
     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/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;