From 89b72adc696fb9a225a575bb95180f8562ba1097 Mon Sep 17 00:00:00 2001 From: "David :D" Date: Tue, 30 Jan 2024 10:43:45 -0800 Subject: [PATCH] fix: ElevatorIOSim has goToSetpoint with PID and Trapazoidal Profiling stuff and Elevator subsystem has AverageFilter for checking if the elevator is at the setpoint :D --- .../org/team1540/robot2024/Constants.java | 24 ++++++------ .../team1540/robot2024/RobotContainer.java | 2 +- .../commands/ElevatorSetpointCommand.java | 29 -------------- .../commands/climb/TrapAndClimbSequence.java | 1 - .../elevator/ElevatorManualCommand.java | 2 +- .../subsystems/elevator/Elevator.java | 23 +++++++---- .../subsystems/elevator/ElevatorIO.java | 6 +-- .../subsystems/elevator/ElevatorIOSim.java | 39 +++++++++++++++++-- .../elevator/ElevatorIOTalonFX.java | 26 +++++++------ .../subsystems/shooter/ShooterPivotIOSim.java | 6 +-- 10 files changed, 85 insertions(+), 73 deletions(-) delete mode 100644 src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 03f30d9c..452a5c6b 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -114,12 +114,12 @@ public static class Pivot { public static class Elevator { public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25); - public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(48.0); - public static final double ELEVATOR_MINIMUM_HEIGHT = Units.inchesToMeters(27.0); + public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(6.0 + 21.0); //TODO: Fix these constants to be more accurate + public static final double ELEVATOR_MINIMUM_HEIGHT = Units.inchesToMeters(6.0); public static final double CLIMBING_HOOKS_MINIMUM_HEIGHT = Units.inchesToMeters(12.0); public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + ELEVATOR_MAX_HEIGHT - ELEVATOR_MINIMUM_HEIGHT; - public static final double ELEVATOR_GEAR_RATIO = 1.0; + public static final double GEAR_RATIO = 2.0 / 1.0; //TODO: Get constants right sometime public static final int TALON_ID_1 = -1; public static final int TALON_ID_2 = -1; public static final double KS = 0.25; @@ -129,10 +129,12 @@ public static class Elevator { public static final double KI = 0; public static final double KD = 0.1; public static final double KG = 0; - public static final int MOTION_MAGIC_CRUISE_VELOCITY = 80; - public static final int MOTION_MAGIC_ACCELERATION = 160; - public static final int MOTION_MAGIC_JERK = 1600; - public static final double SOCKET_DIAMETER = -1; + public static final int CRUISE_VELOCITY_MPS = 80; + public static final int MAXIMUM_ACCELERATION_MPS2 = 160; + public static final int JERK_MP3 = 1600; + public static final double SPROCKET_CIRCUMFERENCE = .0508 * Math.PI; + public static final double MOTOR_ROTS_TO_METERS = GEAR_RATIO * SPROCKET_CIRCUMFERENCE; + public static final double ERROR_TOLERANCE = 0.03; public static final double SIM_CARRIAGE_MASS_KG = 1.55; //TODO: check this number :) public static final double SIM_DRUM_RADIUS_METERS = Units.inchesToMeters(1.1715); //TODO: check this number too @@ -148,15 +150,15 @@ public enum ElevatorState { /** * At height for top of initial climb :D */ - CLIMB(254.0), //TODO: Find these values :D + CLIMB(CHAIN_HEIGHT_METERS + 0.1 - (CLIMBING_HOOKS_MINIMUM_HEIGHT - ELEVATOR_MINIMUM_HEIGHT)), //TODO: Find these values :D /** * At height for trap doing :D */ - TRAP(254.0), //TODO: Find these values :D + TRAP(27.0), //TODO: Find these values :D /** * At height for top of initial climb :D */ - AMP(254.0); //TODO: Find these values :D + AMP(27.0); //TODO: Find these values :D public final double heightMeters; ElevatorState(double heightMeters) { @@ -165,7 +167,7 @@ public enum ElevatorState { } } - public static class Tramp { +public static class Tramp { public static final double AMP_PERCENTAGE = 0.1540; //TODO: Find these values :D public static final double TRAP_PERCENTAGE = 0.1678; //TODO: Find these values :D public static final double TRAP_SCORING_TIME_SECONDS = 1.114; //TODO: Find these values :D diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index a600caf9..4b442afd 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -118,7 +118,7 @@ public RobotContainer() { */ private void configureButtonBindings() { drivetrain.setDefaultCommand(new SwerveDriveCommand(drivetrain, driver)); - elevator.setDefaultCommand(new ElevatorManualCommand(driver, elevator)); // TODO: change back to copilot + elevator.setDefaultCommand(new ElevatorManualCommand(elevator, driver)); driver.x().onTrue(Commands.runOnce(drivetrain::stopWithX, drivetrain)); driver.b().onTrue( Commands.runOnce( diff --git a/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java b/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java deleted file mode 100644 index d5f7069a..00000000 --- a/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.team1540.robot2024.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import org.team1540.robot2024.Constants.Elevator.ElevatorState; -import org.team1540.robot2024.subsystems.elevator.Elevator; - -public class ElevatorSetpointCommand extends Command { - private final Elevator elevator; - private final ElevatorState state; - public ElevatorSetpointCommand(Elevator elevator, ElevatorState state) { - this.elevator = elevator; - this.state = state; - addRequirements(elevator); - } - @Override - public void initialize() { - elevator.goToSetpoint(state.heightMeters); - } - - @Override - public void end(boolean interrupted) { - elevator.stop(); - } - @Override - public boolean isFinished() { - return elevator.isAtSetpoint(); - } - -} diff --git a/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java index 6f55451c..9b08d7f9 100644 --- a/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java +++ b/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java @@ -1,7 +1,6 @@ package org.team1540.robot2024.commands.climb; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import org.team1540.robot2024.Constants; diff --git a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java b/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java index 9415292e..ac205d6e 100644 --- a/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java +++ b/src/main/java/org/team1540/robot2024/commands/elevator/ElevatorManualCommand.java @@ -11,7 +11,7 @@ public class ElevatorManualCommand extends Command { private final Elevator elevator; private final CommandXboxController copilot; - public ElevatorManualCommand(CommandXboxController copilot, Elevator elevator) { + public ElevatorManualCommand(Elevator elevator, CommandXboxController copilot) { this.elevator = elevator; this.copilot = copilot; addRequirements(elevator); diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java index d2b1c9ff..4364129a 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java @@ -1,14 +1,19 @@ package org.team1540.robot2024.subsystems.elevator; +import edu.wpi.first.math.MathUtil; import org.littletonrobotics.junction.Logger; -import org.team1540.robot2024.Constants; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.team1540.robot2024.util.MechanismVisualiser; +import org.team1540.robot2024.util.math.AverageFilter; + +import static org.team1540.robot2024.Constants.Elevator.*; public class Elevator extends SubsystemBase { private final ElevatorIO elevatorIO; private final ElevatorIOInputsAutoLogged elevatorInputs = new ElevatorIOInputsAutoLogged(); - private double setpointRots; + private final AverageFilter elevatorPositionFilter = new AverageFilter(10); + private double setpointMeters; public Elevator(ElevatorIO elevatorIO) { @@ -20,16 +25,20 @@ public Elevator(ElevatorIO elevatorIO) { public void periodic(){ elevatorIO.updateInputs(elevatorInputs); Logger.processInputs("Elevator", elevatorInputs); + MechanismVisualiser.setElevatorPosition(elevatorInputs.positionMeters); + elevatorPositionFilter.add(elevatorInputs.positionMeters); } - public void goToSetpoint(double setpointMeters) { - this.setpointRots = setpointMeters / Constants.Elevator.SOCKET_DIAMETER; - elevatorIO.setSetpoint(setpointRots); + public void goToSetpoint(double newSetpointMeters) { + setpointMeters = newSetpointMeters; + elevatorIO.setPositionMeters(setpointMeters); + + elevatorPositionFilter.clear(); } public boolean isAtSetpoint() { - return elevatorInputs.positionMeters == setpointRots; + return MathUtil.isNear(setpointMeters, elevatorPositionFilter.getAverage(), ERROR_TOLERANCE); } public void setVoltage(double voltage) { @@ -37,6 +46,6 @@ public void setVoltage(double voltage) { } public void stop() { - elevatorIO.stop(); + elevatorIO.setVoltage(0.0); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java index b1f2e49f..6dd591f9 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java @@ -15,13 +15,9 @@ class ElevatorIOInputs { default void updateInputs(ElevatorIOInputs inputs) {} - default void setSetpoint(double position) {} + default void setPositionMeters(double position) {} default void setVoltage(double voltage) {} - - default void stop() {} - - } diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java index cf75eff1..0828869a 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java @@ -1,18 +1,45 @@ package org.team1540.robot2024.subsystems.elevator; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.simulation.ElevatorSim; import static org.team1540.robot2024.Constants.Elevator.*; import static org.team1540.robot2024.Constants.LOOP_PERIOD_SECS; public class ElevatorIOSim implements ElevatorIO{ // fields - private final ElevatorSim elevatorSim = new ElevatorSim(DCMotor.getFalcon500Foc(2), ELEVATOR_GEAR_RATIO, SIM_CARRIAGE_MASS_KG, SIM_DRUM_RADIUS_METERS, ELEVATOR_MINIMUM_HEIGHT, ELEVATOR_MAX_HEIGHT, true, ELEVATOR_MINIMUM_HEIGHT); + private final ElevatorSim elevatorSim = + new ElevatorSim( + DCMotor.getFalcon500Foc(2), + GEAR_RATIO, SIM_CARRIAGE_MASS_KG, + SIM_DRUM_RADIUS_METERS,ELEVATOR_MINIMUM_HEIGHT, + ELEVATOR_MAX_HEIGHT, + true, + ELEVATOR_MINIMUM_HEIGHT); private double elevatorAppliedVolts = 0.0; + private final ProfiledPIDController controller = + new ProfiledPIDController( + KP, + KI, + KD, + new TrapezoidProfile.Constraints(CRUISE_VELOCITY_MPS, MAXIMUM_ACCELERATION_MPS2)); + private final ElevatorFeedforward feedforward = new ElevatorFeedforward(KS, KG, KV); + private boolean isClosedLoop; + private TrapezoidProfile.State setpoint; @Override public void updateInputs(ElevatorIOInputs inputs){ + if (isClosedLoop) { + elevatorAppliedVolts = + controller.calculate(elevatorSim.getPositionMeters(), setpoint) + + feedforward.calculate( + controller.getSetpoint().position, + controller.getSetpoint().velocity); + } + elevatorSim.setInputVoltage(elevatorAppliedVolts); elevatorSim.update(LOOP_PERIOD_SECS); inputs.positionMeters = elevatorSim.getPositionMeters(); @@ -23,8 +50,14 @@ public void updateInputs(ElevatorIOInputs inputs){ inputs.lowerLimit = elevatorSim.hasHitLowerLimit(); } - public void setVoltage(double volts){ + @Override + public void setVoltage(double volts) { + isClosedLoop = false; elevatorAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); //TODO: check this range - elevatorSim.setInputVoltage(elevatorAppliedVolts); + } + @Override + public void setPositionMeters(double position) { + isClosedLoop = true; + setpoint = new TrapezoidProfile.State(position, 0.0); } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java index 75057132..f4716d84 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java @@ -36,7 +36,7 @@ public ElevatorIOTalonFX() { config.CurrentLimits.SupplyCurrentThreshold = 60.0; config.CurrentLimits.SupplyTimeThreshold = 0.1; config.MotorOutput.NeutralMode = NeutralModeValue.Coast; - config.Feedback.SensorToMechanismRatio = Constants.Elevator.ELEVATOR_GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = Constants.Elevator.MOTOR_ROTS_TO_METERS; // TODO: this might not actually be inverted, so fix it if neccesary follower.setControl(new Follower(leader.getDeviceID(), true)); @@ -57,9 +57,9 @@ public ElevatorIOTalonFX() { // setting Motion Magic Settings MotionMagicConfigs motionMagicConfigs = config.MotionMagic; - motionMagicConfigs.MotionMagicCruiseVelocity = Constants.Elevator.MOTION_MAGIC_CRUISE_VELOCITY; - motionMagicConfigs.MotionMagicAcceleration = Constants.Elevator.MOTION_MAGIC_ACCELERATION; - motionMagicConfigs.MotionMagicJerk = Constants.Elevator.MOTION_MAGIC_JERK; + motionMagicConfigs.MotionMagicCruiseVelocity = Constants.Elevator.CRUISE_VELOCITY_MPS; + motionMagicConfigs.MotionMagicAcceleration = Constants.Elevator.MAXIMUM_ACCELERATION_MPS2; + motionMagicConfigs.MotionMagicJerk = Constants.Elevator.JERK_MP3; config.HardwareLimitSwitch.ForwardLimitEnable = true; config.HardwareLimitSwitch.ReverseLimitEnable = true; @@ -70,7 +70,14 @@ public ElevatorIOTalonFX() { @Override public void updateInputs(ElevatorIOInputs inputs) { - BaseStatusSignal.refreshAll(leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent, topLimitStatus, bottomLimitStatus); + BaseStatusSignal.refreshAll( + leaderPosition, + leaderVelocity, + leaderAppliedVolts, + leaderCurrent, + followerCurrent, + topLimitStatus, + bottomLimitStatus); inputs.positionMeters = leaderPosition.getValue(); inputs.velocityRPM = leaderVelocity.getValue(); @@ -81,17 +88,12 @@ public void updateInputs(ElevatorIOInputs inputs) { } @Override - public void setSetpoint(double position) { - leader.setControl(motionMagicOut.withPosition(position)); + public void setPositionMeters(double positionMeters) { + leader.setControl(motionMagicOut.withPosition(positionMeters)); } @Override public void setVoltage(double voltage) { leader.set(voltage*12); } - - @Override - public void stop() { - leader.set(0); - } } diff --git a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java index 7c8d0cb4..89b6ae75 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java +++ b/src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java @@ -28,7 +28,7 @@ public class ShooterPivotIOSim implements ShooterPivotIO { private final ArmFeedforward feedforward = new ArmFeedforward(KS, KG, KV); private boolean isClosedLoop; - private TrapezoidProfile.State goalState; + private TrapezoidProfile.State setpoint; private double appliedVolts; @@ -36,7 +36,7 @@ public class ShooterPivotIOSim implements ShooterPivotIO { public void updateInputs(ShooterPivotIOInputs inputs) { if (isClosedLoop) { appliedVolts = - controller.calculate(Units.radiansToRotations(sim.getAngleRads()), goalState) + controller.calculate(Units.radiansToRotations(sim.getAngleRads()), setpoint) + feedforward.calculate( Units.rotationsToRadians(controller.getSetpoint().position), controller.getSetpoint().velocity); @@ -58,7 +58,7 @@ public void setPosition(Rotation2d position) { Units.radiansToRotations(sim.getVelocityRadPerSec()) ); isClosedLoop = true; - goalState = new TrapezoidProfile.State(position.getRadians(), 0); + setpoint = new TrapezoidProfile.State(position.getRadians(), 0); } @Override