on controller elevator PID control

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

@@ -424,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);

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

@@ -45,8 +45,10 @@ public class ElevatorConstants {
     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 = 100.0;    // 120 inches per second (COOKING) calculated max is 184 in/s
+    public static final double kMaxVelocity = 100.0;    // 100 inches per second (COOKING) calculated max is 184 in/s
-    public static final double kMaxAcceleration = 50;    // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
+    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;

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

@@ -5,16 +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.math.trajectory.TrapezoidProfile.State;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -28,16 +29,14 @@ public class Elevator extends SubsystemBase {
 
     private DigitalInput bottomLimitSwitch;
 
-    private PIDController pidController;
-
-    private PIDController maintainPID;
-
     private ElevatorFeedforward feedForward;
 
     private TrapezoidProfile trapProfile;
+    private TrapezoidProfile trapProfileAlgae;
     private TrapezoidProfile.State goal;
-    private TrapezoidProfile.State state;
+    private TrapezoidProfile.State setpoint;
-    private TrapezoidProfile.Constraints elevatorConstraints;
+
+    private SparkClosedLoopController controller;
     
     public Elevator() {
         elevatorMotor1 = new SparkMax(
@@ -74,7 +73,11 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kFeedForwardV
         );
 
-        trapProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(elevatorConstraints.maxVelocity, ElevatorConstants.kMaxAcceleration));
+        trapProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocity, ElevatorConstants.kMaxAcceleration));
+
+        trapProfileAlgae = new TrapezoidProfile(new TrapezoidProfile.Constraints(ElevatorConstants.kMaxVelocityAlgae, ElevatorConstants.kMaxAccelerationAlgae));
+
+        controller = elevatorMotor1.getClosedLoopController();
     }
 
     @Override
@@ -140,14 +143,12 @@ public class Elevator extends SubsystemBase {
 
         }, 
         () -> {
-
+            controller.setReference(
-            } 
+                setpoint.position, 
-            
+                ControlType.kPosition, 
-/* 
+                ClosedLoopSlot.kSlot0,
-            elevatorMotor1.setVoltage(
+                feedForward.calculate(0.0)
-                    feedForward.calculate(0)
+            );
-                );
-                */
 
         });
         
@@ -172,19 +173,34 @@ public class Elevator extends SubsystemBase {
         return startRun(() -> {
             goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
         }, () -> {
-            TrapezoidProfile.State setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
+            setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
 
-            
+            controller.setReference(
-        }).until();
+                setpoint.position, 
+                ControlType.kPosition, 
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(encoder.getVelocity())
+            );
+
+        }).until(() -> trapProfile.isFinished(encoder.getPosition()));
 
     }          
 
-    public Command goToSetpointAlgae(DoubleSupplier setpoint) {
+    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(
-            }).until();
+                setpoint.position, 
+                ControlType.kPosition, 
+                ClosedLoopSlot.kSlot0,
+                feedForward.calculate(setpoint.velocity)
+            );
+
+        }).until(() -> trapProfileAlgae.isFinished(encoder.getPosition()));
     }   
 
     /**
@@ -224,19 +240,4 @@ public class Elevator extends SubsystemBase {
         return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage();
     }
 
-    public double getPIDUpSetpoint() {
-        return pidController.getSetpoint();
-    }
-
-    public double getPIDUpError() {
-        return pidController.getError();
-    }
-
-    public double getPIDDownSetpoint() {
-        return pidControllerDown.getSetpoint();
-    }
-
-    public double getPIDDownError() {
-        return pidControllerDown.getError();
-    }
 }