IntakeExtension cleanups

src/main/java/com/team2813/Constants.java

@@ -64,4 +64,8 @@ public static boolean onRed() {
     return DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue)
         == DriverStation.Alliance.Red;
   }
+
+  private Constants() {
+    throw new AssertionError("Not instantiable");
+  }
 }

src/main/java/com/team2813/RobotContainer.java

@@ -20,10 +20,7 @@
 import com.team2813.subsystems.drive.ModuleIOSim;
 import com.team2813.subsystems.drive.ModuleIOTalonFX;
 import com.team2813.subsystems.hopper.*;
-import com.team2813.subsystems.intakeextension.IntakeExtension;
-import com.team2813.subsystems.intakeextension.IntakeExtensionIO;
-import com.team2813.subsystems.intakeextension.IntakeExtensionIOReal;
-import com.team2813.subsystems.intakeextension.IntakeExtensionIOSim;
+import com.team2813.subsystems.intakeextension.*;
 import com.team2813.subsystems.intakeroller.IntakeRoller;
 import com.team2813.subsystems.intakeroller.IntakeRollerIO;
 import com.team2813.subsystems.intakeroller.IntakeRollerIOReal;
@@ -174,7 +171,7 @@ public RobotContainer(AllTunerConstants tunerConstants, Mode mode) {
                 new VisionIO() {},
                 new VisionIO() {},
                 new VisionIO() {});
-        intakeExtension = new IntakeExtension(new IntakeExtensionIO() {});
+        intakeExtension = new IntakeExtension(new IntakeExtensionIOStub());
         intakeRoller = new IntakeRoller(new IntakeRollerIO() {});
 
         shooter = new Shooter(new ShooterIO() {});

src/main/java/com/team2813/subsystems/Simulation.java

@@ -2,6 +2,10 @@
 
 import static edu.wpi.first.units.Units.Volts;
 
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.sim.ChassisReference;
+import com.ctre.phoenix6.sim.TalonFXSimState;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.RobotController;
 import org.littletonrobotics.junction.LoggedRobot;
@@ -20,6 +24,32 @@ public static Voltage getMotorSupplyVoltage() {
     return Volts.of(RobotController.getBatteryVoltage());
   }
 
+  /**
+   * Gets the simulated state for the given motor, respecting the orientation.
+   *
+   * @param motor the motor to use to get the sim state; it should be configured
+   * @param motorType the type of the motor to simulate.
+   * @return the sim state of the motor
+   */
+  public static TalonFXSimState getConfiguredSimState(
+      TalonFX motor, TalonFXSimState.MotorType motorType) {
+    TalonFXSimState simState = motor.getSimState();
+    simState.Orientation = getOrientation(motor);
+    simState.setMotorType(motorType);
+    return simState;
+  }
+
+  private static ChassisReference getOrientation(TalonFX motor) {
+    MotorOutputConfigs outputConfigs = new MotorOutputConfigs();
+    // Populate "outputConfigs" with the current configuration of the motor.
+    motor.getConfigurator().refresh(outputConfigs);
+
+    return switch (outputConfigs.Inverted) {
+      case Clockwise_Positive -> ChassisReference.Clockwise_Positive;
+      case CounterClockwise_Positive -> ChassisReference.CounterClockwise_Positive;
+    };
+  }
+
   private Simulation() {
     throw new AssertionError("Not instantiable");
   }

src/main/java/com/team2813/subsystems/intakeextension/IntakeExtension.java

@@ -4,6 +4,7 @@
 import static edu.wpi.first.units.Units.*;
 
 import com.team2813.util.SimulationVisualizer;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj2.command.*;
