diff --git a/src/main/java/com/team2813/Constants.java b/src/main/java/com/team2813/Constants.java index f8096ab8..8a88b5b0 100644 --- a/src/main/java/com/team2813/Constants.java +++ b/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"); + } } diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index c0ddaf7e..525d7095 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/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() {}); diff --git a/src/main/java/com/team2813/subsystems/Simulation.java b/src/main/java/com/team2813/subsystems/Simulation.java index 67d1dd0e..03ee88ab 100644 --- a/src/main/java/com/team2813/subsystems/Simulation.java +++ b/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"); } diff --git a/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtension.java b/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtension.java index b8f198b8..2b3ee1bf 100644 --- a/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtension.java +++ b/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( diff --git a/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIO.java b/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIO.java index 9f8ae601..96331d3d 100644 --- a/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIO.java +++ b/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(); } diff --git a/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOReal.java b/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOReal.java index 24db69c1..4c41188f 100644 --- a/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOReal.java +++ b/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,7 +33,7 @@ public void updateState(IntakeExtensionIOInputs inputs) { @Override public void setExtensionSetpoint(Angle setpoint) { extensionSetpoint = setpoint; - extenderMotor.setControl(positionVoltage.withPosition(setpoint)); + extenderMotor.setControl(new PositionVoltage(setpoint)); } @Override @@ -42,6 +41,10 @@ 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(); diff --git a/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOSim.java b/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOSim.java index 58a714f0..7eddae85 100644 --- a/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOSim.java +++ b/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,47 +11,36 @@ 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 @@ -62,25 +48,28 @@ 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)); } } diff --git a/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOStub.java b/src/main/java/com/team2813/subsystems/intakeextension/IntakeExtensionIOStub.java new file mode 100644 index 00000000..5c945102 --- /dev/null +++ b/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() {} +} diff --git a/src/test/java/com/team2813/subsystems/intakeextension/IntakeExtensionTest.java b/src/test/java/com/team2813/subsystems/intakeextension/IntakeExtensionTest.java index c7e99a69..c1d0edba 100644 --- a/src/test/java/com/team2813/subsystems/intakeextension/IntakeExtensionTest.java +++ b/src/test/java/com/team2813/subsystems/intakeextension/IntakeExtensionTest.java @@ -1,67 +1,84 @@ package com.team2813.subsystems.intakeextension; import static edu.wpi.first.units.Units.Rotations; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.*; import com.team2813.lib2813.testing.junit.jupiter.InitWPILib; -import org.junit.jupiter.api.*; - +import com.team2813.subsystems.Simulation; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.TimeUnit; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import org.junit.jupiter.api.AutoClose; +import org.junit.jupiter.api.BeforeAll; +import org.junit.jupiter.api.Test; +import org.littletonrobotics.junction.Logger; + +/** Tests for {@link IntakeExtension}. */ @InitWPILib public class IntakeExtensionTest { + private static final double ACCEPTABLE_ERROR_IN_ROTATIONS = + IntakeExtension.ACCEPTABLE_ERROR_IN_ROTATIONS * 1.1; @AutoClose private final IntakeExtension intakeExtension = new IntakeExtension(new IntakeExtensionIOSim()); + @BeforeAll + public static void initializeLogger() { + Logger.disableConsoleCapture(); + Logger.AdvancedHooks.disableRobotBaseCheck(); + Logger.start(); + } + @Test public void testIntakeExtensionExtend() { intakeExtension.extend(); - for (int i = 0; i < 50; i++) { - intakeExtension.periodic(); - } - - assertEquals( - IntakeExtensionConstants.ExtenderPositions.OUT.getAngle().in(Rotations), - intakeExtension.getPosition().in(Rotations), - 0.5); + simulateRunLoop(Units.Seconds.of(1)); + + assertAll( + () -> + assertTrue( + intakeExtension.isExtenderAtPosition(), + "Extender should be at position after extending"), + () -> + assertEquals( + IntakeExtensionConstants.ExtenderPositions.OUT.getAngle().in(Rotations), + intakeExtension.getPosition().in(Rotations), + ACCEPTABLE_ERROR_IN_ROTATIONS, + "Extender should be close to the OUT position, in Rotations")); } @Test public void testIntakeExtensionRetract() { intakeExtension.retract(); - for (int i = 0; i < 50; i++) { - intakeExtension.periodic(); - } - - assertEquals( - IntakeExtensionConstants.ExtenderPositions.IN.getAngle().in(Rotations), - intakeExtension.getPosition().in(Rotations), - 0.5); - } - - @Test - public void testIntakeExtensionIsAtPositionExtend() { - intakeExtension.extend(); - - for (int i = 0; i < 50; i++) { - intakeExtension.periodic(); - } - - assertTrue( - intakeExtension.isExtenderAtPosition(), "Extender should be at position after extending"); + simulateRunLoop(Units.Seconds.of(1)); + + assertAll( + () -> + assertTrue( + intakeExtension.isExtenderAtPosition(), + "Extender should be at position after extending"), + () -> + assertEquals( + IntakeExtensionConstants.ExtenderPositions.IN.getAngle().in(Rotations), + intakeExtension.getPosition().in(Rotations), + ACCEPTABLE_ERROR_IN_ROTATIONS, + "Extender should be close to the IN position, in Rotations")); } - @Test - public void testIntakeExtensionIsAtPositionRetract() { - intakeExtension.retract(); + private void simulateRunLoop(Measure duration) { + Time now = Units.Seconds.of(0); + Time target = now.plus(duration); + Time period = Units.Seconds.of(Simulation.TIME_PERIOD); - for (int i = 0; i < 50; i++) { + do { intakeExtension.periodic(); - } - assertTrue( - intakeExtension.isExtenderAtPosition(), "Extender should be at position after retracting"); + SimHooks.stepTiming(Simulation.TIME_PERIOD); + now = now.plus(period); + } while (now.lt(target)); } }