From 42d47d60753150993287d34a82c2e561cb84a5d0 Mon Sep 17 00:00:00 2001 From: Tylr-J42 <84476781+Tylr-J42@users.noreply.github.com> Date: Mon, 19 May 2025 18:16:57 -0400 Subject: [PATCH] in the midst of making controller based pid control --- .../robot/constants/ElevatorConstants.java | 17 +- .../java/frc/robot/subsystems/Elevator.java | 172 +++--------------- 2 files changed, 38 insertions(+), 151 deletions(-) diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index d66e731..559b9d2 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/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 kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2 + public static final double kMaxVelocity = 100.0; // 120 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 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); } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index e7d8948..2f79d13 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/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 pidControllerDown; + private PIDController pidController; 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(() -> { - - 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); + }, () -> { + TrapezoidProfile.State setpoint = trapProfile.calculate(0.02, new TrapezoidProfile.State(encoder.getPosition(), encoder.getVelocity()), goal); + + + }).until(); - } 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(() -> { - - 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) - ); - } + return startRun(() -> { + - }).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() {