Sim elevator with CTRE code

src/main/java/org/curtinfrc/frc2025/Constants.java

@@ -15,7 +15,7 @@
  * (log replay from a file).
  */
 public final class Constants {
-  public static final RobotType robotType = RobotType.COMPBOT;
+  public static final RobotType robotType = RobotType.SIMBOT;
   public static final double ROBOT_X = 0.705;
   public static final double ROBOT_Y = 0.730;
   public static final double FIELD_LENGTH = aprilTagLayout.getFieldLength();

src/main/java/org/curtinfrc/frc2025/Robot.java

@@ -273,6 +273,26 @@ public Robot() {
             drive,
             drive::logTrajectory);
 
+    autoChooser.addCmd(
+        "L3 No end", () -> elevator.goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate()));
+
+    autoChooser.addCmd(
+        "L2 No end", () -> elevator.goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate()));
+
+    autoChooser.addCmd(
+        "L3 End",
+        () ->
+            elevator
+                .goToSetpoint(ElevatorSetpoints.L3, intake.backSensor.negate())
+                .until(elevator.atSetpoint));
+
+    autoChooser.addCmd(
+        "L2 End",
+        () ->
+            elevator
+                .goToSetpoint(ElevatorSetpoints.L2, intake.backSensor.negate())
+                .until(elevator.atSetpoint));
+
     autoChooser.addRoutine("Test Path", () -> Autos.path("Test Path", factory, drive));
     autoChooser.addRoutine(
         "Three Piece Left", () -> Autos.threePieceLeft(factory, drive, ejector, elevator, intake));

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java

