use ctre sim for elevator

src/main/java/frc/robot/Robot.java

@@ -50,8 +50,8 @@
 import frc.robot.cancoder.CANcoderIOReal;
 import frc.robot.canrange.CANrangeIOReal;
 import frc.robot.climber.ClimberSubsystem;
+import frc.robot.elevator.ElevatorIOCTRESim;
 import frc.robot.elevator.ElevatorIOReal;
-import frc.robot.elevator.ElevatorIOSim;
 import frc.robot.elevator.ElevatorSubsystem;
 import frc.robot.intake.IntakeSubsystem;
 import frc.robot.led.LEDIOReal;
@@ -161,7 +161,7 @@ public static enum ScoringSide {
   // Instantiate subsystems
   private final ElevatorSubsystem elevator =
       new ElevatorSubsystem(
-          ROBOT_TYPE != RobotType.SIM ? new ElevatorIOReal() : new ElevatorIOSim());
+          ROBOT_TYPE != RobotType.SIM ? new ElevatorIOReal() : new ElevatorIOCTRESim());
 
   TalonFXConfiguration armRollerConfig =
       createRollerConfig(InvertedValue.Clockwise_Positive, 20.0, 6.62, 0.48, 0.25, 0.0);
@@ -577,6 +577,8 @@ public Robot() {
         intake.ninety().alongWith(Commands.print("dashboard ninety intake")).ignoringDisable(true));
     SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true));
 
