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

@@ -45,25 +45,6 @@
           "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.1268442622950818,
+        "x": 1.2587090163934425,
-        "y": 7.201741803278688
+        "y": 7.08186475409836
       },
       "prevControl": {
-        "x": 2.287270519874242,
+        "x": 2.4191352739726026,
-        "y": 6.774371912194027
+        "y": 6.654494863013699
       },
       "nextControl": null,
       "isLocked": false,
@@ -34,7 +34,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 5.0,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

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

@@ -46,7 +46,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 5.0,
-    "maxAcceleration": 3.5,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

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

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.1268442622950818,
+        "x": 1.2587090163934425,
-        "y": 7.201741803278688
+        "y": 7.08186475409836
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.0019467213114757,
+        "x": 2.550917920503032,
-        "y": 6.434528688524591
+        "y": 6.856479480125757
       },
       "isLocked": false,
       "linkedName": "HP Left Position"
@@ -46,7 +46,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 3.5,
-    "maxAcceleration": 1.25,
+    "maxAcceleration": 1.75,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

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

@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.1268442622950818,
+        "x": 1.2587090163934425,
-        "y": 7.201741803278688
+        "y": 7.08186475409836
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.9266540815180775,
+        "x": 2.058518835616438,
-        "y": 6.744320542331013
+        "y": 6.624443493150685
       },
       "isLocked": false,
       "linkedName": "HP Left Position"
@@ -46,7 +46,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 5.0,
-    "maxAcceleration": 1.5,
+    "maxAcceleration": 1.75,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

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

@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 1.1268442622950818,
+        "x": 1.2587090163934425,
-        "y": 7.201741803278688
+        "y": 7.08186475409836
       },
       "prevControl": {
-        "x": 1.3306352459016395,
+        "x": 1.4625000000000001,
-        "y": 6.926024590163935
+        "y": 6.8061475409836065
       },
       "nextControl": null,
       "isLocked": false,
@@ -34,7 +34,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 5.0,
-    "maxAcceleration": 3.5,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

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

@@ -1,54 +0,0 @@
-{
-  "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/deploy/pathplanner/paths/Start to 30 Right.path

@@ -46,7 +46,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 3.5,
-    "maxAcceleration": 1.25,
+    "maxAcceleration": 1.75,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

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

@@ -339,7 +339,7 @@ public class RobotContainer {
 					), 
 				manipulatorPivot.maintainPosition()
 					.withTimeout(
-						0.1
+						0.01
 					)
 			)
 		);
@@ -354,24 +354,14 @@ public class RobotContainer {
 
 		NamedCommands.registerCommand(
 			"Shoot Algae",
-			shootAlgae().withTimeout(2)   
+			shootAlgae()
 		);
 
 		NamedCommands.registerCommand(
 			"Processor Position", 
 			moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
-			.raceWith(manipulator.runManipulator(() -> 0.85, false))
+			.alongWith(manipulator.runManipulator(() -> 0.85, false))
-		);
+			.until(() -> driver.a().getAsBoolean())
-
-		NamedCommands.registerCommand(
-			"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?
 		);
 	}
 
@@ -630,9 +620,9 @@ public class RobotContainer {
 
 	private Command shootAlgae(){
 		return manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
-		.andThen(elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
-		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>36/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
+		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>43/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
-		elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
 		.raceWith(elevator.maintainPosition()));
 	}
 

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

@@ -56,8 +56,6 @@ 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/Drivetrain.java

@@ -111,8 +111,8 @@ public class Drivetrain extends SubsystemBase {
                 m_rearRight.getPosition()
             },
             new Pose2d(),
-            VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
+            VecBuilder.fill(0.07, 0.7, Units.degreesToRadians(0.5)),
-            VecBuilder.fill(1, 1, Units.degreesToRadians(360))
+            VecBuilder.fill(1, 1, Units.degreesToRadians(4000))
         );
 
         pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
@@ -177,17 +177,17 @@ public class Drivetrain extends SubsystemBase {
 
         gyroBuffer.addSample(Timer.getFPGATimestamp(), m_estimator.getEstimatedPosition().getRotation().getDegrees());
 
-        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));
+        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(4000)));
 
         if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
             if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000)));
             }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000)));
             }else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000)));
             }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000)));
             }
 
             // if the detected tags match your alliances reef tags use their pose estimates  
@@ -209,13 +209,13 @@ public class Drivetrain extends SubsystemBase {
 
         if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
             if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000)));
             }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000)));
             }else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000)));
             }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000)));
             }
 
             if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){

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

@@ -253,64 +253,6 @@ 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
      *