@@ -19,8 +19,7 @@
 public class Elevator extends SubsystemBase {
   private final ElevatorIO io;
   private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
-  private final PIDController pid = new PIDController(kP, 0, kD);
-  private final PIDController climbPID = new PIDController(climbkP, climbkI, climbkD);
+  private final PIDController climbPID = new PIDController(45, 0, 0);
   private ElevatorSetpoints setpoint = ElevatorSetpoints.BASE;
 
   public final Trigger isNotAtCollect = new Trigger(() -> setpoint != ElevatorSetpoints.BASE);
@@ -29,14 +28,7 @@ public class Elevator extends SubsystemBase {
       new Trigger(() -> setpoint != ElevatorSetpoints.BASE && setpoint != ElevatorSetpoints.L1);
 
   public final Trigger toZero = new Trigger(() -> inputs.hominSensor);
-  public final Trigger atSetpoint =
-      new Trigger(
-          () -> {
-            var refrence = setpoint.setpoint / (Math.PI * 2 * 0.03055) * 8.1818;
-            var current = inputs.positionRotations;
-
-            return (refrence - current) < 0.85248701367;
-          });
+  public final Trigger atSetpoint;
 
   @AutoLogOutput(key = "Climber/ElevatorAtSetpoint")
   public final Trigger atClimbSetpoint = new Trigger(climbPID::atSetpoint);
@@ -49,8 +41,21 @@ public class Elevator extends SubsystemBase {
 
   public Elevator(ElevatorIO io) {
     this.io = io;
-    pid.setTolerance(tolerance);
-    climbPID.setTolerance(tolerance);
+    climbPID.setTolerance(0.01);
+
+    atSetpoint =
+        new Trigger(
+            () -> {
+              var referencePos = io.metresToRotations(setpoint.setpoint);
+              var posTolerance = io.metresToRotations(0.005);
+              var currentPos = inputs.positionRotations;
+
+              var velTolerance = io.metresToRotations(0.3);
+              var currentVel = inputs.angularVelocityRotationsPerSecond;
+
+              return Math.abs(referencePos - currentPos) < posTolerance
+                  && Math.abs(0 - currentVel) < velTolerance;
+            });
   }
 
   @Override
@@ -60,7 +65,6 @@ public void periodic() {
     Logger.recordOutput("Elevator/isNotAtCollect", isNotAtCollect.getAsBoolean());
     Logger.recordOutput("Elevator/setpoint", ElevatorSetpoints.struct, setpoint);
     Logger.recordOutput("Elevator/AtSetpoint", atSetpoint.getAsBoolean());
-    Logger.recordOutput("Elevator/ActualError", pid.getError());
 
     // if (inputs.hominSensor) {
     //   io.zero();
@@ -100,7 +104,7 @@ public Command goToClimberSetpoint(ElevatorSetpoints point, BooleanSupplier safe
               setpoint = point;
               var out =
                   climbPID.calculate(
-                      io.positionRotationsToMetres(inputs.positionRotations), setpoint.setpoint);
+                      io.rotationsToMetres(inputs.positionRotations), setpoint.setpoint);
               io.setVoltage(MathUtil.clamp(out, -4, 4));
             }),
         Commands.none(),
@@ -120,7 +124,7 @@ public Pose3d getHeight() {
     return new Pose3d(
         0,
         0,
-        io.positionRotationsToMetres(inputs.positionRotations),
+        io.rotationsToMetres(inputs.positionRotations),
         new Rotation3d(Math.PI / 2, 0, Math.PI / 2));
   }
 }

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorConstants.java

@@ -5,18 +5,6 @@
 import edu.wpi.first.util.struct.StructSerializable;
 
 public class ElevatorConstants {
-  // TODO: TUNE PID and ff
-  public static double tolerance = 0.021;
-  public static double kP = 19;
-  public static double kD = 0;
-  public static double kV = 0;
-  public static double kA = 0.01;
-  public static double kG = 0;
-
-  public static double climbkP = 45;
-  public static double climbkI = 0;
-  public static double climbkD = 0;
-
   public static enum ElevatorSetpoints implements StructSerializable {
     L1(0.1),
     L2(0.221),

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java

@@ -9,7 +9,7 @@ public static class ElevatorIOInputs {
     public double appliedVolts;
     public double currentAmps;
     public double positionRotations;
-    public double angularVelocityRotationsPerMinute;
+    public double angularVelocityRotationsPerSecond;
     public boolean hominSensor;
   }
 
@@ -21,11 +21,11 @@ public default void setPosition(double positionMetres) {}
 
   public default void zero() {}
 
-  public default double positionRotationsToMetres(double rotations) {
+  public default double rotationsToMetres(double rotations) {
     return rotations;
   }
 
-  public default double positionMetresToRotations(double metres) {
+  public default double metresToRotations(double metres) {
     return metres;
   }
 }

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOComp.java

@@ -4,14 +4,12 @@
 
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.GravityTypeValue;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.units.measure.Angle;
@@ -26,17 +24,9 @@ public class ElevatorIOComp implements ElevatorIO {
   private static final double gearing = 8.1818;
   private static final int ID = 7;
   private static final int FOLLOWER_ID = 40;
-  private static final TalonFXConfiguration config =
-      new TalonFXConfiguration()
-          .withMotorOutput(
-              new MotorOutputConfigs()
-                  .withInverted(InvertedValue.CounterClockwise_Positive)
-                  .withNeutralMode(NeutralModeValue.Brake))
-          .withSlot0(new Slot0Configs().withKP(0.4))
-          .withCurrentLimits(
-              new CurrentLimitsConfigs().withSupplyCurrentLimit(30).withStatorCurrentLimit(60));
-
-  private final TalonFX motor = new TalonFX(ID);
+  private final TalonFXConfiguration config;
+
+  protected final TalonFX motor = new TalonFX(ID);
   private final TalonFX follower = new TalonFX(FOLLOWER_ID);
 
   private DigitalInput reset = new DigitalInput(0);
@@ -47,10 +37,39 @@ public class ElevatorIOComp implements ElevatorIO {
   private final StatusSignal<AngularVelocity> velocity = motor.getVelocity();
 
   private final VoltageOut voltageRequest = new VoltageOut(0).withEnableFOC(true);
-  private final PositionVoltage positionRequest = new PositionVoltage(0).withEnableFOC(true);
+  private final MotionMagicVoltage positionRequest = new MotionMagicVoltage(0).withEnableFOC(true);
   private final Follower followRequest = new Follower(ID, false);
 
   public ElevatorIOComp() {
+    config = new TalonFXConfiguration();
+    var output = config.MotorOutput;
+    output.Inverted = InvertedValue.CounterClockwise_Positive;
+    output.NeutralMode = NeutralModeValue.Brake;
+
+    var slot0 = config.Slot0;
+    slot0.GravityType = GravityTypeValue.Elevator_Static;
+    // we lie about units cause of how the dimensional analysis works out
+    // rotations = metres / value
+    // rotations * value = metres
+    // V/rotations = V/metres/value
+    // V/rotations = V*value/metres
+    // so rotationsToMetres does the correct thing here
+    slot0.kG = 0.09;
+    slot0.kP = rotationsToMetres(318.70);
+    slot0.kD = rotationsToMetres(1.33);
+    slot0.kV = rotationsToMetres(5.29);
+    slot0.kV = rotationsToMetres(0.01);
+
+    var currentLimits = config.CurrentLimits;
+    currentLimits.StatorCurrentLimitEnable = true;
+    currentLimits.StatorCurrentLimit = 60;
+    currentLimits.SupplyCurrentLimitEnable = true;
+    currentLimits.SupplyCurrentLimit = 30;
+
+    var mmConfig = config.MotionMagic;
+    mmConfig.MotionMagicCruiseVelocity = metresToRotations(2.24);
+    mmConfig.MotionMagicAcceleration = metresToRotations(49.00);
+
     tryUntilOk(5, () -> motor.getConfigurator().apply(config));
     tryUntilOk(5, () -> follower.getConfigurator().apply(config));
     BaseStatusSignal.setUpdateFrequencyForAll(20.0, velocity, voltage, current, position);
@@ -66,7 +85,7 @@ public void updateInputs(ElevatorIOInputs inputs) {
     inputs.appliedVolts = voltage.getValueAsDouble();
     inputs.currentAmps = current.getValueAsDouble();
     inputs.positionRotations = position.getValueAsDouble();
-    inputs.angularVelocityRotationsPerMinute = velocity.getValueAsDouble();
+    inputs.angularVelocityRotationsPerSecond = velocity.getValueAsDouble();
     inputs.hominSensor = reset.get();
   }
 
@@ -78,7 +97,7 @@ public void setVoltage(double volts) {
 
   @Override
   public void setPosition(double positionMetres) {
-    positionRequest.withPosition(positionMetresToRotations(positionMetres));
+    positionRequest.withPosition(metresToRotations(positionMetres));
     motor.setControl(positionRequest);
   }
 
@@ -88,12 +107,12 @@ public void zero() {
   }
 
   @Override
-  public double positionRotationsToMetres(double rotations) {
+  public double rotationsToMetres(double rotations) {
     return rotations * Math.PI * 2 * pulleyRadiusMeters / gearing;
   }
 
   @Override
-  public double positionMetresToRotations(double metres) {
+  public double metresToRotations(double metres) {
     return metres / (Math.PI * 2 * pulleyRadiusMeters) * gearing;
   }
 }

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIOSim.java

@@ -1,28 +1,49 @@
 package org.curtinfrc.frc2025.subsystems.elevator;
 
-import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.kA;
-import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.kV;
-
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.simulation.ElevatorSim;
 
-public class ElevatorIOSim implements ElevatorIO {
+public class ElevatorIOSim extends ElevatorIOComp {
   private final ElevatorSim elevatorSim =
-      new ElevatorSim(kV, kA, DCMotor.getNEO(1), 0, 0.6, true, 0);
-  private double volts = 0;
+      new ElevatorSim(5.29, 0.01, DCMotor.getKrakenX60Foc(2), 0, 0.7, true, 0, 0.0005, 0.005);
 
-  @Override
-  public void updateInputs(ElevatorIOInputs inputs) {
+  private double lastSimTime;
+  private final Notifier notifier;
+
+  public ElevatorIOSim() {
+    super();
+    lastSimTime = RobotController.getFPGATime();
+
+    notifier =
+        new Notifier(
+            () -> {
+              final double currentTime = RobotController.getFPGATime();
+              double deltaTime = currentTime - lastSimTime;
+              lastSimTime = currentTime;
+
+              updateSimState(deltaTime);
+            });
+    notifier.startPeriodic(0.001);
+  }
+
+  private void updateSimState(double deltaTime) {
+    var simState = motor.getSimState();
+    elevatorSim.setInput(simState.getMotorVoltage());
     elevatorSim.update(0.02);
-    // ignore velocity cause we dont care
-    inputs.currentAmps = elevatorSim.getCurrentDrawAmps();
-    inputs.appliedVolts = volts;
-    inputs.positionRotations = elevatorSim.getPositionMeters();
+    var rotorPosition = metresToRotations(elevatorSim.getPositionMeters());
+    simState.setRawRotorPosition(rotorPosition);
+    var rotorVel = metresToRotations(elevatorSim.getVelocityMetersPerSecond());
+    simState.setRotorVelocity(rotorVel);
   }
 
   @Override
-  public void setVoltage(double volts) {
-    this.volts = volts;
-    elevatorSim.setInputVoltage(volts);
+  public void updateInputs(ElevatorIOInputs inputs) {
+    super.updateInputs(inputs);
+    final double currentTime = RobotController.getFPGATime();
+    double deltaTime = currentTime - lastSimTime;
+    lastSimTime = currentTime;
+    updateSimState(deltaTime);
   }
 }