no worky

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,
-        "y": 7.08186475409836
+        "x": 1.1268442622950818,
+        "y": 7.201741803278688
       },
       "prevControl": {
-        "x": 2.115173057489333,
-        "y": 6.135246603413428
+        "x": 2.287270519874242,
+        "y": 6.774371912194027
       },
       "nextControl": null,
       "isLocked": false,
@@ -34,7 +34,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 5.0,
-    "maxAcceleration": 2.0,
+    "maxAcceleration": 2.5,
     "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": 2.5,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

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

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.2587090163934425,
-        "y": 7.08186475409836
+        "x": 1.1268442622950818,
+        "y": 7.201741803278688
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.550917920503032,
-        "y": 6.856479480125757
+        "x": 2.0019467213114757,
+        "y": 6.434528688524591
       },
       "isLocked": false,
       "linkedName": "HP Left Position"
     },
     {
       "anchor": {
-        "x": 3.6442622950819668,
-        "y": 5.031967213114754
+        "x": 3.609900518622585,
+        "y": 5.005924534374863
       },
       "prevControl": {
-        "x": 3.328722911520323,
-        "y": 5.557866185717494
+        "x": 3.2943611350609414,
+        "y": 5.531823506977602
       },
       "nextControl": null,
       "isLocked": false,
@@ -46,7 +46,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 3.5,
-    "maxAcceleration": 1.5,
+    "maxAcceleration": 1.25,
     "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.2587090163934425,
-        "y": 7.08186475409836
+        "x": 1.1268442622950818,
+        "y": 7.201741803278688
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.7741803278688524,
-        "y": 6.530430327868851
+        "x": 1.9266540815180775,
+        "y": 6.744320542331013
       },
       "isLocked": false,
       "linkedName": "HP Left Position"
@@ -20,8 +20,8 @@
         "y": 5.211782786885246
       },
       "prevControl": {
-        "x": 3.2246926229508195,
-        "y": 6.230737704918033
+        "x": 2.975085616438356,
+        "y": 6.023416095890411
       },
       "nextControl": null,
       "isLocked": false,
@@ -45,7 +45,7 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.5,
+    "maxVelocity": 5.0,
     "maxAcceleration": 1.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,

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

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 3.6442622950819668,
-        "y": 5.031967213114754
+        "x": 3.609900518622585,
+        "y": 5.005924534374863
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.5117625600811717,
-        "y": 5.243966789116026
+        "x": 3.47740078362179,
+        "y": 5.217924110376135
       },
       "isLocked": false,
       "linkedName": "L"
     },
     {
       "anchor": {
-        "x": 1.2587090163934425,
-        "y": 7.08186475409836
+        "x": 1.1268442622950818,
+        "y": 7.201741803278688
       },
       "prevControl": {
-        "x": 1.4625000000000001,
-        "y": 6.8061475409836065
+        "x": 1.3306352459016395,
+        "y": 6.926024590163935
       },
       "nextControl": null,
       "isLocked": false,
@@ -34,7 +34,7 @@
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 5.0,
-    "maxAcceleration": 2.5,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 400.0,
     "nominalVoltage": 12.0,

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

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.756421232876712,
+        "y": 5.227054794520548
+      },
+      "prevControl": {
+        "x": 2.756421232876712,
+        "y": 5.227054794520548
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.5,
+    "maxAcceleration": 1.75,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 400.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -59.18537788806707
+  },
+  "reversed": false,
+  "folder": "Left Paths",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

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/deploy/pathplanner/paths/Start to 30 Right.path

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

src/main/java/frc/robot/Robot.java

@@ -7,7 +7,7 @@ package frc.robot;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.Logger;
-//import org.littletonrobotics.junction.networktables.NT4Publisher;
+import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
@@ -35,7 +35,7 @@ public Robot() {
 
     if (isReal()) {
         Logger.addDataReceiver(new WPILOGWriter()); // Log to a USB stick ("/U/logs")
-        //Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
+        Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables
         new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
     } else {
         setUseTiming(false); // Run as fast as possible

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

@@ -111,9 +111,9 @@ public class RobotContainer {
 
 		drivetrain.setDefaultCommand(
 			drivetrain.drive(
-				() -> driver.getLeftY() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
-				() -> driver.getLeftX() * Math.pow(Math.abs(driver.getLeftY()) + Math.abs(driver.getLeftX()), 3),
-				driver::getRightX, //Math.signum(driver.getRightX()) * Math.pow(driver.getRightX(), 3)
+				() -> Math.pow(driver.getLeftY(), 3),
+				() -> Math.pow(driver.getLeftX(), 3),
+				() -> driver.getRightX(), 
 				() -> true
 			)
 		);
@@ -339,7 +339,7 @@ public class RobotContainer {
 					), 
 				manipulatorPivot.maintainPosition()
 					.withTimeout(
-						0.01
+						0.1
 					)
 			)
 		);
@@ -354,14 +354,24 @@ public class RobotContainer {
 
 		NamedCommands.registerCommand(
 			"Shoot Algae",
-			shootAlgae()
+			shootAlgae().withTimeout(2)   
 		);
 
 		NamedCommands.registerCommand(
 			"Processor Position", 
 			moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
-			.alongWith(manipulator.runManipulator(() -> 0.85, false))
-			.until(() -> driver.a().getAsBoolean())
+			.raceWith(manipulator.runManipulator(() -> 0.85, false))
+		);
+
+		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?
 		);
 	}
 
