After champs code

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

@@ -45,6 +45,25 @@
           "data": {
             "name": "Shoot Algae"
           }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "HP Pickup"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Post-Barge Backup"
+                }
+              }
+            ]
+          }
         }
       ]
     }

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

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.2587090163934425,
+        "x": 1.1268442622950818,
-        "y": 7.08186475409836
+        "y": 7.201741803278688
       },
       "prevControl": {
-        "x": 2.4191352739726026,
+        "x": 2.287270519874242,
-        "y": 6.654494863013699
+        "y": 6.774371912194027
       },
       "nextControl": null,
       "isLocked": false,

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

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.2587090163934425,
+        "x": 1.1268442622950818,
-        "y": 7.08186475409836
+        "y": 7.201741803278688
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.550917920503032,
+        "x": 2.0019467213114757,
-        "y": 6.856479480125757
+        "y": 6.434528688524591
       },
       "isLocked": false,
       "linkedName": "HP Left Position"

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

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.2587090163934425,
+        "x": 1.1268442622950818,
-        "y": 7.08186475409836
+        "y": 7.201741803278688
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.058518835616438,
+        "x": 1.9266540815180775,
-        "y": 6.624443493150685
+        "y": 6.744320542331013
       },
       "isLocked": false,
       "linkedName": "HP Left Position"

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

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.2587090163934425,
+        "x": 1.1268442622950818,
-        "y": 7.08186475409836
+        "y": 7.201741803278688
       },
       "prevControl": {
-        "x": 1.4625000000000001,
+        "x": 1.3306352459016395,
-        "y": 6.8061475409836065
+        "y": 6.926024590163935
       },
       "nextControl": null,
       "isLocked": false,

src/main/deploy/pathplanner/paths/Post-Barge Backup.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.032020547945206,
+        "y": 4.761258561643835
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.71311475409836,
+        "y": 4.9480532786885245
+      },
+      "isLocked": false,
+      "linkedName": "Pre-Barge"
+    },
+    {
+      "anchor": {
+        "x": 5.933913934426228,
+        "y": 5.247745901639345
+      },
+      "prevControl": {
+        "x": 6.641188524590164,
+        "y": 5.043954918032786
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Center",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

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

@@ -339,7 +339,7 @@ public class RobotContainer {
 					), 
 				manipulatorPivot.maintainPosition()
 					.withTimeout(
-						0.01
+						0.1
 					)
 			)
 		);
@@ -354,7 +354,7 @@ public class RobotContainer {
 
 		NamedCommands.registerCommand(
 			"Shoot Algae",
-			shootAlgae()
+			shootAlgae().withTimeout(2)   
 		);
 
 		NamedCommands.registerCommand(
@@ -367,6 +367,11 @@ public class RobotContainer {
 			"Pickup Algae L2", 
 			moveWithAlgae(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
 			.raceWith(manipulator.runManipulator(() -> 0.85, false))
+			.andThen(
+				elevator.maintainPosition()
+				.alongWith(manipulatorPivot.maintainPosition())).withTimeout(0.1)
+
+			//Dont you need a holdPosition call?
 		);
 	}
 
@@ -625,9 +630,9 @@ public class RobotContainer {
 
 	private Command shootAlgae(){
 		return manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
-		.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.andThen(elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
-		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>43/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
+		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>36/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
-		elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
 		.raceWith(elevator.maintainPosition()));
 	}
 

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

@@ -56,6 +56,8 @@ public class ElevatorConstants {
 
     public static final double kVoltageLimit = 7;
 
+    public static final double kVoltageLimitAlgae = 9;
+
     // 1, 7, 10 are the defaults for these, change as necessary
     public static final double kSysIDRampRate = .25;
     public static final double kSysIDStepVolts = 3;

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

@@ -253,6 +253,64 @@ public class Elevator extends SubsystemBase {
         }
     }          
 
+    public Command goToSetpointAlgae(DoubleSupplier setpoint) {
+            
+        if (setpoint.getAsDouble() == 0) {
+            return startRun(() -> {
+        
+                pidControllerUp.reset();
+                pidControllerDown.reset();
+                pidControllerUp.setSetpoint(setpoint.getAsDouble());
+                pidControllerDown.setSetpoint(setpoint.getAsDouble());
+                
+                }, 
+                () -> {
+                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
+                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
+        
+                    if(setpoint.getAsDouble()>encoder.getPosition()) 
+                        elevatorMotor1.setVoltage( MathUtil.clamp(
+                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
+                        );
+                    else{
+                        elevatorMotor1.setVoltage(
+                            MathUtil.clamp(
+                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
+                        );
+                    }
+                
+                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
+                    .andThen(runManualElevator(() -> -.5)
+                    .until(() -> encoder.getPosition() == 0));
+
+        } else {
+            return startRun(() -> {
+        
+                pidControllerUp.reset();
+                pidControllerDown.reset();
+                pidControllerUp.setSetpoint(setpoint.getAsDouble());
+                pidControllerDown.setSetpoint(setpoint.getAsDouble());
+        
+                },
+                () -> {
+                    double upOutput = pidControllerUp.calculate(getEncoderPosition());
+                    double downOutput = pidControllerDown.calculate(getEncoderPosition());
+        
+                    if(setpoint.getAsDouble()>encoder.getPosition()) 
+                        elevatorMotor1.setVoltage( MathUtil.clamp(
+                            upOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
+                        );
+                    else{
+                        elevatorMotor1.setVoltage(
+                            MathUtil.clamp(
+                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimitAlgae * -1, ElevatorConstants.kVoltageLimitAlgae)
+                        );
+                    }
+                
+                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
+        }
+    }   
+
     /**
      * Returns the current encoder position
      *