fix: ElevatorIOSim has goToSetpoint with PID and Trapazoidal Profilin…

…g stuff and Elevator subsystem has AverageFilter for checking if the elevator is at the setpoint :D

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

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(

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;

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);

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,23 +25,27 @@ 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) {
         elevatorIO.setVoltage(voltage);
     }
 
     public void stop() {
-        elevatorIO.stop();
+        elevatorIO.setVoltage(0.0);
     }
 }

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() {}
-
-
 }
 
 

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);
     }
 }

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);
-    }
 }

src/main/java/org/team1540/robot2024/subsystems/shooter/ShooterPivotIOSim.java

@@ -28,15 +28,15 @@ 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;
 
     @Override
     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