+    SmartDashboard.putData("test elevator", elevator.setExtensionMeters(() -> 1));
+
     manualArmRezeroAlert =
         new Alert(
             "Arm has been manually rezeroed at least once this match. Arm cancoder may not be working!",

src/main/java/frc/robot/elevator/ElevatorIOCTRESim.java

@@ -0,0 +1,87 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.elevator;
+
+import com.ctre.phoenix6.sim.ChassisReference;
+import com.ctre.phoenix6.sim.TalonFXSimState;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+
+/** Add your docs here. */
+public class ElevatorIOCTRESim extends ElevatorIOReal {
+  TalonFXSimState leaderTalonSim;
+  TalonFXSimState followerTalonSim;
+
+  private final ElevatorSim elevatorPhysicsSim =
+      new ElevatorSim(
+          //   DCMotor.getKrakenX60Foc(2),
+          // for 2 kraken x44s
+          new DCMotor(
+              12.0,
+              4.05,
+              275,
+              1.4,
+              Units.rotationsPerMinuteToRadiansPerSecond(7530.0),
+              2), // not sure if this is supposed to be at
+          // 12v?
+          ElevatorSubsystem.GEAR_RATIO,
+          // Add half of first stage mass bc its on a 2:1 ratio compared to carriage
+          // First stage weighs 3.345 lbs
+          // Carriage weighs 8.863
+          // Arm weighs 5.625
+          Units.lbsToKilograms((3.345 / 2) + 8.863 + 5.625),
+          ElevatorSubsystem.SPROCKET_DIAMETER_METERS / 2,
+          0.0,
+          ElevatorSubsystem.MAX_EXTENSION_METERS,
+          true,
+          0.0);
+
+  public ElevatorIOCTRESim() {
+    super();
+    leaderTalonSim = leader.getSimState();
+    leaderTalonSim.Orientation = ChassisReference.Clockwise_Positive;
+    followerTalonSim = follower.getSimState();
+    followerTalonSim.Orientation = ChassisReference.Clockwise_Positive;
+  }
+
+  public void updateInputs(ElevatorIOInputs inputs) {
+    if (DriverStation.isDisabled()) {
+      stop();
+    }
+    // set the supply voltage of the motor
+    leaderTalonSim.setSupplyVoltage(RobotController.getBatteryVoltage());
+    elevatorPhysicsSim.setInputVoltage(leaderTalonSim.getMotorVoltage());
+    elevatorPhysicsSim.update(0.020); // assume 20 ms loop time
+
+    // update the ctre motor sim to match the wpilib physics sim
+    // note that "set" does not command the motor to do anything
+    // it just updates the value the ctre sim has stored
+
+    // convert meters -> rotations for rotor position because it doesn't let you set linear position
+    // directly
+    leaderTalonSim.setRawRotorPosition(
+        elevatorPhysicsSim.getPositionMeters()
+            / (Math.PI * ElevatorSubsystem.SPROCKET_DIAMETER_METERS)
+            * ElevatorSubsystem.GEAR_RATIO);
+    // convert meters/second -> rotations/second
+    leaderTalonSim.setRotorVelocity(
+        elevatorPhysicsSim.getVelocityMetersPerSecond()
+            * (Math.PI * ElevatorSubsystem.SPROCKET_DIAMETER_METERS)
+            / ElevatorSubsystem.GEAR_RATIO);
+
+    followerTalonSim.setRawRotorPosition(
+        elevatorPhysicsSim.getPositionMeters()
+            / (Math.PI * ElevatorSubsystem.SPROCKET_DIAMETER_METERS)
+            * ElevatorSubsystem.GEAR_RATIO);
+    // setting the follower velocity does not seem to do anything
+
+    // updates the values in the logtable
+    // it will pull those values from the motor (same as irl)
+    super.updateInputs(inputs);
+  }
+}

src/main/java/frc/robot/elevator/ElevatorIOReal.java

@@ -14,10 +14,29 @@
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Temperature;
 import edu.wpi.first.units.measure.Voltage;
+import org.littletonrobotics.junction.AutoLog;
+
+public class ElevatorIOReal {
+
+  @AutoLog
+  public static class ElevatorIOInputs {
+    public double leaderPositionMeters = 0.0;
+    public double leaderVelocityMetersPerSec = 0.0;
+    public double leaderStatorCurrentAmps = 0.0;
+    public double leaderSupplyCurrentAmps = 0.0;
+    public double leaderVoltage = 0.0;
+    public double leaderTempC = 0.0;
+
+    public double followerPositionMeters = 0.0;
+    public double followerVelocityMetersPerSec = 0.0;
+    public double followerStatorCurrentAmps = 0.0;
+    public double followerSupplyCurrentAmps = 0.0;
+    public double followerVoltage = 0.0;
+    public double followerTempC = 0.0;
+  }
 
-public class ElevatorIOReal implements ElevatorIO {
-  private TalonFX leader = new TalonFX(10, "*");
-  private TalonFX follower = new TalonFX(11, "*");
+  protected TalonFX leader = new TalonFX(10, "*");
+  protected TalonFX follower = new TalonFX(11, "*");
 
   // Conversion from angle to distance happens in sensor to mechanism ratio
   private final BaseStatusSignal leaderPositionMeters = leader.getPosition();
@@ -38,6 +57,9 @@ public class ElevatorIOReal implements ElevatorIO {
   private DynamicMotionMagicVoltage motionMagicVoltage;
   private TorqueCurrentFOC torqueCurrent = new TorqueCurrentFOC(0.0);
 
+  // this is only for sim
+  protected double positionSetpoint = 0.0;
+
   public ElevatorIOReal() {
     TalonFXConfiguration config = new TalonFXConfiguration();
 
@@ -98,7 +120,6 @@ public ElevatorIOReal() {
     follower.optimizeBusUtilization();
   }
 
-  @Override
   public void updateInputs(ElevatorIOInputs inputs) {
     BaseStatusSignal.refreshAll(
         leaderPositionMeters,
@@ -129,25 +150,26 @@ public void updateInputs(ElevatorIOInputs inputs) {
     inputs.followerTempC = followerTemp.getValueAsDouble();
   }
 
-  @Override
   public void setVoltage(double volts) {
     leader.setControl(voltageOut.withOutput(volts));
   }
 
-  @Override
   public void setCurrent(double amps) {
     leader.setControl(torqueCurrent.withOutput(amps));
   }
 
-  @Override
   public void setPositionSetpoint(double positionMeters, double acceleration) {
+    positionSetpoint = positionMeters;
     leader.setControl(
         motionMagicVoltage.withPosition(positionMeters).withAcceleration(acceleration));
   }
 
-  @Override
   public void resetEncoder(double position) {
     leader.setPosition(position);
     follower.setPosition(position);
   }
+
+  public void stop() {
+    setVoltage(0.0);
+  }
 }

src/main/java/frc/robot/elevator/ElevatorSubsystem.java

@@ -73,7 +73,7 @@ public double getExtensionMeters() {
     }
   }
 
-  private ElevatorIO io;
+  private ElevatorIOReal io;
   private ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
 
   private LinearFilter currentFilter = LinearFilter.movingAverage(5);
@@ -90,7 +90,7 @@ public double getExtensionMeters() {
   private final SysIdRoutine voltageSysid;
   private final SysIdRoutine currentSysid;
 
-  public ElevatorSubsystem(ElevatorIO io) {
+  public ElevatorSubsystem(ElevatorIOReal io) {
     this.io = io;
     voltageSysid =
         new SysIdRoutine(