Changes from 2/25 build session

src/main/deploy/pathplanner/autos/1 L4 Center.auto

@@ -0,0 +1,31 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Start to H"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot Coral L4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "H Backup"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

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

@@ -34,9 +34,9 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },

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

@@ -41,9 +41,9 @@
   ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },

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

@@ -46,9 +46,9 @@
   ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.7,
+        "y": 4.3
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.053790983606557,
+        "y": 4.312704918032787
+      },
+      "isLocked": false,
+      "linkedName": "H"
+    },
+    {
+      "anchor": {
+        "x": 6.389446721311475,
+        "y": 4.3
+      },
+      "prevControl": {
+        "x": 6.1394487099079695,
+        "y": 4.300997143065429
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

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

@@ -41,9 +41,9 @@
   ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },

src/main/deploy/pathplanner/paths/New Path.path

@@ -45,9 +45,9 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },

src/main/deploy/pathplanner/paths/Right Start to 150 Right.path

@@ -41,9 +41,9 @@
   ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },

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

@@ -41,9 +41,9 @@
   ],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },

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

@@ -0,0 +1,61 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.588217213114754,
+        "y": 3.9890368852459024
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.916905737704918,
+        "y": 4.036987704918033
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.7,
+        "y": 4.3
+      },
+      "prevControl": {
+        "x": 6.347336065573771,
+        "y": 4.2640368852459005
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "H"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [
+    {
+      "name": "Lift L4",
+      "waypointRelativePos": 0.08214624881291864,
+      "endWaypointRelativePos": null,
+      "command": null
+    }
+  ],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/fein.path

@@ -16,28 +16,28 @@
     },
     {
       "anchor": {
-        "x": 4.459426229508197,
+        "x": 6.209631147540984,
-        "y": 5.978995901639344
+        "y": 6.074897540983606
       },
       "prevControl": {
-        "x": 5.370491803278689,
+        "x": 7.120696721311476,
-        "y": 5.978995901639344
+        "y": 6.074897540983606
       },
       "nextControl": {
-        "x": 2.995158619919281,
+        "x": 4.745363537952068,
-        "y": 5.978995901639344
+        "y": 6.074897540983606
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.6543032786885246,
+        "x": 4.8670081967213115,
-        "y": 6.254713114754098
+        "y": 6.973975409836065
       },
       "prevControl": {
-        "x": 2.613319672131147,
+        "x": 5.826024590163935,
-        "y": 6.278688524590164
+        "y": 6.9979508196721305
       },
       "nextControl": null,
       "isLocked": false,
@@ -50,9 +50,9 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.0,
-    "maxAcceleration": 4.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0,
+    "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
@@ -64,7 +64,7 @@
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": -90.0
+    "rotation": 180.0
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/settings.json

@@ -5,11 +5,11 @@
   "pathFolders": [],
   "autoFolders": [],
   "defaultMaxVel": 4.0,
-  "defaultMaxAccel": 4.0,
+  "defaultMaxAccel": 1.0,
   "defaultMaxAngVel": 540.0,
-  "defaultMaxAngAccel": 720.0,
+  "defaultMaxAngAccel": 400.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 53.0,
+  "robotMass": 48.35,
   "robotMOI": 6.883,
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.038,

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

@@ -22,6 +22,8 @@ import java.util.function.IntSupplier;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
+import com.pathplanner.lib.events.EventTrigger;
+import com.pathplanner.lib.path.EventMarker;
 
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
@@ -70,6 +72,8 @@ public class RobotContainer {
 
 		manipulatorPivot = new ManipulatorPivot();
 
+		configureNamedCommands();
+
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
@@ -81,8 +85,6 @@ public class RobotContainer {
 		//elevatorSysIDBindings();
 		//elevatorOnlyBindings();
 
-		configureNamedCommands();
-
 		configureShuffleboard();
 	}
 
@@ -114,8 +116,8 @@ public class RobotContainer {
 
 		drivetrain.setDefaultCommand(
 			drivetrain.drive(
-				driver::getLeftY,
+				() -> driver.getLeftY(),
-				driver::getLeftX,
+				() -> driver.getLeftX(),
 				driver::getRightX,
 				() -> true
 			)
@@ -148,7 +150,7 @@ public class RobotContainer {
 		);
 
 		driver.leftTrigger().whileTrue( 
-			manipulator.runUntilCollected(() -> 0.35)
+			manipulator.runUntilCollected(() -> 0.75)
 		);
 
 		driver.start().and(driver.back()).onTrue(
@@ -217,21 +219,27 @@ public class RobotContainer {
 
 		operator.x().onTrue(
 			safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
-			
+				.alongWith(manipulator.runManipulator(() -> 0.5, false))
-		).and(driver.rightTrigger().whileFalse(manipulator.runManipulator(() -> 0.5, false)));
+				.until(driver.rightTrigger())
+		);
 
 		operator.b().onTrue(
 			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
-		).and(driver.rightTrigger().whileFalse(manipulator.runManipulator(() -> 0.5, false)));
+				.alongWith(manipulator.runManipulator(() -> 0.5, false))
+				.until(driver.rightTrigger())
+		);
 
-		operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition))
+		operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
-		.and(driver.rightTrigger().whileFalse(manipulator.runManipulator(() -> 0.5, false)));
+			.alongWith(manipulator.runManipulator(() -> 0.5, false))
+			.until(driver.rightTrigger())
+		);
 
 	}
 
 	private void configureNamedCommands() {
+		new EventTrigger("Lift L4").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
 		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
-		NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true));
+		NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2));
 		NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.35));
 		NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position));
 		NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kCoralIntakePosition));
@@ -311,6 +319,9 @@ public class RobotContainer {
 
 		sensorTab.addDouble("dt distance", drivetrain::driveDistance);
 
+		sensorTab.addDouble("velocity", drivetrain::getVelocity);
+
+
 		//sensorTab.add("odometry", drivetrain::getPose);
 
  	}

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

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

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

@@ -45,7 +45,7 @@ public class ElevatorConstants {
     public static final double kL3Position = 23.0;
     public static final double kL4Position = 50.5;
     public static final double kL4TransitionPosition = 40.0;
-    public static final double kL2AlgaePosition = 21.0;
+    public static final double kL2AlgaePosition = 23.0;
     public static final double kL3AlgaePosition = 39.0;
     public static final double kProcessorPosition = 4.0;
     /**The position of the top of the elevator brace */

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

@@ -370,4 +370,8 @@ public class Drivetrain extends SubsystemBase {
     public double driveDistance(){
         return m_frontLeft.getTotalDist();
     }
+
+    public double getVelocity(){
+        return m_frontLeft.getState().speedMetersPerSecond;
+    }
 }