@@ -16,6 +17,7 @@
  * pinion gear.
  */
 public class IntakeExtension extends SubsystemBase implements AutoCloseable {
+  static final double ACCEPTABLE_ERROR_IN_ROTATIONS = 0.4;
   private final IntakeExtensionIO io;
   private final IntakeExtensionIOInputsAutoLogged replayedInputs =
       new IntakeExtensionIOInputsAutoLogged();
@@ -50,10 +52,8 @@ public void periodic() {
         replayedInputs
             .extenderMotorPosition
             .minus(replayedInputs.extenderMotorSetpoint)
-            .abs(Rotation);
-
-    // Is the error between the setpoint greater than half a rotation.
-    extenderAtPosition = error <= 0.4;
+            .abs(Units.Rotation);
+    extenderAtPosition = error <= ACCEPTABLE_ERROR_IN_ROTATIONS;
 
     Logger.recordOutput("IntakeExtension/extenderAtPosition", extenderAtPosition);
     Logger.recordOutput(

src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIO.java

@@ -16,12 +16,12 @@ class IntakeExtensionIOInputs {
     public Angle extenderMotorSetpoint = Rotation.of(0);
   }
 
-  default void updateState(IntakeExtensionIOInputs inputs) {}
+  void updateState(IntakeExtensionIOInputs inputs);
 
-  default void setExtenderVoltage(Voltage extensionVoltage) {}
+  void setExtenderVoltage(Voltage extensionVoltage);
 
-  default void setExtensionSetpoint(Angle setpoint) {}
+  void setExtensionSetpoint(Angle setpoint);
 
   @Override
-  default void close() throws Exception {}
+  void close();
 }

src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOReal.java

@@ -5,21 +5,20 @@
 
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.sim.TalonFXSimState;
 import com.team2813.Constants;
+import com.team2813.subsystems.Simulation;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Voltage;
 
-public class IntakeExtensionIOReal implements IntakeExtensionIO {
+public final class IntakeExtensionIOReal implements IntakeExtensionIO {
   private final TalonFX extenderMotor;
-  private Angle extensionSetpoint;
-
-  private PositionVoltage positionVoltage = new PositionVoltage(0);
+  private Angle extensionSetpoint = Rotation.of(0);
 
   public IntakeExtensionIOReal() {
     extenderMotor = new TalonFX(Constants.EXTENDER_MOTOR_CAN_ID);
     extenderMotor.getConfigurator().apply(IntakeExtensionConstants.EXTENDER_MOTOR_CONFIG);
     extenderMotor.setPosition(Rotation.of(0)); // intake should be fully retracted on bootup
-    extensionSetpoint = Rotation.of(0);
   }
 
   @Override
@@ -34,14 +33,18 @@ public void updateState(IntakeExtensionIOInputs inputs) {
   @Override
   public void setExtensionSetpoint(Angle setpoint) {
     extensionSetpoint = setpoint;
-    extenderMotor.setControl(positionVoltage.withPosition(setpoint));
+    extenderMotor.setControl(new PositionVoltage(setpoint));
   }
 
   @Override
   public void setExtenderVoltage(Voltage extenderVoltage) {
     extenderMotor.setVoltage(extenderVoltage.in(Volts));
   }
 
+  TalonFXSimState getExtenderMotorSimState() {
+    return Simulation.getConfiguredSimState(extenderMotor, TalonFXSimState.MotorType.KrakenX44);
+  }
+
   @Override
   public void close() {
     extenderMotor.close();

src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOSim.java

@@ -2,10 +2,7 @@
 
 import static edu.wpi.first.units.Units.*;
 
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.sim.TalonFXSimState;
-import com.team2813.Constants;
 import com.team2813.subsystems.Simulation;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
@@ -14,73 +11,65 @@
 import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.simulation.ElevatorSim;
 
-public class IntakeExtensionIOSim implements IntakeExtensionIO {
-  private final TalonFX extenderMotor;
-  private final TalonFXSimState extenderMotorSimState;
-
+public final class IntakeExtensionIOSim implements IntakeExtensionIO {
+  private final IntakeExtensionIOReal realIntakeExtension;
   private final ElevatorSim extenderSim;
-
   // PID controller for simulation - TalonFX sim doesn't run internal PID
   private final PIDController simPidController;
+  private boolean pidControlEnabled;
 
-  private Angle extensionSetpoint;
-
-  // Motor to extension direction depends on how the motor is geared with respect to the extension
-  // mechanism.
-  private static final double MOTOR_DIRECTION =
-      IntakeExtensionConstants.EXTENDER_MOTOR_CONFIG.MotorOutput.Inverted
-              == InvertedValue.CounterClockwise_Positive
-          ? 1.0
-          : -1.0;
+  private final TalonFXSimState extenderMotorSimState;
 
   public IntakeExtensionIOSim() {
-    extenderMotor = new TalonFX(Constants.EXTENDER_MOTOR_CAN_ID);
-    extenderMotorSimState = extenderMotor.getSimState();
-
-    extenderMotor.getConfigurator().apply(IntakeExtensionConstants.EXTENDER_MOTOR_CONFIG);
+    realIntakeExtension = new IntakeExtensionIOReal();
+    extenderMotorSimState = realIntakeExtension.getExtenderMotorSimState();
 
+    double minHeightMeters = IntakeExtensionConstants.RETRACTED_POSITION.in(Meters);
+    double maxHeightMeters = IntakeExtensionConstants.EXTENDED_POSITION.in(Meters);
     extenderSim =
         new ElevatorSim(
             DCMotor.getKrakenX44(1),
-            IntakeExtensionConstants.EXTENDER_MOTOR_TO_EXTENDER_GEARING,
-            IntakeExtensionConstants.WEIGHT_OF_EXTENDER_CARRIAGE.in(Kilograms),
-            IntakeExtensionConstants.PULLEY_RADIUS.in(Meters),
-            IntakeExtensionConstants.RETRACTED_POSITION.in(Meters),
-            IntakeExtensionConstants.EXTENDED_POSITION.in(Meters),
-            false,
-            0); // Start unextended
+            /* gearing= */ IntakeExtensionConstants.EXTENDER_MOTOR_TO_EXTENDER_GEARING,
+            /* carriageMassKg= */ IntakeExtensionConstants.WEIGHT_OF_EXTENDER_CARRIAGE.in(
+                Kilograms),
+            /* drumRadiusMeters= */ IntakeExtensionConstants.PULLEY_RADIUS.in(Meters),
+            minHeightMeters,
+            maxHeightMeters,
+            /* simulateGravity= */ false,
+            /* startingHeightMeters= */ minHeightMeters);
 
     // Initialize PID controller with same gains as motor config
     var slot0 = IntakeExtensionConstants.EXTENDER_MOTOR_CONFIG.Slot0;
     simPidController = new PIDController(slot0.kP, slot0.kI, slot0.kD);
-
-    extensionSetpoint = Rotation.of(0);
   }
 
   @Override
   public void updateState(IntakeExtensionIOInputs inputs) {
     // Continue supplying the simulated motor with 12V voltage.
     extenderMotorSimState.setSupplyVoltage(Simulation.getMotorSupplyVoltage());
 
-    // TalonFX simulation doesn't run the internal PID controller, so we simulate it ourselves
-    // using a WPILib PIDController with the same gains.
-    double currentPositionRotations =
-        extenderSim.getPositionMeters()
-            * IntakeExtensionConstants.DISTANCE_METERS_TO_MOTOR_ROTATIONS;
-    double setpointRotations = extensionSetpoint.in(Rotations);
+    if (pidControlEnabled) {
+      // TalonFX simulation doesn't run the internal PID controller, so we simulate it ourselves
+      // using a WPILib PIDController with the same gains.
+      double currentPositionRotations =
+          extenderSim.getPositionMeters()
+              * IntakeExtensionConstants.DISTANCE_METERS_TO_MOTOR_ROTATIONS;
+      double setpointRotations = simPidController.getSetpoint();
 
-    // Calculate PID output voltage
-    double pidOutput = simPidController.calculate(currentPositionRotations, setpointRotations);
+      // Calculate PID output voltage
+      double pidOutput = simPidController.calculate(currentPositionRotations, setpointRotations);
 
-    // Add static friction feedforward (kS). Velocity feedforward (kV) is not used in this
-    // position-control simulation.
-    var slot0 = IntakeExtensionConstants.EXTENDER_MOTOR_CONFIG.Slot0;
-    double feedforward = Math.signum(pidOutput) * slot0.kS;
+      // Add static friction feedforward (kS). Velocity feedforward (kV) is not used in this
+      // position-control simulation.
+      var slot0 = IntakeExtensionConstants.EXTENDER_MOTOR_CONFIG.Slot0;
+      double feedforward = Math.signum(pidOutput) * slot0.kS;
 
-    // Clamp to supply voltage
-    double motorVoltage = MathUtil.clamp(pidOutput + feedforward, -12.0, 12.0);
+      // Clamp to supply voltage
+      double supplyVoltage = Simulation.getMotorSupplyVoltage().in(Volts);
+      double motorVoltage = MathUtil.clamp(pidOutput + feedforward, -supplyVoltage, supplyVoltage);
 
-    extenderSim.setInput(motorVoltage);
+      extenderSim.setInput(motorVoltage);
+    }
 
     // With the new motor voltage, step forward the simulation by 20ms. This will update the
     // position and velocity of the simulated mechanism.
@@ -93,41 +82,37 @@ public void updateState(IntakeExtensionIOInputs inputs) {
     // raw rotor position. By setting the raw rotor position with the opposite sign, getPosition()
     // will return the correct positive value for extended positions.
     extenderMotorSimState.setRotorVelocity(
-        MOTOR_DIRECTION
-            * extenderSim.getVelocityMetersPerSecond()
+        extenderSim.getVelocityMetersPerSecond()
             * IntakeExtensionConstants.DISTANCE_METERS_TO_MOTOR_ROTATIONS);
     extenderMotorSimState.setRawRotorPosition(
-        MOTOR_DIRECTION
-            * extenderSim.getPositionMeters()
+        extenderSim.getPositionMeters()
             * IntakeExtensionConstants.DISTANCE_METERS_TO_MOTOR_ROTATIONS);
 
     // Finally update all simulated inputs.
-    // Report position directly from ElevatorSim to avoid TalonFX inversion issues
-    inputs.extenderMotorVoltage = extenderMotor.getMotorVoltage().getValue();
-    inputs.extenderMotorRPS = extenderMotor.getRotorVelocity().getValue();
-    inputs.extenderMotorCurrent = extenderMotor.getStatorCurrent().getValue();
+    realIntakeExtension.updateState(inputs);
     inputs.extenderMotorPosition =
         Rotations.of(
             extenderSim.getPositionMeters()
                 * IntakeExtensionConstants.DISTANCE_METERS_TO_MOTOR_ROTATIONS);
-    inputs.extenderMotorSetpoint = extensionSetpoint;
   }
 
   @Override
   public void setExtensionSetpoint(Angle setpoint) {
-    extensionSetpoint = setpoint;
-    // In simulation we do not call setControl; PID is handled manually by simPidController using
-    // extensionSetpoint
+    pidControlEnabled = true;
+    realIntakeExtension.setExtensionSetpoint(setpoint);
+    simPidController.setSetpoint(setpoint.in(Rotations));
   }
 
   @Override
   public void close() {
-    extenderMotor.close();
+    realIntakeExtension.close();
+    simPidController.close();
   }
 
   @Override
   public void setExtenderVoltage(Voltage extensionVoltage) {
-    extenderMotor.setVoltage(extensionVoltage.in(Volts));
+    pidControlEnabled = false;
+    realIntakeExtension.setExtenderVoltage(extensionVoltage);
     extenderSim.setInputVoltage(extensionVoltage.in(Volts));
   }
 }

src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOStub.java

@@ -0,0 +1,20 @@
+package com.team2813.subsystems.intakeextension;
+
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Voltage;
+
+/** No-nothing implementation of IntakeExtensionIO; used for replay mode. */
+public final class IntakeExtensionIOStub implements IntakeExtensionIO {
+
+  @Override
+  public void updateState(IntakeExtensionIOInputs inputs) {}
+
+  @Override
+  public void setExtenderVoltage(Voltage extensionVoltage) {}
+
+  @Override
+  public void setExtensionSetpoint(Angle setpoint) {}
+
+  @Override
+  public void close() {}
+}