progress on 3 piece auto

src/main/deploy/pathplanner/autos/3 Coral Left.auto

@@ -10,18 +10,6 @@
             "pathName": "Start to 30 Right"
           }
         },
-        {
-          "type": "named",
-          "data": {
-            "name": "Lift L4"
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "J Approach"
-          }
-        },
         {
           "type": "named",
           "data": {
@@ -62,19 +50,38 @@
         {
           "type": "named",
           "data": {
-            "name": "Lift L4"
+            "name": "Shoot Coral L4"
           }
         },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
               {
                 "type": "path",
                 "data": {
-            "pathName": "K Approach"
+                  "pathName": "L Backup"
                 }
               },
               {
                 "type": "named",
                 "data": {
-            "name": "Shoot Coral L4"
+                  "name": "HP Pickup"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Collect Coral"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HP to K"
           }
         }
       ]

src/main/deploy/pathplanner/paths/30 Right to HP.path

@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.5,
+    "maxVelocity": 5.0,
-    "maxAcceleration": 1.75,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": -120.06858282186238
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/330 Right to HP.path

@@ -45,8 +45,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.5,
+    "maxVelocity": 5.0,
-    "maxAcceleration": 1.75,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -62,5 +62,5 @@
     "velocity": 0,
     "rotation": -59.99999999999999
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/HP to K.path

@@ -0,0 +1,66 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.1987704918032787,
+        "y": 7.189754098360656
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.7142418032786886,
+        "y": 6.638319672131147
+      },
+      "isLocked": false,
+      "linkedName": "HP Left Position"
+    },
+    {
+      "anchor": {
+        "x": 3.9679303278688525,
+        "y": 5.211782786885246
+      },
+      "prevControl": {
+        "x": 3.2246926229508195,
+        "y": 6.230737704918033
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.0,
+      "endWaypointRelativePos": null,
+      "command": {
+        "type": "named",
+        "data": {
+          "name": "Lift L4"
+        }
+      }
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -60.49491285058726
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -53.97262661489646
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/L Backup.path

@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.5,
+    "maxVelocity": 5.0,
-    "maxAcceleration": 1.75,
+    "maxAcceleration": 2.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
@@ -50,5 +50,5 @@
     "velocity": 0,
     "rotation": -59.69923999693802
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

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

@@ -238,10 +238,10 @@ public class RobotContainer {
 		//new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
 
 		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
-		NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(1).andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.1)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition())));
+		NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(0.5).andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.01)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition())));
-		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runManipulator(() -> 0, false).withTimeout(0.1)));
+		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runManipulator(() -> 0, false).withTimeout(0.01)));
 		NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)
-			.andThen(elevator.maintainPosition().withTimeout(0.1), manipulatorPivot.maintainPosition().withTimeout(0.1)));
+			.andThen(elevator.maintainPosition().withTimeout(0.1), manipulatorPivot.maintainPosition().withTimeout(0.01)));
 		NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition));
 	}
 

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

@@ -12,12 +12,12 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class AutoConstants {
     public static final double kMaxSpeedMetersPerSecond = 5.5;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 4;
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
-    public static final double kPXController = 3;
+    public static final double kPXController = 3.5;
-    public static final double kPYController = 3;
+    public static final double kPYController = 5;
     public static final double kPThetaController = 5.5;
 
     // Constraint for the motion profiled robot angle controller

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

@@ -174,7 +174,7 @@ public class Drivetrain extends SubsystemBase {
 
         gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
 
-        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360)));
+        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
 
         if(vision.getOrangeTagDetected()){
             if(vision.getOrangeDist() < 60){