@@ -414,26 +424,6 @@ public class RobotContainer {
 		sensorTab.addDouble("ElevMotor2", elevator::getMotor2)
 			.withWidget(BuiltInWidgets.kGraph);
 
-		sensorTab.addDouble("Elevator setpoint up", elevator::getPIDUpSetpoint)
-			.withSize(1, 1)
-			.withPosition(5, 0)
-			.withWidget(BuiltInWidgets.kTextView);
-
-		sensorTab.addDouble("Elevator error up", elevator::getPIDUpError)
-			.withSize(1, 1)
-			.withPosition(5, 1)
-			.withWidget(BuiltInWidgets.kTextView);
-
-			sensorTab.addDouble("Elevator setpoint down", elevator::getPIDDownSetpoint)
-			.withSize(1, 1)
-			.withPosition(5, 0)
-			.withWidget(BuiltInWidgets.kTextView);
-
-		sensorTab.addDouble("Elevator error down", elevator::getPIDDownError)
-			.withSize(1, 1)
-			.withPosition(5, 1)
-			.withWidget(BuiltInWidgets.kTextView);
-
 		sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput);
 
 		sensorTab.addDouble("velocity", drivetrain::getVelocity);
@@ -620,9 +610,9 @@ public class RobotContainer {
 
 	private Command shootAlgae(){
 		return manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
-		.andThen(elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
-		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>43/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
-		elevator.goToSetpoint(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.andThen(elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
+		.raceWith(elevator.maintainPosition())).until(() -> elevator.getEncoderPosition()>36/* 44*/).andThen(manipulator.runManipulator(() -> -1, false),
+		elevator.goToSetpointAlgae(() -> ElevatorConstants.kL4Position), manipulatorPivot.goToSetpointAlgae(() -> ManipulatorPivotConstants.kBargeShotPosition)
 		.raceWith(elevator.maintainPosition()));
 	}
 

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

@@ -16,6 +16,8 @@ public class AutoConstants {
     public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
     public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
 
+    public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5;
+
     public static final double kPXYController = 3.5;
     public static final double kPThetaController = 5;
 

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

@@ -13,7 +13,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
 public class DrivetrainConstants {
     // Driving Parameters - Note that these are not the maximum capable speeds of
     // the robot, rather the allowed maximum speeds
-    public static final double kMaxSpeedMetersPerSecond = 5.5;
+    public static final double kMaxSpeedMetersPerSecond = 5.5 * 0.75;
     public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
 
     // Chassis configuration

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

@@ -5,6 +5,7 @@ import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Seconds;
 
 import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
 import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
 
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
@@ -22,6 +23,7 @@ public class ElevatorConstants {
 
     public static final int kCurrentLimit = 40;
 
+    /*
     public static final double kUpControllerP = 5.6;//7; //
     public static final double kUpControllerI = 0;
     public static final double kUpControllerD = 0.28;//0.28
@@ -31,15 +33,22 @@ public class ElevatorConstants {
     public static final double kDownControllerD = 0.57;//0.175;//0.1;//0.35
 
     public static final double kMaintainP = 3;
-     
+    */
+
+    public static final double kP = 3;//7; //
+    public static final double kI = 0;
+    public static final double kD = 0;//.28;//0.28 
+
     public static final double kAllowedError = 1;
 
     public static final double kFeedForwardS = (0.95 - 0.2)/2*0.8;   /* kG too high - kG too low / 2  0.95, 0.2 */
     public static final double kFeedForwardG = (0.95 + 0.2)/2;  /* kG too high + kG too low / 2 */    // calculated value 0.6
     public static final double kFeedForwardV = 0.12;   // calculated value 0.12
 
-    public static final double kMaxVelocity = 150.0;    // 120 inches per second (COOKING) calculated max is 184 in/s
-    public static final double kMaxAcceleration = 240;    // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
+    public static final double kMaxVelocity = 100.0;    // 100 inches per second (COOKING) calculated max is 184 in/s
+    public static final double kMaxAcceleration = 50;    // 50 inches per second^2 (also COOKING) calculated max is 600 in/s^2
+    public static final double kMaxVelocityAlgae = 120;
+    public static final double kMaxAccelerationAlgae = 400;
 
     public static final double kCoralIntakePosition = 0;
     public static final double kL1Position = 17;
@@ -56,6 +65,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;
@@ -81,5 +92,9 @@ public class ElevatorConstants {
         motorConfig.encoder
             .positionConversionFactor(kEncoderPositionConversionFactor)
             .velocityConversionFactor(kEncoderVelocityConversionFactor);
+        motorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1);
     }
 }

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

@@ -85,6 +85,6 @@ public class VisionConstants {
         {5.322, 2.511}//22
     };
 
-    public static final double latencyFudge = 0.03;
+    public static final double latencyFudge = 0.0;
 
 }

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.07, 0.7, Units.degreesToRadians(0.5)),
-            VecBuilder.fill(1, 1, Units.degreesToRadians(4000))
+            VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(5)),
+            VecBuilder.fill(1, 1, Units.degreesToRadians(360))
         );
 
         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(4000)));
