in the midst of making controller based pid control

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,20 @@ 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 = 5.6;//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 kMaxVelocity = 100.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 kMaxAcceleration = 50;    // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
 
     public static final double kCoralIntakePosition = 0;
     public static final double kL1Position = 17;
@@ -83,5 +90,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/subsystems/Elevator.java

@@ -13,6 +13,8 @@ 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;
@@ -26,12 +28,16 @@ public class Elevator extends SubsystemBase {
 
     private DigitalInput bottomLimitSwitch;
 
-    private PIDController pidControllerUp;
+    private PIDController pidController;
-    private PIDController pidControllerDown;
 
     private PIDController maintainPID;
 
     private ElevatorFeedforward feedForward;
+
+    private TrapezoidProfile trapProfile;
+    private TrapezoidProfile.State goal;
+    private TrapezoidProfile.State state;
+    private TrapezoidProfile.Constraints elevatorConstraints;
     
     public Elevator() {
         elevatorMotor1 = new SparkMax(
@@ -62,33 +68,13 @@ 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(elevatorConstraints.maxVelocity, ElevatorConstants.kMaxAcceleration));
     }
 
     @Override
@@ -98,8 +84,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,21 +137,10 @@ 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)
-                );
             } 
             
 /* 
@@ -192,123 +165,26 @@ 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) {
+    public Command goToSetpoint(DoubleSupplier setGoal) {
             
-        if (setpoint.getAsDouble() == 0) {
+        return startRun(() -> {
-            return startRun(() -> {
+            goal = new TrapezoidProfile.State(setGoal.getAsDouble(), 0.0);
-        
+        }, () -> {
-                pidControllerUp.reset();
+            TrapezoidProfile.State setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal);
-                pidControllerDown.reset();
+
-                pidControllerUp.setSetpoint(setpoint.getAsDouble());
+            
-                pidControllerDown.setSetpoint(setpoint.getAsDouble());
+        }).until();
-                
-                }, 
-                () -> {
-                    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));
 
-        } 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 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(() -> {
-            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());
+            }).until();
-        }
     }   
 
     /**
@@ -349,11 +225,11 @@ public class Elevator extends SubsystemBase {
     }
 
     public double getPIDUpSetpoint() {
-        return pidControllerUp.getSetpoint();
+        return pidController.getSetpoint();
     }
 
     public double getPIDUpError() {
-        return pidControllerUp.getError();
+        return pidController.getError();
     }
 
     public double getPIDDownSetpoint() {