feat: ElevatorIOSim :)

src/main/java/org/team1540/robot2024/Constants.java

@@ -62,21 +62,23 @@ public static class Elevator {
         public static final double ELEVATOR_MINIMUM_HEIGHT = Units.inchesToMeters(27.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 GEAR_RATIO = -1;
-        public static final int talonId1 = -1;
-        public static final int talonId2 = -1;
+
+        public static final double ELEVATOR_GEAR_RATIO = -1;
+        public static final int TALON_ID_1 = -1;
+        public static final int TALON_ID_2 = -1;
         public static final double KS = 0.25;
         public static final double KV = 0.12;
         public static final double KA = 0.01;
         public static final double KP = 4.8;
         public static final double KI = 0;
         public static final double KD = 0.1;
         public static final double KG = 0;
-        public static final int motionMagicCruiseVelocity = 80;
-        public static final int motionMagicAcceleration = 160;
-        public static final int motionMagicJerk = 1600;
-        public static final double SOCKET_DIAMETER = -1;       
+        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 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
 
         public enum ElevatorState {
             /**

src/main/java/org/team1540/robot2024/subsystems/elevator/Elevator.java

@@ -29,7 +29,7 @@ public void goToSetpoint(double setpointMeters) {
     }
 
     public boolean isAtSetpoint() {
-        return elevatorInputs.positionRots == setpointRots;
+        return elevatorInputs.positionMeters == setpointRots;
     }
 
     public void stop() {

src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIO.java

@@ -5,7 +5,7 @@
 public interface ElevatorIO {
     @AutoLog
     class ElevatorIOInputs {
-        public double positionRots = 0.0;
+        public double positionMeters = 0.0;
         public double velocityRPM = 0.0;
         public double voltage = 0.0;
         public double[] current = new double[] {};

src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOSim.java

@@ -0,0 +1,30 @@
+package org.team1540.robot2024.subsystems.elevator;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.plant.DCMotor;
+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 double elevatorAppliedVolts = 0.0;
+
+    @Override
+    public void updateInputs(ElevatorIOInputs inputs){
+        elevatorSim.update(LOOP_PERIOD_SECS);
+
+        inputs.positionMeters = elevatorSim.getPositionMeters();
+        inputs.velocityRPM = elevatorSim.getVelocityMetersPerSecond();
+        inputs.voltage = elevatorAppliedVolts;
+        inputs.current = new double[]{elevatorSim.getCurrentDrawAmps()};
+        inputs.upperLimit = elevatorSim.hasHitUpperLimit();
+        inputs.lowerLimit = elevatorSim.hasHitLowerLimit();
+    }
+
+    public void setElevatorAppliedVolts(double volts){
+        elevatorAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); //TODO: check this range
+        elevatorSim.setInputVoltage(elevatorAppliedVolts);
+    }
+}

src/main/java/org/team1540/robot2024/subsystems/elevator/ElevatorIOTalonFX.java

@@ -14,13 +14,11 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.ReverseLimitValue;
 
-import edu.wpi.first.wpilibj.DigitalInput;
-
 public class ElevatorIOTalonFX implements ElevatorIO {
 
     private final MotionMagicVoltage motionMagicOut = new MotionMagicVoltage(0).withEnableFOC(true);
-    private final TalonFX leader = new TalonFX(Constants.Elevator.talonId1);
-    private final TalonFX follower = new TalonFX(Constants.Elevator.talonId2);
+    private final TalonFX leader = new TalonFX(Constants.Elevator.TALON_ID_1);
+    private final TalonFX follower = new TalonFX(Constants.Elevator.TALON_ID_2);
 
     private final StatusSignal<Double> leaderPosition = leader.getPosition();
     private final StatusSignal<Double> leaderVelocity = leader.getVelocity();
@@ -38,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.GEAR_RATIO;
+        config.Feedback.SensorToMechanismRatio = Constants.Elevator.ELEVATOR_GEAR_RATIO;
         // TODO: this might not actually be inverted, so fix it if neccesary
         follower.setControl(new Follower(leader.getDeviceID(), true));
 
@@ -59,9 +57,9 @@ public ElevatorIOTalonFX() {
 
         // setting Motion Magic Settings
         MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
-        motionMagicConfigs.MotionMagicCruiseVelocity = Constants.Elevator.motionMagicCruiseVelocity;
-        motionMagicConfigs.MotionMagicAcceleration = Constants.Elevator.motionMagicAcceleration;
-        motionMagicConfigs.MotionMagicJerk = Constants.Elevator.motionMagicJerk;
+        motionMagicConfigs.MotionMagicCruiseVelocity = Constants.Elevator.MOTION_MAGIC_CRUISE_VELOCITY;
+        motionMagicConfigs.MotionMagicAcceleration = Constants.Elevator.MOTION_MAGIC_ACCELERATION;
+        motionMagicConfigs.MotionMagicJerk = Constants.Elevator.MOTION_MAGIC_JERK;
 
         config.HardwareLimitSwitch.ForwardLimitEnable = true;
         config.HardwareLimitSwitch.ReverseLimitEnable = true;
@@ -74,7 +72,7 @@ public ElevatorIOTalonFX() {
     public void updateInputs(ElevatorIOInputs inputs) {
         BaseStatusSignal.refreshAll(leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent, topLimitStatus, bottomLimitStatus);
 
-        inputs.positionRots = leaderPosition.getValue();
+        inputs.positionMeters = leaderPosition.getValue();
         inputs.velocityRPM = leaderVelocity.getValue();
         inputs.voltage = leaderAppliedVolts.getValue();
         inputs.current = new double[] {leaderCurrent.getValue(), followerCurrent.getValue()};