+        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.40, 0.40, Units.degreesToRadians(360)));
 
         if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){
             if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) < 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
             }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) < 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
             }else if(vision.getOrangeDist() < 60 && Math.abs(getVelocity()) > 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
             }else if(vision.getOrangeDist() < 100 && Math.abs(getVelocity()) > 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
             }
 
             // if the detected tags match your alliances reef tags use their pose estimates  
@@ -204,17 +204,18 @@ public class Drivetrain extends SubsystemBase {
         Logger.recordOutput("orange dist", vision.getOrangeDist());
         Logger.recordOutput("orange detected", vision.getOrangeTagDetected());
         Logger.recordOutput("orange tag", vision.getOrangeTagDetected());
+        Logger.recordOutput("orange FPS", vision.getOrangeFPS());
 
 
         if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){
             if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) < 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(4000)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.03, 0.03, Units.degreesToRadians(360)));
             }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) < 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(4000)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360)));
             }else if(vision.getBlackDist() < 60 && Math.abs(getVelocity()) > 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(4000)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(360)));
             }else if(vision.getBlackDist() < 100 && Math.abs(getVelocity()) > 3){
-                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(4000)));
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360)));
             }
 
             if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
@@ -230,6 +231,7 @@ public class Drivetrain extends SubsystemBase {
         Logger.recordOutput("black dist", vision.getBlackDist());
         Logger.recordOutput("black detected", vision.getBlackTagDetected());
         Logger.recordOutput("black tag", vision.getBlackTagDetected());
+        Logger.recordOutput("black FPS", vision.getBlackFPS());
 
         Logger.recordOutput("drive velocity", getVelocity());
         Logger.recordOutput("closest tag", getClosestTag());
@@ -360,8 +362,8 @@ public class Drivetrain extends SubsystemBase {
                 );
 
                 double speed = Math.hypot(controlEffort.vxMetersPerSecond, controlEffort.vyMetersPerSecond);
-                if (speed > AutoConstants.kMaxSpeedMetersPerSecond) {
-                    double mul = AutoConstants.kMaxSpeedMetersPerSecond / speed;
+                if (speed > AutoConstants.kMaxSpeedMetersPerSecondAutoAlign) {
+                    double mul = AutoConstants.kMaxSpeedMetersPerSecondAutoAlign / speed;
                     controlEffort.vxMetersPerSecond *= mul;
                     controlEffort.vyMetersPerSecond *= mul;
                 }

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

@@ -5,14 +5,17 @@ import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkBase.PersistMode;
 import com.revrobotics.spark.SparkBase.ResetMode;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ElevatorFeedforward;
-import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -26,12 +29,14 @@ public class Elevator extends SubsystemBase {
 
     private DigitalInput bottomLimitSwitch;
 
-    private PIDController pidControllerUp;
-    private PIDController pidControllerDown;
-
-    private PIDController maintainPID;
-
     private ElevatorFeedforward feedForward;
+
+    private TrapezoidProfile trapProfile;
+    private TrapezoidProfile trapProfileAlgae;
+    private TrapezoidProfile.State goal;
+    private TrapezoidProfile.State setpoint;
+
+    private SparkClosedLoopController controller;
     
     public Elevator() {
         elevatorMotor1 = new SparkMax(
@@ -62,33 +67,17 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kBottomLimitSwitchID
         );
 
-        pidControllerDown = new PIDController(
-            ElevatorConstants.kDownControllerP, 
-            ElevatorConstants.kDownControllerI,
-            ElevatorConstants.kDownControllerD
-        );
-        pidControllerDown.setSetpoint(0);
-
-        pidControllerDown.setTolerance(ElevatorConstants.kAllowedError);
-
-        pidControllerUp = new PIDController(
-            ElevatorConstants.kUpControllerP, 
-            ElevatorConstants.kUpControllerI,
-            ElevatorConstants.kUpControllerD
-        );
-        pidControllerUp.setSetpoint(0);
-
-        pidControllerUp.setTolerance(ElevatorConstants.kAllowedError);
-
-        maintainPID = new PIDController(ElevatorConstants.kMaintainP, 0, 0);
-
-        maintainPID.setTolerance(ElevatorConstants.kAllowedError);
-
         feedForward = new ElevatorFeedforward(
             ElevatorConstants.kFeedForwardS, 
             ElevatorConstants.kFeedForwardG, 
             ElevatorConstants.kFeedForwardV
         );
+
+        trapProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocity, ElevatorConstants.kMaxAcceleration));
+
+        trapProfileAlgae = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocityAlgae, ElevatorConstants.kMaxAccelerationAlgae));
+
+        controller = elevatorMotor1.getClosedLoopController();
     }
 
     @Override
