From a145c290fd4c54c46d4537b64d278732a6cba2fd Mon Sep 17 00:00:00 2001 From: Tylr-J42 Date: Sat, 22 Feb 2025 02:48:58 -0500 Subject: [PATCH] pid gain scheduling --- src/main/java/frc/robot/RobotContainer.java | 8 +- .../robot/constants/ElevatorConstants.java | 4 +- .../java/frc/robot/subsystems/Drivetrain.java | 9 +- .../java/frc/robot/subsystems/Elevator.java | 115 +++++++++--------- 4 files changed, 68 insertions(+), 68 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6de4d05..ac14107 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -138,6 +138,8 @@ public class RobotContainer { driver.start().and(driver.back()).onTrue( startingConfig() ); + + driver.y().onTrue(drivetrain.zeroHeading()); driver.povDown().whileTrue(climberPivot.runPivot(-0.5)); driver.povUp().whileTrue(climberPivot.runPivot(0.5)); @@ -189,7 +191,6 @@ public class RobotContainer { operator.y().whileTrue( elevator.goToSetpoint(() -> ElevatorConstants.kL2Position) ); - } @@ -245,12 +246,12 @@ public class RobotContainer { sensorTab.addDouble("ElevMotor2", elevator::getMotor2) .withWidget(BuiltInWidgets.kGraph); - sensorTab.addDouble("Elevator setpoint", elevator::getPIDSetpoint) + sensorTab.addDouble("Elevator setpoint", elevator::getPIDUpSetpoint) .withSize(1, 1) .withPosition(5, 0) .withWidget(BuiltInWidgets.kTextView); - sensorTab.addDouble("Elevator error", elevator::getPIDError) + sensorTab.addDouble("Elevator error", elevator::getPIDUpError) .withSize(1, 1) .withPosition(5, 1) .withWidget(BuiltInWidgets.kTextView); @@ -382,7 +383,6 @@ public class RobotContainer { * @param armPosition The target rotation of the arm * @return Moves the elevator and arm to the setpoints */ - @SuppressWarnings("unused") private Command safeMoveManipulator(double elevatorPosition, double armPosition) { return moveManipulatorUtil(elevatorPosition, ManipulatorPivotConstants.kPivotSafeStowPosition, false, true) .andThen(manipulatorPivot.goToSetpoint(() -> armPosition)); diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index c7bd9f7..c345ffe 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -43,14 +43,14 @@ public class ElevatorConstants { public static final double kL1Position = 0; public static final double kL2Position = 14.5; public static final double kL3Position = 29.0; - public static final double kL4Position = 53.0; + public static final double kL4Position = 52.0; public static final double kL4TransitionPosition = 40.0; public static final double kL2AlgaePosition = 22.0; public static final double kL3AlgaePosition = 39.0; public static final double kProcessorPosition = 4.0; /**The position of the top of the elevator brace */ public static final double kBracePosition = 0; - public static final double kMaxHeight = 47.5; //actual is 53 + public static final double kMaxHeight = 52.0; //actual is 52 // 1, 7, 10 are the defaults for these, change as necessary public static final double kSysIDRampRate = .25; diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 3d0698a..b0d6239 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -248,9 +248,12 @@ public class Drivetrain extends SubsystemBase { m_rearRight.resetEncoders(); } - /** Zeroes the heading of the robot. */ - public void zeroHeading() { - gyro.reset(); + /** Zeroes the heading of the robot. + * @return */ + public Command zeroHeading() { + return run(() -> { + gyro.reset(); + }); } public double getGyroValue() { diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 45bbe98..2de584f 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -24,7 +24,8 @@ public class Elevator extends SubsystemBase { private DigitalInput bottomLimitSwitch; - private PIDController pidController; + private PIDController pidControllerUp; + private PIDController pidControllerDown; private ElevatorFeedforward feedForward; @@ -57,14 +58,23 @@ public class Elevator extends SubsystemBase { ElevatorConstants.kBottomLimitSwitchID ); - pidController = new PIDController( + pidControllerDown = new PIDController( ElevatorConstants.kDownControllerP, ElevatorConstants.kDownControllerI, ElevatorConstants.kDownControllerD ); - pidController.setSetpoint(0); + pidControllerDown.setSetpoint(0); - pidController.setTolerance(ElevatorConstants.kAllowedError); + pidControllerDown.setTolerance(ElevatorConstants.kAllowedError); + + pidControllerUp = new PIDController( + ElevatorConstants.kUpControllerP, + ElevatorConstants.kUpControllerI, + ElevatorConstants.kUpControllerD + ); + pidControllerUp.setSetpoint(0); + + pidControllerUp.setTolerance(ElevatorConstants.kAllowedError); feedForward = new ElevatorFeedforward( ElevatorConstants.kFeedForwardS, @@ -132,53 +142,30 @@ public class Elevator extends SubsystemBase { return startRun(() -> { - //pidController.reset(); - // pidController.setSetpoint(encoder.getPosition()); + }, () -> { - /*if (!pidController.atSetpoint()) { - elevatorMotor1.setVoltage( - pidController.calculate( - encoder.getPosition() - ) + feedForward.calculate(0) - ); - } else { - elevatorMotor1.setVoltage( - feedForward.calculate(0) - ); - }*/ - - elevatorMotor1.setVoltage( - feedForward.calculate(0) - ); - }); - - - - - /* - elevatorMotor1.setVoltage( - pidController.calculate( - encoder.getPosition(), - clampedSetpoint - ) + feedForward.calculate(0) - ); -*/ - /* - if (!pidController.atSetpoint()) { - elevatorMotor1.setVoltage( - pidController.calculate( - encoder.getPosition(), - clampedSetpoint - ) + feedForward.calculate(0) - ); + if (!pidControllerUp.atSetpoint()) { + if(encoder.getPosition()>pidControllerUp.getSetpoint()){ + elevatorMotor1.setVoltage( + pidControllerUp.calculate( + encoder.getPosition() + ) + feedForward.calculate(0) + ); + }else{ + elevatorMotor1.setVoltage( + pidControllerDown.calculate( + encoder.getPosition() + ) + feedForward.calculate(0) + ); + } } else { elevatorMotor1.setVoltage( feedForward.calculate(0) ); } - - });*/ + }); + } /** @@ -192,26 +179,28 @@ public class Elevator extends SubsystemBase { return startRun(() -> { - pidController.setSetpoint(setpoint.getAsDouble()); - pidController.reset(); + pidControllerUp.setSetpoint(setpoint.getAsDouble()); + pidControllerDown.setSetpoint(setpoint.getAsDouble()); + pidControllerUp.reset(); + pidControllerDown.reset(); }, () -> { - if (!pidController.atSetpoint()) { + if(setpoint.getAsDouble()>encoder.getPosition()) elevatorMotor1.setVoltage( - pidController.calculate( - encoder.getPosition(), - setpoint.getAsDouble() + pidControllerUp.calculate( + encoder.getPosition() ) + feedForward.calculate(0) ); - } else { + else{ elevatorMotor1.setVoltage( - feedForward.calculate(0) + pidControllerDown.calculate( + encoder.getPosition() + ) + feedForward.calculate(0) ); } - }).until(() -> pidController.atSetpoint()); - - + }).until(() -> pidControllerUp.atSetpoint()); + /* elevatorMotor1.setVoltage( @@ -293,11 +282,19 @@ public class Elevator extends SubsystemBase { return elevatorMotor2.getAppliedOutput()*elevatorMotor2.getBusVoltage(); } - public double getPIDSetpoint() { - return pidController.getSetpoint(); + public double getPIDUpSetpoint() { + return pidControllerUp.getSetpoint(); } - public double getPIDError() { - return pidController.getError(); + public double getPIDUpError() { + return pidControllerUp.getError(); + } + + public double getPIDDownSetpoint() { + return pidControllerDown.getSetpoint(); + } + + public double getPIDDownError() { + return pidControllerDown.getError(); } } \ No newline at end of file