@@ -98,8 +87,6 @@ public class Elevator extends SubsystemBase {
         }
 
         Logger.recordOutput("elevator position", getEncoderPosition());
-        Logger.recordOutput("elevator up setpoint", pidControllerUp.getSetpoint());
-        Logger.recordOutput("elevator down setpoint", pidControllerDown.getSetpoint());
     }
 
     /**
@@ -153,28 +140,15 @@ public class Elevator extends SubsystemBase {
     public Command maintainPosition() {
 
         return startRun(() -> {
-            maintainPID.reset();
-            maintainPID.setSetpoint(pidControllerUp.getSetpoint());
+
         }, 
         () -> {
-
-            double maintainOutput = maintainPID.calculate(getEncoderPosition());
-
-            if(!maintainPID.atSetpoint()) 
-                elevatorMotor1.setVoltage( MathUtil.clamp(
-                    maintainOutput + feedForward.calculate(0), -2, 2)
-                );
-            else{
-                elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
-            } 
-            
-/* 
-            elevatorMotor1.setVoltage(
-                    feedForward.calculate(0)
-                );
-                */
+            controller.setReference(
+                encoder.getPosition(), 
+                ControlType.kPosition, 
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(0.0)
+            );
 
         });
         
@@ -192,67 +166,43 @@ public class Elevator extends SubsystemBase {
      * Moves the elevator to a target destination (setpoint). 
      * 
      * @param setpoint Target destination of the subsystem
-     * @param timeout Time to achieve the setpoint before quitting
      * @return Sets motor voltage to achieve the target destination
      */
-    public Command goToSetpoint(DoubleSupplier setpoint) {
-            
-        if (setpoint.getAsDouble() == 0) {
-            return startRun(() -> {
+    public Command goToSetpoint(DoubleSupplier setGoal) {
         
-                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.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    else{
-                        elevatorMotor1.setVoltage(
-                            MathUtil.clamp(
-                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    }
-                
-                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint())
-                    .andThen(runManualElevator(() -> -.5)
-                    .until(() -> encoder.getPosition() == 0));
+        return startRun(() -> {
+            goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
+        }, () -> {
+            setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
+
+            controller.setReference(
+                setpoint.position, 
+                ControlType.kPosition, 
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(encoder.getVelocity())
+            );
+
+        }).until(() -> trapProfile.isFinished(encoder.getPosition()));
 
-        } 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.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    else{
-                        elevatorMotor1.setVoltage(
-                            MathUtil.clamp(
-                            downOutput + feedForward.calculate(0), ElevatorConstants.kVoltageLimit * -1, ElevatorConstants.kVoltageLimit)
-                        );
-                    }
-                
-                }).until(() -> pidControllerUp.atSetpoint() || pidControllerDown.atSetpoint());
-        }
     }          
 
+    public Command goToSetpointAlgae(DoubleSupplier setGoal) {
+            
+        return startRun(() -> {
+            goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
+        }, () -> {
+            setpoint = trapProfileAlgae.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
+
+            controller.setReference(
+                setpoint.position, 
+                ControlType.kPosition, 
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(setpoint.velocity)
+            );
+
+        }).until(() -> trapProfileAlgae.isFinished(encoder.getPosition()));
+    }   
+
     /**
      * Returns the current encoder position
      * 
@@ -290,19 +240,4 @@ public class Elevator extends SubsystemBase {
         return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
     }
 
-    public double getPIDUpSetpoint() {
-        return pidControllerUp.getSetpoint();
-    }
-
-    public double getPIDUpError() {
-        return pidControllerUp.getError();
-    }
-
-    public double getPIDDownSetpoint() {
-        return pidControllerDown.getSetpoint();
-    }
-
-    public double getPIDDownError() {
-        return pidControllerDown.getError();
-    }
 }