diff --git a/org/ironmaple/simulation/drivesims/SwerveModuleSimulation.java b/org/ironmaple/simulation/drivesims/SwerveModuleSimulation.java new file mode 100644 index 0000000..4608d07 --- /dev/null +++ b/org/ironmaple/simulation/drivesims/SwerveModuleSimulation.java @@ -0,0 +1,538 @@ +package org.ironmaple.simulation.drivesims; + +import static edu.wpi.first.units.Units.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.measure.*; +import java.util.Queue; +import java.util.concurrent.ConcurrentLinkedQueue; +import org.dyn4j.geometry.Vector2; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; +import org.ironmaple.simulation.motorsims.*; + +/** + * + * + *

Simulation for a Single Swerve Module.

+ * + *

Check Online + * Documentation + * + *

This class provides a simulation for a single swerve module in the {@link SwerveDriveSimulation}. + * + *

1. Purpose

+ * + *

This class serves as the bridge between your code and the physics engine. + * + *

You will apply voltage outputs to the drive/steer motor of the module and obtain their encoder readings in your + * code, just as how you deal with your physical motors. + * + *

2. Perspectives

+ * + * + * + *

3. Simulating Odometry

+ * + * + * + *

An example of how to simulate odometry using this class is the ModuleIOSim.java + * from the Advanced Swerve Drive with maple-sim example. + */ +public class SwerveModuleSimulation { + public final SwerveModuleSimulationConfig config; + + private final MapleMotorSim steerMotorSim; + + private Voltage driveMotorAppliedVoltage = Volts.zero(); + private Current driveMotorStatorCurrent = Amps.zero(); + private Angle driveWheelFinalPosition = Radians.zero(); + private AngularVelocity driveWheelFinalSpeed = RadiansPerSecond.zero(); + + private SimulatedMotorController driveMotorController; + + private final Angle steerRelativeEncoderOffSet = Radians.of((Math.random() - 0.5) * 30); + private final Queue driveWheelFinalPositionCache; + private final Queue steerAbsolutePositionCache; + + /** + * + * + *

Constructs a Swerve Module Simulation.

+ * + *

If you are using {@link SimulatedArena#overrideSimulationTimings(Time, int)} to use custom timings, you must + * call the method before constructing any swerve module simulations using this constructor. + * + * @param config the configuration + */ + public SwerveModuleSimulation(SwerveModuleSimulationConfig config) { + this.config = config; + + SimulatedBattery.addElectricalAppliances(this::getDriveMotorSupplyCurrent); + this.steerMotorSim = new MapleMotorSim(config.steerMotorConfigs); + + this.driveWheelFinalPositionCache = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedArena.getSimulationSubTicksIn1Period(); i++) + driveWheelFinalPositionCache.offer(driveWheelFinalPosition); + this.steerAbsolutePositionCache = new ConcurrentLinkedQueue<>(); + for (int i = 0; i < SimulatedArena.getSimulationSubTicksIn1Period(); i++) + steerAbsolutePositionCache.offer(getSteerAbsoluteFacing()); + + this.driveMotorController = new SimulatedMotorController.GenericMotorController(config.driveMotorConfigs.motor); + this.steerMotorSim.useSimpleDCMotorController(); + } + + public SimMotorConfigs getDriveMotorConfigs() { + return config.driveMotorConfigs; + } + + public SimMotorConfigs getSteerMotorConfigs() { + return steerMotorSim.getConfigs(); + } + + /** + * + * + *

Sets the motor controller for the drive motor.

+ * + *

The configured controller runs control loop on the motor. + * + * @param driveMotorController the motor controller to control the drive motor + */ + public T useDriveMotorController(T driveMotorController) { + this.driveMotorController = driveMotorController; + return driveMotorController; + } + + public SimulatedMotorController.GenericMotorController useGenericMotorControllerForDrive() { + return useDriveMotorController( + new SimulatedMotorController.GenericMotorController(config.driveMotorConfigs.motor)); + } + + /** + * + * + *

Requests the Steering Motor to Run at a Specified Output.

+ * + *

Think of it as the requestOutput() of your physical steering motor. + * + * @param steerMotorController the motor controller to control the steer motor + */ + public T useSteerMotorController(T steerMotorController) { + return this.steerMotorSim.useMotorController(steerMotorController); + } + + public SimulatedMotorController.GenericMotorController useGenericControllerForSteer() { + return this.steerMotorSim.useSimpleDCMotorController(); + } + + /** + * + * + *

Updates the Simulation for This Module.

+ * + *

Note: Friction forces are not simulated in this method. + * + * @param moduleCurrentGroundVelocityWorldRelative the current ground velocity of the module, relative to the world + * @param robotFacing the absolute facing of the robot, relative to the world + * @param gravityForceOnModuleNewtons the gravitational force acting on this module, in newtons + * @return the propelling force generated by the module, as a {@link Vector2} object + */ + public Vector2 updateSimulationSubTickGetModuleForce( + Vector2 moduleCurrentGroundVelocityWorldRelative, + Rotation2d robotFacing, + double gravityForceOnModuleNewtons) { + /* Step1: Update the steer mechanism simulation */ + steerMotorSim.update(SimulatedArena.getSimulationDt()); + + /* Step2: Simulate the amount of propelling force generated by the module. */ + final double grippingForceNewtons = config.getGrippingForceNewtons(gravityForceOnModuleNewtons); + final Rotation2d moduleWorldFacing = this.getSteerAbsoluteFacing().plus(robotFacing); + final Vector2 propellingForce = + getPropellingForce(grippingForceNewtons, moduleWorldFacing, moduleCurrentGroundVelocityWorldRelative); + + /* Step3: Updates and caches the encoder readings for odometry simulation. */ + updateEncoderCaches(); + + return propellingForce; + } + + /** + * + * + *

Calculates the amount of propelling force that the module generates.

+ * + *

For most of the time, that propelling force is directly applied to the drivetrain. And the drive wheel runs as + * fast as the ground velocity + * + *

However, if the propelling force exceeds the gripping, only the max gripping force is applied. The rest of the + * propelling force will cause the wheel to start skidding and make the odometry inaccurate. + * + * @param grippingForceNewtons the amount of gripping force that wheel can generate, in newtons + * @param moduleWorldFacing the current world facing of the module + * @param moduleCurrentGroundVelocity the current ground velocity of the module, world-reference + * @return a vector representing the propelling force that the module generates, world-reference + */ + private Vector2 getPropellingForce( + double grippingForceNewtons, Rotation2d moduleWorldFacing, Vector2 moduleCurrentGroundVelocity) { + final double driveWheelTorque = getDriveWheelTorque(); + double propellingForceNewtons = driveWheelTorque / config.WHEEL_RADIUS.in(Meters); + final boolean skidding = Math.abs(propellingForceNewtons) > grippingForceNewtons; + if (skidding) propellingForceNewtons = Math.copySign(grippingForceNewtons, propellingForceNewtons); + + final double floorVelocityProjectionOnWheelDirectionMPS = moduleCurrentGroundVelocity.getMagnitude() + * Math.cos(moduleCurrentGroundVelocity.getAngleBetween(new Vector2(moduleWorldFacing.getRadians()))); + + // if the chassis is tightly gripped on floor, the floor velocity is projected to the wheel + this.driveWheelFinalSpeed = + RadiansPerSecond.of(floorVelocityProjectionOnWheelDirectionMPS / config.WHEEL_RADIUS.in(Meters)); + + // if the module is skidding + if (skidding) { + final AngularVelocity skiddingEquilibriumWheelSpeed = config.driveMotorConfigs.calculateMechanismVelocity( + config.driveMotorConfigs.calculateCurrent( + NewtonMeters.of(propellingForceNewtons * config.WHEEL_RADIUS.in(Meters))), + driveMotorAppliedVoltage); + this.driveWheelFinalSpeed = driveWheelFinalSpeed.times(0.5).plus(skiddingEquilibriumWheelSpeed.times(0.5)); + } + + return Vector2.create(propellingForceNewtons, moduleWorldFacing.getRadians()); + } + + /** + * + * + *

Calculates the amount of torque that the drive motor can generate on the wheel.

+ * + * @return the amount of torque on the wheel by the drive motor, in Newton * Meters + */ + private double getDriveWheelTorque() { + driveMotorAppliedVoltage = driveMotorController.updateControlSignal( + driveWheelFinalPosition, + driveWheelFinalSpeed, + getDriveEncoderUnGearedPosition(), + getDriveEncoderUnGearedSpeed()); + + driveMotorAppliedVoltage = SimulatedBattery.clamp(driveMotorAppliedVoltage); + + /* calculate the stator current */ + driveMotorStatorCurrent = + config.driveMotorConfigs.calculateCurrent(driveWheelFinalSpeed, driveMotorAppliedVoltage); + + /* calculate the torque generated */ + Torque driveWheelTorque = config.driveMotorConfigs.calculateTorque(driveMotorStatorCurrent); + + /* calculates the torque if you included losses from friction */ + Torque driveWheelTorqueWithFriction = NewtonMeters.of(MathUtil.applyDeadband( + driveWheelTorque.in(NewtonMeters), + config.driveMotorConfigs.friction.in(NewtonMeters), + Double.POSITIVE_INFINITY)); + return driveWheelTorqueWithFriction.in(NewtonMeters); + } + + /** @return the current module state of this simulation module */ + public SwerveModuleState getCurrentState() { + return new SwerveModuleState( + MetersPerSecond.of(getDriveWheelFinalSpeed().in(RadiansPerSecond) * config.WHEEL_RADIUS.in(Meters)), + getSteerAbsoluteFacing()); + } + + /** + * + * + *

Obtains the "free spin" state of the module

+ * + *

The "free spin" state of a simulated module refers to its state after spinning freely for a long time under + * the current input voltage + * + * @return the free spinning module state + */ + protected SwerveModuleState getFreeSpinState() { + return new SwerveModuleState( + config.driveMotorConfigs + .calculateMechanismVelocity( + config.driveMotorConfigs.calculateCurrent(config.driveMotorConfigs.friction), + driveMotorAppliedVoltage) + .in(RadiansPerSecond) + * config.WHEEL_RADIUS.in(Meters), + getSteerAbsoluteFacing()); + } + + /** + * + * + *

Cache the encoder values for high-frequency odometry.

+ * + *

An internal method to cache the encoder values to their queues. + */ + private void updateEncoderCaches() { + /* Increment of drive wheel position */ + this.driveWheelFinalPosition = + this.driveWheelFinalPosition.plus(this.driveWheelFinalSpeed.times(SimulatedArena.getSimulationDt())); + + /* cache sensor readings to queue for high-frequency odometry */ + this.steerAbsolutePositionCache.poll(); + this.steerAbsolutePositionCache.offer(getSteerAbsoluteFacing()); + + this.driveWheelFinalPositionCache.poll(); + this.driveWheelFinalPositionCache.offer(driveWheelFinalPosition); + } + + /** + * + * + *

Obtains the Actual Output Voltage of the Drive Motor.

+ * + * @return the actual output voltage of the drive motor + */ + public Voltage getDriveMotorAppliedVoltage() { + return driveMotorAppliedVoltage; + } + + /** + * + * + *

Obtains the Actual Output Voltage of the Steering Motor.

+ * + * @return the actual output voltage of the steering motor + * @see MapleMotorSim#getAppliedVoltage() + */ + public Voltage getSteerMotorAppliedVoltage() { + return steerMotorSim.getAppliedVoltage(); + } + + /** + * + * + *

Obtains the Amount of Current Supplied to the Drive Motor.

+ * + * @return the current supplied to the drive motor + */ + public Current getDriveMotorSupplyCurrent() { + return getDriveMotorStatorCurrent().times(driveMotorAppliedVoltage.div(SimulatedBattery.getBatteryVoltage())); + } + + /** + * + * + *

Obtains the Stator current the Drive Motor.

+ * + * @return the stator current of the drive motor + */ + public Current getDriveMotorStatorCurrent() { + return driveMotorStatorCurrent; + } + + /** + * + * + *

Obtains the Amount of Current Supplied to the Steer Motor.

+ * + * @return the current supplied to the steer motor + * @see MapleMotorSim#getSupplyCurrent() + */ + public Current getSteerMotorSupplyCurrent() { + return steerMotorSim.getSupplyCurrent(); + } + + /** + * + * + *

Obtains the Stator current the Steer Motor.

+ * + * @return the stator current of the drive motor + * @see MapleMotorSim#getSupplyCurrent() + */ + public Current getSteerMotorStatorCurrent() { + return steerMotorSim.getStatorCurrent(); + } + + /** + * + * + *

Obtains the Position of the Drive Encoder.

+ * + *

This value represents the un-geared position of the encoder, i.e., the amount of radians the drive motor's + * encoder has rotated. + * + * @return the position of the drive motor's encoder (un-geared) + */ + public Angle getDriveEncoderUnGearedPosition() { + return getDriveWheelFinalPosition().times(config.DRIVE_GEAR_RATIO); + } + + /** + * + * + *

Obtains the Final Position of the Wheel.

+ * + *

This method provides the final position of the drive encoder in terms of wheel angle. + * + * @return the final position of the drive encoder (wheel rotations) + */ + public Angle getDriveWheelFinalPosition() { + return driveWheelFinalPosition; + } + + /** + * + * + *

Obtains the Speed of the Drive Encoder.

+ * + * @return the un-geared speed of the drive encoder + */ + public AngularVelocity getDriveEncoderUnGearedSpeed() { + return getDriveWheelFinalSpeed().times(config.DRIVE_GEAR_RATIO); + } + + /** + * + * + *

Obtains the Final Speed of the Wheel.

+ * + * @return the final speed of the drive wheel + */ + public AngularVelocity getDriveWheelFinalSpeed() { + return driveWheelFinalSpeed; + } + + /** + * + * + *

Obtains the Relative Position of the Steer Encoder.

+ * + * @return the relative encoder position of the steer motor + * @see MapleMotorSim#getEncoderPosition() + */ + public Angle getSteerRelativeEncoderPosition() { + return getSteerAbsoluteFacing() + .getMeasure() + .times(config.STEER_GEAR_RATIO) + .plus(steerRelativeEncoderOffSet); + } + + /** + * + * + *

Obtains the Speed of the Steer Relative Encoder (Geared).

+ * + * @return the speed of the steer relative encoder + * @see MapleMotorSim#getEncoderVelocity() + */ + public AngularVelocity getSteerRelativeEncoderVelocity() { + return getSteerAbsoluteEncoderSpeed().times(config.STEER_GEAR_RATIO); + } + + /** + * + * + *

Obtains the Absolute Facing of the Steer Mechanism.

+ * + * @return the absolute facing of the steer mechanism, as a {@link Rotation2d} + */ + public Rotation2d getSteerAbsoluteFacing() { + return new Rotation2d(getSteerAbsoluteAngle()); + } + + /** + * + * + *

Obtains the Absolute Angle of the Steer Mechanism.

+ * + * @return the (continuous) final angle of the steer mechanism, as a {@link Angle} + * @see MapleMotorSim#getAngularPosition() + */ + public Angle getSteerAbsoluteAngle() { + return steerMotorSim.getAngularPosition(); + } + + /** + * + * + *

Obtains the Absolute Rotational Velocity of the Steer Mechanism.

+ * + * @return the absolute angular velocity of the steer mechanism + */ + public AngularVelocity getSteerAbsoluteEncoderSpeed() { + return steerMotorSim.getVelocity(); + } + + /** + * + * + *

Obtains the Cached Readings of the Drive Encoder's Un-Geared Position.

+ * + *

The values of {@link #getDriveEncoderUnGearedPosition()} are cached at each sub-tick and can be retrieved + * using this method. + * + * @return an array of cached drive encoder un-geared positions + */ + public Angle[] getCachedDriveEncoderUnGearedPositions() { + return driveWheelFinalPositionCache.stream() + .map(value -> value.times(config.DRIVE_GEAR_RATIO)) + .toArray(Angle[]::new); + } + + /** + * + * + *

Obtains the Cached Readings of the Drive Encoder's Final Position (Wheel Rotations).

+ * + *

The values of {@link #getDriveWheelFinalPosition()} are cached at each sub-tick and are divided by the gear + * ratio to obtain the final wheel rotations. + * + * @return an array of cached drive encoder final positions (wheel rotations) + */ + public Angle[] getCachedDriveWheelFinalPositions() { + return driveWheelFinalPositionCache.toArray(Angle[]::new); + } + + /** + * + * + *

Obtains the Cached Readings of the Steer Relative Encoder's Position.

+ * + *

The values of {@link #getSteerRelativeEncoderPosition()} are cached at each sub-tick and can be retrieved + * using this method. + * + * @return an array of cached steer relative encoder positions + */ + public Angle[] getCachedSteerRelativeEncoderPositions() { + return steerAbsolutePositionCache.stream() + .map(absoluteFacing -> absoluteFacing + .getMeasure() + .times(config.STEER_GEAR_RATIO) + .plus(steerRelativeEncoderOffSet)) + .toArray(Angle[]::new); + } + + /** + * + * + *

Obtains the Cached Readings of the Steer Absolute Positions.

+ * + *

The values of {@link #getSteerAbsoluteFacing()} are cached at each sub-tick and can be retrieved using this + * method. + * + * @return an array of cached absolute steer positions, as {@link Rotation2d} objects + */ + public Rotation2d[] getCachedSteerAbsolutePositions() { + return steerAbsolutePositionCache.toArray(Rotation2d[]::new); + } +} diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713..fe7985a 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,13 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + {}, + {}, + {}, + { + "guid": "Keyboard0" + } ] } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5aca4e1..0eb6458 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,9 @@ package frc.robot; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.seasonspecific.rebuilt2026.Arena2026Rebuilt; +import org.ironmaple.simulation.seasonspecific.reefscape2025.Arena2025Reefscape; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -77,8 +80,8 @@ public void robotInit() { case SIM: // Running a physics simulator, log to NT Logger.addDataReceiver(new NT4Publisher()); - break; - + break; + case REPLAY: // Replaying a log, set up replay source setUseTiming(false); // Run as fast as possible @@ -151,7 +154,7 @@ private void linkFollowPathLogging() { Logger.recordOutput(pair.getFirst(), pair.getSecond()); }); } - + /** * This autonomous runs the autonomous command selected by your * {@link RobotContainer} class. @@ -176,7 +179,6 @@ public void autonomousPeriodic() { @Override public void teleopInit() { - // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -209,10 +211,20 @@ public void testPeriodic() { /** This function is called once when the robot is first started up. */ @Override public void simulationInit() { + // Initialize the 2025 Reefscape arena + Arena2026Rebuilt arena = new Arena2026Rebuilt(); + arena.clearGamePieces(); + arena.placeGamePiecesOnField(); + + SimulatedArena.overrideInstance(arena); } - /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() { + SimulatedArena.getInstance().simulationPeriodic(); + + Logger.recordOutput( + "SimulatedArena/Fuel", + SimulatedArena.getInstance().getGamePiecesArrayByType("Fuel")); } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java index 8e91539..e49103f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java @@ -11,22 +11,22 @@ import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.configs.IntakeConfig; import frc.robot.lib.util.DashboardMotorControlLoopConfigurator.MotorControlLoopConfig; +import frc.robot.subsystems.swerve.SwerveDrive; +import org.ironmaple.simulation.IntakeSimulation; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import static edu.wpi.first.units.Units.Meters; public class IntakeIOSim implements IntakeIO { private final DCMotor rollerMotorModel = DCMotor.getKrakenX60Foc(1); private final DCMotor pivotMotorModel = DCMotor.getKrakenX60Foc(1); - private final DCMotorSim rollerSim = - new DCMotorSim( + private final DCMotorSim rollerSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(rollerMotorModel, 0.00015, 1.53), - rollerMotorModel - ); + rollerMotorModel); - private final DCMotorSim pivotSim = - new DCMotorSim( + private final DCMotorSim pivotSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(pivotMotorModel, 0.00015, 21.428), - pivotMotorModel - ); + pivotMotorModel); private PIDController rollerFeedback; private SimpleMotorFeedforward rollerFeedforward; @@ -38,6 +38,8 @@ public class IntakeIOSim implements IntakeIO { private boolean isRollerEStopped = false; private boolean isPivotEStopped = false; + private IntakeSimulation intakeSim; + private double lastTimeInputs = Timer.getTimestamp(); private final IntakeConfig config; @@ -45,17 +47,29 @@ public IntakeIOSim(IntakeConfig config) { this.config = config; rollerFeedback = new PIDController(config.rollerKP, config.rollerKI, config.rollerKD); rollerFeedforward = new SimpleMotorFeedforward(config.rollerKS, config.rollerKV, config.rollerKA); + double pivotMaxVelRadPerSec = config.pivotMaxVelocityRotationsPerSec * 2.0 * Math.PI; double pivotMaxAccelRadPerSec2 = config.pivotMaxAccelerationRotationsPerSec2 * 2.0 * Math.PI; - pivotFeedback = - new ProfiledPIDController( + + pivotFeedback = new ProfiledPIDController( config.pivotKP, config.pivotKI, config.pivotKD, - new TrapezoidProfile.Constraints(pivotMaxVelRadPerSec, pivotMaxAccelRadPerSec2) - ); + new TrapezoidProfile.Constraints(pivotMaxVelRadPerSec, pivotMaxAccelRadPerSec2)); pivotSim.setState(config.pivotStartingAngleRotations * 2 * Math.PI, 0); + + SwerveDriveSimulation driveSim = SwerveDrive.getInstance().getSimulation(); + if (driveSim != null) { + this.intakeSim = IntakeSimulation.OverTheBumperIntake( + "Intake", + driveSim, + Meters.of(0.6), + Meters.of(0.3), + IntakeSimulation.IntakeSide.BACK, + 1); + intakeSim.register(); + } } @Override @@ -67,25 +81,21 @@ public void updateInputs(IntakeIOInputs inputs) { rollerSim.setInputVoltage(0); } else if (isRollerClosedLoop) { rollerSim.setInputVoltage( - MathUtil.clamp( - rollerFeedforward.calculate(desiredRollerVelocityRotationsPerSec) + - rollerFeedback.calculate(rollerSim.getAngularVelocityRadPerSec() / (2 * Math.PI)), - -12, - 12 - ) - ); + MathUtil.clamp( + rollerFeedforward.calculate(desiredRollerVelocityRotationsPerSec) + + rollerFeedback.calculate(rollerSim.getAngularVelocityRadPerSec() / (2 * Math.PI)), + -12, + 12)); } if (isPivotEStopped) { pivotSim.setInputVoltage(0); } else if (isPivotClosedLoop) { pivotSim.setInputVoltage( - MathUtil.clamp( - pivotFeedback.calculate(pivotSim.getAngularPositionRad()), - -12, - 12 - ) - ); + MathUtil.clamp( + pivotFeedback.calculate(pivotSim.getAngularPositionRad()), + -12, + 12)); } rollerSim.update(dt); @@ -101,6 +111,14 @@ public void updateInputs(IntakeIOInputs inputs) { inputs.pivotAppliedVolts = pivotSim.getInputVoltage(); inputs.pivotTorqueCurrent = pivotSim.getCurrentDrawAmps(); inputs.pivotTemperatureFahrenheit = 70.0; + + if (intakeSim != null) { + if (inputs.rollerAppliedVolts > 2.0) { + intakeSim.startIntake(); + } else { + intakeSim.stopIntake(); + } + } } @Override @@ -129,8 +147,8 @@ public void setPivotAngle(double angleRotations) { return; } double clampedAngle = MathUtil.clamp(angleRotations, - config.pivotMinAngleRotations, - config.pivotMaxAngleRotations); + config.pivotMinAngleRotations, + config.pivotMaxAngleRotations); pivotFeedback.setGoal(clampedAngle * (2 * Math.PI)); isPivotClosedLoop = true; } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 50dc136..a7b46e1 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.swerve; +import static edu.wpi.first.units.Units.Meter; +import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; @@ -9,6 +11,11 @@ import java.util.concurrent.locks.ReentrantLock; import java.util.function.DoubleSupplier; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -20,6 +27,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -41,6 +49,7 @@ import frc.robot.subsystems.swerve.gyro.GyroIO; import frc.robot.subsystems.swerve.gyro.GyroIOInputsAutoLogged; import frc.robot.subsystems.swerve.gyro.GyroIOPigeon2; +import frc.robot.subsystems.swerve.gyro.GyroIOSim; import frc.robot.subsystems.swerve.module.ModuleIO; import frc.robot.subsystems.swerve.module.ModuleIOInputsAutoLogged; import frc.robot.subsystems.swerve.module.ModuleIOSim; @@ -48,6 +57,7 @@ public class SwerveDrive extends SubsystemBase { private static SwerveDrive instance = null; + public static SwerveDrive getInstance() { if (instance == null) { instance = new SwerveDrive(); @@ -104,7 +114,6 @@ public enum CurrentTranslationOverrideState { CAPPED } - // FSM State Variables private DesiredSystemState desiredSystemState = DesiredSystemState.DISABLED; private CurrentSystemState currentSystemState = CurrentSystemState.DISABLED; @@ -136,7 +145,8 @@ public enum CurrentTranslationOverrideState { private PIDController omegaOverridePIDController; private PIDController snappedOmegaOverridePIDController; private static final double OMEGA_OVERRIDE_CONTROLLER_MAX_VELOCITY_FACTOR = 0.6; - private static final double RANGED_ROTATION_BUFFER_RAD = Math.toRadians(15.0); // Buffer to prevent oscillation at boundaries + private static final double RANGED_ROTATION_BUFFER_RAD = Math.toRadians(15.0); // Buffer to prevent oscillation at + // boundaries private boolean shouldOverrideOmega = false; private double omegaOverride = 0.0; @@ -146,7 +156,8 @@ public enum CurrentTranslationOverrideState { private boolean shouldOverrideOmegaVelocityCap = false; private double omegaVelocityCapMaxRadiansPerSec = Double.MAX_VALUE; - // Translational speed freezing (used during shooting) - only vx/vy are frozen, omega remains controlled + // Translational speed freezing (used during shooting) - only vx/vy are frozen, + // omega remains controlled private boolean shouldOverrideTranslationalSpeedsFrozen = false; private double frozenVxMetersPerSec = 0.0; private double frozenVyMetersPerSec = 0.0; @@ -182,16 +193,16 @@ public enum CurrentTranslationOverrideState { private static final int BACK_RIGHT_INDEX = 3; private SwerveModulePosition[] modulePositions = new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() - }; + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; private SwerveModuleState[] moduleStates = new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState(), + new SwerveModuleState() }; private ChassisSpeeds desiredRobotRelativeSpeeds = new ChassisSpeeds(); @@ -210,144 +221,151 @@ public enum CurrentTranslationOverrideState { private final SysIdRoutine driveCharacterizationSysIdRoutine; private final SysIdRoutine steerCharacterizationSysIdRoutine; + private SwerveDriveSimulation swerveDriveSimulation; + + final DriveTrainSimulationConfig driveTrainSimulationConfig = DriveTrainSimulationConfig.Default() + .withGyro(COTS.ofPigeon2()) + .withSwerveModule(COTS.ofMark4( + DCMotor.getKrakenX60(1), + DCMotor.getFalcon500(1), + COTS.WHEELS.COLSONS.cof, + 2)) + .withTrackLengthTrackWidth(Meters.of(0.52), Meters.of(0.65)) + .withBumperSize(Meters.of(0.762), Meters.of(0.6223)); // ~30 x ~24.5 inches + @SuppressWarnings("static-access") private SwerveDrive() { boolean useSimulation = Constants.shouldUseSimulation(Constants.SimOnlySubsystems.SWERVE); SwerveConfig swerveConfig = ConfigLoader.load( - "swerve", - ConfigLoader.getModeFolder(Constants.SimOnlySubsystems.SWERVE), - SwerveConfig.class - ); + "swerve", + ConfigLoader.getModeFolder(Constants.SimOnlySubsystems.SWERVE), + SwerveConfig.class); drivetrainConfig = swerveConfig.drivetrain; moduleGeneralConfig = swerveConfig.moduleGeneral; if (useSimulation) { + // Create the maple-sim swerve drive simulation + swerveDriveSimulation = new SwerveDriveSimulation( + driveTrainSimulationConfig, + new Pose2d(3, 3, new Rotation2d())); + // Register with the simulated arena for physics updates + SimulatedArena.getInstance().addDriveTrainSimulation(swerveDriveSimulation); + modules = new ModuleIO[] { - new ModuleIOSim(moduleGeneralConfig, 0), - new ModuleIOSim(moduleGeneralConfig, 1), - new ModuleIOSim(moduleGeneralConfig, 2), - new ModuleIOSim(moduleGeneralConfig, 3) + new ModuleIOSim(swerveDriveSimulation.getModules()[0], moduleGeneralConfig, 0), + new ModuleIOSim(swerveDriveSimulation.getModules()[1], moduleGeneralConfig, 1), + new ModuleIOSim(swerveDriveSimulation.getModules()[2], moduleGeneralConfig, 2), + new ModuleIOSim(swerveDriveSimulation.getModules()[3], moduleGeneralConfig, 3) }; - gyroIO = new GyroIO() {}; + gyroIO = new GyroIOSim(swerveDriveSimulation.getGyroSimulation()); } else if (Constants.currentMode == Constants.Mode.REPLAY) { modules = new ModuleIO[] { - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {} + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + }, + new ModuleIO() { + } }; gyroIO = new GyroIOPigeon2(swerveConfig.gyro); } else { modules = new ModuleIO[] { - new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.frontLeft), - new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.frontRight), - new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.backLeft), - new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.backRight) + new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.frontLeft), + new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.frontRight), + new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.backLeft), + new ModuleIOTalonFX(moduleGeneralConfig, swerveConfig.backRight) }; gyroIO = new GyroIOPigeon2(swerveConfig.gyro); PhoenixOdometryThread.getInstance().start(); } driveControlLoopConfigurator = new DashboardMotorControlLoopConfigurator( - "Swerve/driveControlLoop", - new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( - moduleGeneralConfig.driveKP, - moduleGeneralConfig.driveKI, - moduleGeneralConfig.driveKD, - moduleGeneralConfig.driveKS, - moduleGeneralConfig.driveKV, - moduleGeneralConfig.driveKA - ) - ); + "Swerve/driveControlLoop", + new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( + moduleGeneralConfig.driveKP, + moduleGeneralConfig.driveKI, + moduleGeneralConfig.driveKD, + moduleGeneralConfig.driveKS, + moduleGeneralConfig.driveKV, + moduleGeneralConfig.driveKA)); steerControlLoopConfigurator = new DashboardMotorControlLoopConfigurator( - "Swerve/steerControlLoop", - new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( - moduleGeneralConfig.steerKP, - moduleGeneralConfig.steerKI, - moduleGeneralConfig.steerKD, - moduleGeneralConfig.steerKS, - moduleGeneralConfig.steerKV, - moduleGeneralConfig.steerKA - ) - ); + "Swerve/steerControlLoop", + new DashboardMotorControlLoopConfigurator.MotorControlLoopConfig( + moduleGeneralConfig.steerKP, + moduleGeneralConfig.steerKI, + moduleGeneralConfig.steerKD, + moduleGeneralConfig.steerKS, + moduleGeneralConfig.steerKV, + moduleGeneralConfig.steerKA)); kinematics = new SwerveDriveKinematics( - drivetrainConfig.getFrontLeftPositionMeters(), - drivetrainConfig.getFrontRightPositionMeters(), - drivetrainConfig.getBackLeftPositionMeters(), - drivetrainConfig.getBackRightPositionMeters() - ); + drivetrainConfig.getFrontLeftPositionMeters(), + drivetrainConfig.getFrontRightPositionMeters(), + drivetrainConfig.getBackLeftPositionMeters(), + drivetrainConfig.getBackRightPositionMeters()); - // Create the SysId routine - this is going to be in torque current foc units not voltage + // Create the SysId routine - this is going to be in torque current foc units + // not voltage driveCharacterizationSysIdRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - Volts.of(1.5).per(Second), Volts.of(12), Seconds.of(15), // Use default config - (state) -> Logger.recordOutput("DriveCharacterizationSysIdRoutineState", state.toString()) - ), - new SysIdRoutine.Mechanism( - (torqueCurrentFOC) -> { - for (ModuleIO module : modules) { - module.setDriveTorqueCurrentFOC(torqueCurrentFOC.in(Volts), new Rotation2d(0)); - } - }, - null, // No log consumer, since data is recorded by AdvantageKit - this - ) - ); + new SysIdRoutine.Config( + Volts.of(1.5).per(Second), Volts.of(12), Seconds.of(15), // Use default config + (state) -> Logger.recordOutput("DriveCharacterizationSysIdRoutineState", state.toString())), + new SysIdRoutine.Mechanism( + (torqueCurrentFOC) -> { + for (ModuleIO module : modules) { + module.setDriveTorqueCurrentFOC(torqueCurrentFOC.in(Volts), new Rotation2d(0)); + } + }, + null, // No log consumer, since data is recorded by AdvantageKit + this)); steerCharacterizationSysIdRoutine = new SysIdRoutine( - new SysIdRoutine.Config( - Volts.of(1).per(Second), Volts.of(7), Seconds.of(10), // Use default config - (state) -> Logger.recordOutput("SteerCharacterizationSysIdRoutineState", state.toString()) - ), - new SysIdRoutine.Mechanism( - (torqueCurrentFOC) -> { - for (ModuleIO module : modules) { - module.setSteerTorqueCurrentFOC(torqueCurrentFOC.in(Volts), 0); - } - }, - null, // No log consumer, since data is recorded by AdvantageKit - this - ) - ); + new SysIdRoutine.Config( + Volts.of(1).per(Second), Volts.of(7), Seconds.of(10), // Use default config + (state) -> Logger.recordOutput("SteerCharacterizationSysIdRoutineState", state.toString())), + new SysIdRoutine.Mechanism( + (torqueCurrentFOC) -> { + for (ModuleIO module : modules) { + module.setSteerTorqueCurrentFOC(torqueCurrentFOC.in(Volts), 0); + } + }, + null, // No log consumer, since data is recorded by AdvantageKit + this)); // Configure FollowPath builder using drivetrain config followPathBuilder = new FollowPath.Builder( - this, - RobotState.getInstance()::getEstimatedPose, - RobotState.getInstance()::getRobotRelativeSpeeds, - this::driveRobotRelative, - new PIDController( - drivetrainConfig.followPathTranslationKP, - drivetrainConfig.followPathTranslationKI, - drivetrainConfig.followPathTranslationKD - ), - new PIDController( - drivetrainConfig.followPathRotationKP, - drivetrainConfig.followPathRotationKI, - drivetrainConfig.followPathRotationKD - ), - new PIDController( - drivetrainConfig.followPathCrossTrackKP, - drivetrainConfig.followPathCrossTrackKI, - drivetrainConfig.followPathCrossTrackKD - ) - ).withDefaultShouldFlip().withTRatioBasedTranslationHandoffs(true); + this, + RobotState.getInstance()::getEstimatedPose, + RobotState.getInstance()::getRobotRelativeSpeeds, + this::driveRobotRelative, + new PIDController( + drivetrainConfig.followPathTranslationKP, + drivetrainConfig.followPathTranslationKI, + drivetrainConfig.followPathTranslationKD), + new PIDController( + drivetrainConfig.followPathRotationKP, + drivetrainConfig.followPathRotationKI, + drivetrainConfig.followPathRotationKD), + new PIDController( + drivetrainConfig.followPathCrossTrackKP, + drivetrainConfig.followPathCrossTrackKI, + drivetrainConfig.followPathCrossTrackKD)) + .withDefaultShouldFlip().withTRatioBasedTranslationHandoffs(true); // Configure omega override PID controllers with velocity limiting omegaOverridePIDController = new PIDController( - drivetrainConfig.omegaOverrideKP, - drivetrainConfig.omegaOverrideKI, - drivetrainConfig.omegaOverrideKD - ); + drivetrainConfig.omegaOverrideKP, + drivetrainConfig.omegaOverrideKI, + drivetrainConfig.omegaOverrideKD); omegaOverridePIDController.enableContinuousInput(-Math.PI, Math.PI); omegaOverridePIDController.setTolerance(Math.toRadians(drivetrainConfig.rangedRotationToleranceDeg)); snappedOmegaOverridePIDController = new PIDController( - drivetrainConfig.omegaOverrideKP, - drivetrainConfig.omegaOverrideKI, - drivetrainConfig.omegaOverrideKD - ); + drivetrainConfig.omegaOverrideKP, + drivetrainConfig.omegaOverrideKI, + drivetrainConfig.omegaOverrideKD); snappedOmegaOverridePIDController.enableContinuousInput(-Math.PI, Math.PI); snappedOmegaOverridePIDController.setTolerance(Math.toRadians(drivetrainConfig.snappedToleranceDeg)); @@ -355,7 +373,7 @@ private SwerveDrive() { @Override public void periodic() { - double dt = Timer.getTimestamp() - prevLoopTime; + double dt = Timer.getTimestamp() - prevLoopTime; prevLoopTime = Timer.getTimestamp(); Logger.recordOutput("SwerveDrive/dtPeriodic", dt); @@ -378,9 +396,8 @@ public void periodic() { for (int i = 0; i < 4; i++) { moduleStates[i] = new SwerveModuleState( - moduleInputs[i].driveVelocityMetersPerSec, - moduleInputs[i].steerPosition - ); + moduleInputs[i].driveVelocityMetersPerSec, + moduleInputs[i].steerPosition); } if (driveControlLoopConfigurator.hasChanged()) { @@ -400,27 +417,21 @@ public void periodic() { for (int i = 0; i < odometryTimestampsSeconds.length; i++) { for (int j = 0; j < 4; j++) { modulePositions[j] = new SwerveModulePosition( - moduleInputs[j].odometryDrivePositionsMeters[i], - moduleInputs[j].odometrySteerPositions[i] - ); + moduleInputs[j].odometryDrivePositionsMeters[i], + moduleInputs[j].odometrySteerPositions[i]); } - + RobotState.getInstance().addOdometryObservation( - new OdometryObservation( - odometryTimestampsSeconds[i], - gyroInputs.isConnected, - modulePositions, - moduleStates, - gyroInputs.isConnected ? - new Rotation3d( - gyroInputs.gyroOrientation.getX(), - gyroInputs.gyroOrientation.getY(), - gyroInputs.odometryYawPositions[i].getRadians() - ) : - new Rotation3d(), - gyroInputs.isConnected ? gyroInputs.yawVelocityRadPerSec : 0 - ) - ); + new OdometryObservation( + odometryTimestampsSeconds[i], + gyroInputs.isConnected, + modulePositions, + moduleStates, + gyroInputs.isConnected ? new Rotation3d( + gyroInputs.gyroOrientation.getX(), + gyroInputs.gyroOrientation.getY(), + gyroInputs.odometryYawPositions[i].getRadians()) : new Rotation3d(), + gyroInputs.isConnected ? gyroInputs.yawVelocityRadPerSec : 0)); updatedPoses.add(RobotState.getInstance().getEstimatedPose()); } @@ -433,7 +444,8 @@ public void periodic() { handleStateTransitions(); handleCurrentState(); - Logger.recordOutput("SwerveDrive/CurrentCommand", this.getCurrentCommand() == null ? "" : this.getCurrentCommand().toString()); + Logger.recordOutput("SwerveDrive/CurrentCommand", + this.getCurrentCommand() == null ? "" : this.getCurrentCommand().toString()); } /** @@ -459,7 +471,7 @@ private void handleStateTransitions() { currentSystemState = CurrentSystemState.FOLLOW_PATH; } break; - + case PREPARE_FOR_AUTO: if (currentPath != null) { modulesAlignmentTargetRotation = currentPath.getInitialModuleDirection(); @@ -574,8 +586,8 @@ private void handleCurrentState() { break; } - } + private void cancelPathCommand() { if (currentPathCommand != null && currentPathCommand.isScheduled()) { currentPathCommand.cancel(); @@ -588,7 +600,7 @@ private void handleDISABLEDSystemState() { setWheelCoast(true); } cancelPathCommand(); - + driveFieldRelative(new ChassisSpeeds(0, 0, 0)); previousSystemState = CurrentSystemState.DISABLED; @@ -601,7 +613,8 @@ private void handleIdleSystemState() { cancelPathCommand(); // Only cancel running commands, but don't null out finished commands - // This prevents re-scheduling when path completes while desired state is still FOLLOW_PATH + // This prevents re-scheduling when path completes while desired state is still + // FOLLOW_PATH if (currentPathCommand != null && currentPathCommand.isScheduled()) { currentPathCommand.cancel(); } @@ -619,10 +632,9 @@ private void handleTeleopSystemState() { invert = Constants.shouldFlipPath() ? -1 : 1; ChassisSpeeds desiredFieldRelativeSpeeds = new ChassisSpeeds( - vxNormalizedSupplier.getAsDouble() * drivetrainConfig.maxTranslationalVelocityMetersPerSec * invert, - vyNormalizedSupplier.getAsDouble() * drivetrainConfig.maxTranslationalVelocityMetersPerSec * invert, - omegaNormalizedSupplier.getAsDouble() * drivetrainConfig.maxAngularVelocityRadiansPerSec - ); + vxNormalizedSupplier.getAsDouble() * drivetrainConfig.maxTranslationalVelocityMetersPerSec * invert, + vyNormalizedSupplier.getAsDouble() * drivetrainConfig.maxTranslationalVelocityMetersPerSec * invert, + omegaNormalizedSupplier.getAsDouble() * drivetrainConfig.maxAngularVelocityRadiansPerSec); driveFieldRelative(desiredFieldRelativeSpeeds); previousSystemState = CurrentSystemState.TELEOP; @@ -632,9 +644,10 @@ private void handleFollowPathSystemState() { if (previousSystemState == CurrentSystemState.DISABLED) { setWheelCoast(false); } - + // Schedule path command if not already running - if (currentPath != null && (currentPathCommand == null || (!currentPathCommand.isScheduled() && !currentPathCommand.isFinished()))) { + if (currentPath != null && (currentPathCommand == null + || (!currentPathCommand.isScheduled() && !currentPathCommand.isFinished()))) { currentPathCommand = buildPathCommand(currentPath); CommandScheduler.getInstance().schedule(currentPathCommand); } @@ -644,7 +657,8 @@ private void handleFollowPathSystemState() { } private void prepareForAuto() { - if (previousSystemState != CurrentSystemState.PREPARE_FOR_AUTO && previousSystemState != CurrentSystemState.READY_FOR_AUTO) { + if (previousSystemState != CurrentSystemState.PREPARE_FOR_AUTO + && previousSystemState != CurrentSystemState.READY_FOR_AUTO) { setWheelCoast(false); } cancelPathCommand(); @@ -681,11 +695,12 @@ private void handleNoneOmegaOverrideState() { previousOmegaOverrideState = CurrentOmegaOverrideState.NONE; } - + private void handleRangedRotationOmegaOverrideState() { shouldOverrideOmega = true; shouldOverrideOmegaVelocityCap = false; - if (isWithinRotationRange(RANGED_ROTATION_BUFFER_RAD - Math.toRadians(drivetrainConfig.rangedRotationToleranceDeg))) { + if (isWithinRotationRange( + RANGED_ROTATION_BUFFER_RAD - Math.toRadians(drivetrainConfig.rangedRotationToleranceDeg))) { omegaOverride = limitOmegaForRange(lastUnoverriddenOmega); // TODO: BAD FIX } else { omegaOverride = calculateReturnToRangeOmega(); @@ -696,7 +711,7 @@ private void handleRangedRotationNominalOmegaOverrideState() { handleRangedRotationOmegaOverrideState(); previousOmegaOverrideState = CurrentOmegaOverrideState.RANGED_NOMINAL; } - + private void handleRangedRotationReturningOmegaOverrideState() { handleRangedRotationOmegaOverrideState(); previousOmegaOverrideState = CurrentOmegaOverrideState.RANGED_RETURNING; @@ -752,13 +767,15 @@ private void handleCappedTranslationOverrideState() { } /** - * Builds a path command using the followPathBuilder, applying pose reset if configured. + * Builds a path command using the followPathBuilder, applying pose reset if + * configured. */ private Command buildPathCommand(Path path) { if (shouldResetPose) { return followPathBuilder.withPoseReset(RobotState.getInstance()::resetPose).build(path); } - return followPathBuilder.withPoseReset((Pose2d pose) -> {}).build(path); + return followPathBuilder.withPoseReset((Pose2d pose) -> { + }).build(path); } /** @@ -775,17 +792,20 @@ private boolean isAtSnapTarget() { } /** - * Checks if robot rotation is within the specified range with an optional buffer. - * @param buffer The buffer in radians to constrict the range by (applied to both min and max) + * Checks if robot rotation is within the specified range with an optional + * buffer. + * + * @param buffer The buffer in radians to constrict the range by (applied to + * both min and max) */ private boolean isWithinRotationRange(double buffer) { Rotation2d currentRotation = RobotState.getInstance().getEstimatedPose().getRotation(); - + // Normalize angles to -PI to PI for comparison double current = MathUtil.angleModulus(currentRotation.getRadians()); double min = MathUtil.angleModulus(rotationRangeMin.getRadians() + buffer); double max = MathUtil.angleModulus(rotationRangeMax.getRadians() - buffer); - + // Handle wrap-around case if (min <= max) { return current >= min && current <= max; @@ -796,34 +816,36 @@ private boolean isWithinRotationRange(double buffer) { } /** - * Limits omega using sqrt(2*a*d) formula to prevent exceeding the padded rotation bounds. + * Limits omega using sqrt(2*a*d) formula to prevent exceeding the padded + * rotation bounds. * Uses the padded range (user range constricted by buffer) as the boundary. - * Accounts for current robot angular velocity to be more conservative when already + * Accounts for current robot angular velocity to be more conservative when + * already * moving towards a boundary. */ private double limitOmegaForRange(double desiredOmega) { Rotation2d currentRotation = RobotState.getInstance().getEstimatedPose().getRotation(); double currentOmega = RobotState.getInstance().getYawVelocityRadPerSec(); - + double current = currentRotation.getRadians(); // Use padded range bounds (constricted by buffer) double min = rotationRangeMin.getRadians() + RANGED_ROTATION_BUFFER_RAD; double max = rotationRangeMax.getRadians() - RANGED_ROTATION_BUFFER_RAD; - + // Calculate distance to padded bounds (using Rotation2d for proper wrapping) double distToMin = Math.abs(MathUtil.angleModulus(current - min)); double distToMax = Math.abs(MathUtil.angleModulus(max - current)); - + double maxAngularAccel = drivetrainConfig.maxAngularAccelerationRadiansPerSecSec; - + // Calculate stopping distance from current velocity: d = ω² / (2α) double stoppingDistFromCurrent = (currentOmega * currentOmega) / (2 * maxAngularAccel); - + // Adjust effective distances based on current velocity direction // If moving towards a boundary, reduce effective distance by stopping distance double effectiveDistToMax = distToMax; double effectiveDistToMin = distToMin; - + if (currentOmega > 0) { // Moving towards max, reduce effective distance to max effectiveDistToMax = Math.max(0, distToMax - stoppingDistFromCurrent); @@ -831,8 +853,9 @@ private double limitOmegaForRange(double desiredOmega) { // Moving towards min (negative omega), reduce effective distance to min effectiveDistToMin = Math.max(0, distToMin - stoppingDistFromCurrent); } - - // sqrt(2*a*d) formula for max velocity to stop at boundary using effective distances + + // sqrt(2*a*d) formula for max velocity to stop at boundary using effective + // distances double maxOmegaToMin = Math.sqrt(2 * maxAngularAccel * effectiveDistToMin); double maxOmegaToMax = Math.sqrt(2 * maxAngularAccel * effectiveDistToMax); @@ -841,7 +864,7 @@ private double limitOmegaForRange(double desiredOmega) { } if (currentRotation.getRadians() > max) { return Math.min(desiredOmega, 0); - } + } // Clamp omega based on direction if (desiredOmega > 0) { @@ -854,20 +877,22 @@ private double limitOmegaForRange(double desiredOmega) { } /** - * Calculates omega to return to rotation range using PID with velocity limiting. - * Uses an internal buffer to target slightly inside the range to prevent oscillation at boundaries. + * Calculates omega to return to rotation range using PID with velocity + * limiting. + * Uses an internal buffer to target slightly inside the range to prevent + * oscillation at boundaries. */ private double calculateReturnToRangeOmega() { Rotation2d currentRotation = RobotState.getInstance().getEstimatedPose().getRotation(); - + double current = currentRotation.getRadians(); double min = rotationRangeMin.getRadians(); double max = rotationRangeMax.getRadians(); - + // Find closest bound double distToMin = Math.abs(MathUtil.angleModulus(current - min)); double distToMax = Math.abs(MathUtil.angleModulus(current - max)); - + double targetAngle; if (distToMin < distToMax) { // Target slightly inside the min boundary (add buffer) @@ -876,15 +901,15 @@ private double calculateReturnToRangeOmega() { // Target slightly inside the max boundary (subtract buffer) targetAngle = max - RANGED_ROTATION_BUFFER_RAD; } - + // Calculate PID output omegaOverridePIDController.setSetpoint(targetAngle); double pidOutput = omegaOverridePIDController.calculate(current); - + // Apply velocity limit (0.6 of max omega) - double maxOmega = drivetrainConfig.maxAngularVelocityRadiansPerSec * OMEGA_OVERRIDE_CONTROLLER_MAX_VELOCITY_FACTOR; + double maxOmega = drivetrainConfig.maxAngularVelocityRadiansPerSec + * OMEGA_OVERRIDE_CONTROLLER_MAX_VELOCITY_FACTOR; - return MathUtil.clamp(pidOutput, -maxOmega, maxOmega); } @@ -896,16 +921,16 @@ private double calculateSnapOmega() { snappedOmegaOverridePIDController.setSetpoint(target); double pidOutput = snappedOmegaOverridePIDController.calculate(current); - double maxOmega = drivetrainConfig.maxAngularVelocityRadiansPerSec * OMEGA_OVERRIDE_CONTROLLER_MAX_VELOCITY_FACTOR; + double maxOmega = drivetrainConfig.maxAngularVelocityRadiansPerSec + * OMEGA_OVERRIDE_CONTROLLER_MAX_VELOCITY_FACTOR; return MathUtil.clamp(pidOutput, -maxOmega, maxOmega); } // Supplier setters public void setTeleopInputSuppliers( - DoubleSupplier vxNormalized, - DoubleSupplier vyNormalized, - DoubleSupplier omegaNormalized - ) { + DoubleSupplier vxNormalized, + DoubleSupplier vyNormalized, + DoubleSupplier omegaNormalized) { this.vxNormalizedSupplier = vxNormalized; this.vyNormalizedSupplier = vyNormalized; this.omegaNormalizedSupplier = omegaNormalized; @@ -993,23 +1018,23 @@ public void setDesiredTranslationOverrideState(DesiredTranslationOverrideState d // Existing drive methods private ChassisSpeeds compensateRobotRelativeSpeeds(ChassisSpeeds speeds) { - Rotation2d angularVelocity = new Rotation2d(speeds.omegaRadiansPerSecond * drivetrainConfig.rotationCompensationCoefficient); + Rotation2d angularVelocity = new Rotation2d( + speeds.omegaRadiansPerSecond * drivetrainConfig.rotationCompensationCoefficient); if (angularVelocity.getRadians() != 0.0) { speeds = ChassisSpeeds.fromFieldRelativeSpeeds( - ChassisSpeeds.fromRobotRelativeSpeeds( // why should this be split into two? - speeds.vxMetersPerSecond, - speeds.vyMetersPerSecond, - speeds.omegaRadiansPerSecond, - RobotState.getInstance().getEstimatedPose().getRotation().plus(angularVelocity) - ), - RobotState.getInstance().getEstimatedPose().getRotation() - ); + ChassisSpeeds.fromRobotRelativeSpeeds( // why should this be split into two? + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + speeds.omegaRadiansPerSecond, + RobotState.getInstance().getEstimatedPose().getRotation().plus(angularVelocity)), + RobotState.getInstance().getEstimatedPose().getRotation()); } return speeds; } - // return a supplier that is true if the modules are aligned within the tolerance + // return a supplier that is true if the modules are aligned within the + // tolerance private void alignModules(Rotation2d targetRotation, double toleranceDeg) { modulesAlignmentTargetRotation = targetRotation; modulesAlignmentToleranceDeg = toleranceDeg; @@ -1022,8 +1047,10 @@ private void alignModules(Rotation2d targetRotation, double toleranceDeg) { private boolean areModulesAligned() { for (int i = 0; i < 4; i++) { - if (Math.abs(moduleStates[i].angle.minus(modulesAlignmentTargetRotation).getDegrees()) > modulesAlignmentToleranceDeg && - Math.abs(moduleStates[i].angle.plus(Rotation2d.fromDegrees(180)).minus(modulesAlignmentTargetRotation).getDegrees()) > modulesAlignmentToleranceDeg) { + if (Math.abs(moduleStates[i].angle.minus(modulesAlignmentTargetRotation) + .getDegrees()) > modulesAlignmentToleranceDeg && + Math.abs(moduleStates[i].angle.plus(Rotation2d.fromDegrees(180)) + .minus(modulesAlignmentTargetRotation).getDegrees()) > modulesAlignmentToleranceDeg) { return false; } } @@ -1031,7 +1058,7 @@ private boolean areModulesAligned() { } public void driveRobotRelative(ChassisSpeeds speeds) { - double dt = Timer.getTimestamp() - prevDriveTime; + double dt = Timer.getTimestamp() - prevDriveTime; prevDriveTime = Timer.getTimestamp(); lastUnoverriddenOmega = speeds.omegaRadiansPerSecond; @@ -1039,73 +1066,70 @@ public void driveRobotRelative(ChassisSpeeds speeds) { if (shouldOverrideOmega) { desiredRobotRelativeSpeeds = new ChassisSpeeds( - desiredRobotRelativeSpeeds.vxMetersPerSecond, - desiredRobotRelativeSpeeds.vyMetersPerSecond, - omegaOverride - ); + desiredRobotRelativeSpeeds.vxMetersPerSecond, + desiredRobotRelativeSpeeds.vyMetersPerSecond, + omegaOverride); } if (shouldOverrideOmegaVelocityCap) { desiredRobotRelativeSpeeds = new ChassisSpeeds( - desiredRobotRelativeSpeeds.vxMetersPerSecond, - desiredRobotRelativeSpeeds.vyMetersPerSecond, - MathUtil.clamp( - desiredRobotRelativeSpeeds.omegaRadiansPerSecond, - -omegaVelocityCapMaxRadiansPerSec, - omegaVelocityCapMaxRadiansPerSec - ) - ); + desiredRobotRelativeSpeeds.vxMetersPerSecond, + desiredRobotRelativeSpeeds.vyMetersPerSecond, + MathUtil.clamp( + desiredRobotRelativeSpeeds.omegaRadiansPerSecond, + -omegaVelocityCapMaxRadiansPerSec, + omegaVelocityCapMaxRadiansPerSec)); } if (shouldOverrideTranslationVelocityCap) { double maxVelocity = translationVelocityCapMaxVelocityMetersPerSec; - double currentMagnitude = Math.hypot(desiredRobotRelativeSpeeds.vxMetersPerSecond, desiredRobotRelativeSpeeds.vyMetersPerSecond); + double currentMagnitude = Math.hypot(desiredRobotRelativeSpeeds.vxMetersPerSecond, + desiredRobotRelativeSpeeds.vyMetersPerSecond); if (currentMagnitude > maxVelocity && currentMagnitude > 0) { double scale = maxVelocity / currentMagnitude; desiredRobotRelativeSpeeds = new ChassisSpeeds( - desiredRobotRelativeSpeeds.vxMetersPerSecond * scale, - desiredRobotRelativeSpeeds.vyMetersPerSecond * scale, - desiredRobotRelativeSpeeds.omegaRadiansPerSecond - ); + desiredRobotRelativeSpeeds.vxMetersPerSecond * scale, + desiredRobotRelativeSpeeds.vyMetersPerSecond * scale, + desiredRobotRelativeSpeeds.omegaRadiansPerSecond); } } if (shouldOverrideTranslationalSpeedsFrozen) { ChassisSpeeds frozenFieldRelativeSpeeds = new ChassisSpeeds( - frozenVxMetersPerSec, - frozenVyMetersPerSec, - desiredRobotRelativeSpeeds.omegaRadiansPerSecond - ); + frozenVxMetersPerSec, + frozenVyMetersPerSec, + desiredRobotRelativeSpeeds.omegaRadiansPerSecond); - desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(frozenFieldRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); + desiredRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(frozenFieldRelativeSpeeds, + RobotState.getInstance().getEstimatedPose().getRotation()); } - ChassisSpeeds desiredFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(desiredRobotRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); + ChassisSpeeds desiredFieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(desiredRobotRelativeSpeeds, + RobotState.getInstance().getEstimatedPose().getRotation()); Logger.recordOutput("SwerveDrive/desiredFieldRelativeSpeeds", desiredFieldRelativeSpeeds); Logger.recordOutput("SwerveDrive/desiredRobotRelativeSpeeds", desiredRobotRelativeSpeeds); - + // Limit acceleration to prevent sudden changes in speed obtainableFieldRelativeSpeeds = ChassisRateLimiter.limit( - desiredFieldRelativeSpeeds, - obtainableFieldRelativeSpeeds, - dt, - drivetrainConfig.maxTranslationalAccelerationMetersPerSecSec, - drivetrainConfig.maxAngularAccelerationRadiansPerSecSec, - drivetrainConfig.maxTranslationalVelocityMetersPerSec, - drivetrainConfig.maxAngularVelocityRadiansPerSec - ); - + desiredFieldRelativeSpeeds, + obtainableFieldRelativeSpeeds, + dt, + drivetrainConfig.maxTranslationalAccelerationMetersPerSecSec, + drivetrainConfig.maxAngularAccelerationRadiansPerSecSec, + drivetrainConfig.maxTranslationalVelocityMetersPerSec, + drivetrainConfig.maxAngularVelocityRadiansPerSec); + Logger.recordOutput("SwerveDrive/obtainableFieldRelativeSpeeds", obtainableFieldRelativeSpeeds); - ChassisSpeeds obtainableRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(obtainableFieldRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); + ChassisSpeeds obtainableRobotRelativeSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( + obtainableFieldRelativeSpeeds, RobotState.getInstance().getEstimatedPose().getRotation()); Logger.recordOutput("SwerveDrive/obtainableRobotRelativeSpeeds", obtainableRobotRelativeSpeeds); SwerveModuleState[] moduleSetpoints = kinematics.toSwerveModuleStates(obtainableRobotRelativeSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds( - moduleSetpoints, - drivetrainConfig.maxModuleVelocity - ); + moduleSetpoints, + drivetrainConfig.maxModuleVelocity); Logger.recordOutput("SwerveDrive/desaturatedModuleSetpoints", moduleSetpoints); for (int i = 0; i < 4; i++) { @@ -1116,7 +1140,8 @@ public void driveRobotRelative(ChassisSpeeds speeds) { } public void driveFieldRelative(ChassisSpeeds speeds) { - speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, RobotState.getInstance().getEstimatedPose().getRotation()); + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, + RobotState.getInstance().getEstimatedPose().getRotation()); driveRobotRelative(speeds); } @@ -1217,4 +1242,13 @@ public Command getQuasistaticSteerCharacterizationSysIdRoutine(Direction directi public FollowPath.Builder getFollowPathBuilder() { return followPathBuilder; } + + /** + * Returns the SwerveDriveSimulation instance. + * + * @return The simulation instance, or null if not in simulation mode. + */ + public SwerveDriveSimulation getSimulation() { + return swerveDriveSimulation; + } } diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java new file mode 100644 index 0000000..a666785 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroIOSim.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems.swerve.gyro; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.wpilibj.Timer; + +import org.ironmaple.simulation.drivesims.GyroSimulation; + +/** + * Gyro IO implementation backed by maple-sim's GyroSimulation. + * Provides simulated gyro data (yaw, yaw velocity) with realistic noise/drift + * modeled by maple-sim. + */ +public class GyroIOSim implements GyroIO { + private final GyroSimulation gyroSimulation; + + public GyroIOSim(GyroSimulation gyroSimulation) { + this.gyroSimulation = gyroSimulation; + } + + @Override + public void updateInputs(GyroIOInputs inputs) { + inputs.isConnected = true; + + inputs.gyroOrientation = new Rotation3d( + 0.0, + 0.0, + gyroSimulation.getGyroReading().getRadians()); + inputs.yawVelocityRadPerSec = gyroSimulation.getMeasuredAngularVelocity().in( + edu.wpi.first.units.Units.RadiansPerSecond); + + Rotation2d[] cachedYawPositions = gyroSimulation.getCachedGyroReadings(); + inputs.odometryYawPositions = cachedYawPositions; + + double currentTime = Timer.getFPGATimestamp(); + double dt = 0.02 / cachedYawPositions.length; + + inputs.odometryTimestampsSeconds = new double[cachedYawPositions.length]; + for (int i = 0; i < cachedYawPositions.length; i++) { + inputs.odometryTimestampsSeconds[i] = currentTime - (cachedYawPositions.length - 1 - i) * dt; + } + } + + @Override + public void resetGyro(Rotation2d yaw) { + // pose estimator handles the offset + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java index 54a3752..bd39dd1 100644 --- a/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/swerve/module/ModuleIOSim.java @@ -1,109 +1,119 @@ package frc.robot.subsystems.swerve.module; +import static edu.wpi.first.units.Units.*; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; import frc.robot.configs.SwerveModuleGeneralConfig; import frc.robot.lib.util.DashboardMotorControlLoopConfigurator.MotorControlLoopConfig; -public class ModuleIOSim implements ModuleIO { - private final DCMotor driveMotorModel = DCMotor.getKrakenX60Foc(1); - private final DCMotor turnMotorModel = DCMotor.getKrakenX60Foc(1); - - private final DCMotorSim driveSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(driveMotorModel, (2.8087 * 0.0194 * 0.0485614385) / 6.12, 6.12), // J = (kA_linear * Kt * r) / G - driveMotorModel - ); - private final DCMotorSim steerSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(turnMotorModel, 0.00015, 21.428), // magic number because steer is not important - turnMotorModel - ); - +import org.ironmaple.simulation.drivesims.SwerveModuleSimulation; +import org.ironmaple.simulation.motorsims.SimulatedMotorController; - private PIDController driveFeedback = new PIDController(0, 0.0, 0.0); - private PIDController steerFeedback = new PIDController(25, 0.0, 0.0); +/** + * Module IO implementation backed by maple-sim's SwerveModuleSimulation. + * Each instance wraps one of the four SwerveModuleSimulation objects + * obtained from the SwerveDriveSimulation. + */ +public class ModuleIOSim implements ModuleIO { + private final SwerveModuleSimulation moduleSimulation; + private final SimulatedMotorController.GenericMotorController driveMotorController; + private final SimulatedMotorController.GenericMotorController steerMotorController; + private final int moduleID; - private SimpleMotorFeedforward driveFeedforward = new SimpleMotorFeedforward(0.0, 2.44, 0.1); + private PIDController driveFeedback; + private PIDController steerFeedback; + private SimpleMotorFeedforward driveFeedforward; - private boolean isSteerClosedLoop = true; - private boolean isDriveClosedLoop = true; + private boolean isDriveClosedLoop = false; + private boolean isSteerClosedLoop = false; private boolean isDriveEStopped = false; private boolean isSteerEStopped = false; private SwerveModuleState lastDesiredState = new SwerveModuleState(); - private double lastTimeInputs = Timer.getTimestamp(); - - private final int moduleID; - public ModuleIOSim(SwerveModuleGeneralConfig config, int moduleID) { + private final double wheelRadiusMeters; + + public ModuleIOSim(SwerveModuleSimulation moduleSimulation, SwerveModuleGeneralConfig config, int moduleID) { + this.moduleSimulation = moduleSimulation; this.moduleID = moduleID; + this.wheelRadiusMeters = config.driveWheelRadiusMeters; + + this.driveMotorController = moduleSimulation.useGenericMotorControllerForDrive(); + this.steerMotorController = moduleSimulation.useGenericControllerForSteer(); + + driveFeedback = new PIDController(0.05, 0.0, 0.0); + steerFeedback = new PIDController(8.0, 0.0, 0.0); steerFeedback.enableContinuousInput(-Math.PI, Math.PI); + + driveFeedforward = new SimpleMotorFeedforward(0.0, 0.13); } @Override public void updateInputs(ModuleIOInputs inputs) { - double dt = Timer.getTimestamp() - lastTimeInputs; - lastTimeInputs = Timer.getTimestamp(); - if (isDriveEStopped) { - driveSim.setInputVoltage(0); + driveMotorController.requestVoltage(Volts.of(0)); } else if (isDriveClosedLoop) { - driveSim.setInputVoltage( - MathUtil.clamp( - driveFeedforward.calculate(lastDesiredState.speedMetersPerSecond) + - driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec() * 0.0485614385), // wheel radius in meters - -12, - 12 - ) - ); + double driveVelocityMPS = moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond) + * wheelRadiusMeters; + double driveVolts = driveFeedforward.calculate(lastDesiredState.speedMetersPerSecond) + + driveFeedback.calculate(driveVelocityMPS, lastDesiredState.speedMetersPerSecond); + driveMotorController.requestVoltage(Volts.of(MathUtil.clamp(driveVolts, -12, 12))); } if (isSteerEStopped) { - steerSim.setInputVoltage(0); + steerMotorController.requestVoltage(Volts.of(0)); } else if (isSteerClosedLoop) { - steerSim.setInputVoltage( - MathUtil.clamp( - steerFeedback.calculate(steerSim.getAngularPositionRad()), - -12, - 12 - ) - ); + double steerVolts = steerFeedback.calculate( + moduleSimulation.getSteerAbsoluteFacing().getRadians()); + steerMotorController.requestVoltage(Volts.of(MathUtil.clamp(steerVolts, -12, 12))); } - steerSim.update(dt); - driveSim.update(dt); - - inputs.drivePositionMeters = driveSim.getAngularPositionRad() * 0.0485614385; // wheel radius in meters - inputs.driveVelocityMetersPerSec = driveSim.getAngularVelocityRadPerSec() * 0.0485614385; - inputs.driveAppliedVolts = driveSim.getInputVoltage(); + inputs.drivePositionMeters = moduleSimulation.getDriveWheelFinalPosition().in(Radians) + * wheelRadiusMeters; + inputs.driveVelocityMetersPerSec = moduleSimulation.getDriveWheelFinalSpeed().in(RadiansPerSecond) + * wheelRadiusMeters; + inputs.driveAppliedVolts = moduleSimulation.getDriveMotorAppliedVoltage().in(Volts); - inputs.steerPosition = new Rotation2d(steerSim.getAngularPosition()); - inputs.steerVelocityRadPerSec = steerSim.getAngularVelocityRadPerSec(); - inputs.steerAppliedVolts = steerSim.getInputVoltage(); + inputs.steerPosition = moduleSimulation.getSteerAbsoluteFacing(); + inputs.steerVelocityRadPerSec = moduleSimulation.getSteerAbsoluteEncoderSpeed().in(RadiansPerSecond); + inputs.steerAppliedVolts = moduleSimulation.getSteerMotorAppliedVoltage().in(Volts); inputs.steerEncoderAbsolutePosition = inputs.steerPosition; inputs.steerEncoderPosition = inputs.steerPosition; - inputs.driveTorqueCurrent = driveSim.getCurrentDrawAmps(); - inputs.steerTorqueCurrent = steerSim.getCurrentDrawAmps(); + inputs.driveTorqueCurrent = moduleSimulation.getDriveMotorSupplyCurrent().in(Amps); + inputs.steerTorqueCurrent = moduleSimulation.getSteerMotorSupplyCurrent().in(Amps); + + Angle[] cachedDrivePositions = moduleSimulation.getCachedDriveWheelFinalPositions(); + + double currentTime = Timer.getFPGATimestamp(); + double dt = 0.02 / cachedDrivePositions.length; - inputs.odometryTimestampsSeconds = new double[] {Timer.getTimestamp()}; - inputs.odometryDrivePositionsMeters = new double[] {inputs.drivePositionMeters}; - inputs.odometrySteerPositions = new Rotation2d[] {inputs.steerPosition}; + inputs.odometryTimestampsSeconds = new double[cachedDrivePositions.length]; + for (int i = 0; i < cachedDrivePositions.length; i++) { + inputs.odometryTimestampsSeconds[i] = currentTime - (cachedDrivePositions.length - 1 - i) * dt; + } + + inputs.odometryDrivePositionsMeters = new double[cachedDrivePositions.length]; + for (int i = 0; i < cachedDrivePositions.length; i++) { + inputs.odometryDrivePositionsMeters[i] = cachedDrivePositions[i].in(Radians) * wheelRadiusMeters; + } + + Rotation2d[] cachedSteerPositions = moduleSimulation.getCachedSteerAbsolutePositions(); + inputs.odometrySteerPositions = cachedSteerPositions; } @Override public void setState(SwerveModuleState state) { + lastDesiredState = state; + if (isDriveEStopped) { - driveSim.setInputVoltage(0); isDriveClosedLoop = false; } else { driveFeedback.setSetpoint(state.speedMetersPerSecond); @@ -111,37 +121,31 @@ public void setState(SwerveModuleState state) { } if (isSteerEStopped) { - steerSim.setInputVoltage(0); isSteerClosedLoop = false; } else { steerFeedback.setSetpoint(state.angle.getRadians()); isSteerClosedLoop = true; } - - lastDesiredState = state; } @Override public void setSteerTorqueCurrentFOC(double torqueCurrentFOC, double driveVelocityMetersPerSec) { - // In sim, treat torqueCurrentFOC as voltage for simplicity - steerSim.setInputVoltage(isSteerEStopped ? 0 : torqueCurrentFOC); - + steerMotorController.requestVoltage(Volts.of(isSteerEStopped ? 0 : torqueCurrentFOC)); isDriveClosedLoop = false; - isSteerClosedLoop = !isSteerEStopped; + isSteerClosedLoop = false; } @Override public void setDriveTorqueCurrentFOC(double torqueCurrentFOC, Rotation2d steerAngle) { - // In sim, treat torqueCurrentFOC as voltage for simplicity - driveSim.setInputVoltage(isDriveEStopped ? 0 : torqueCurrentFOC); - - isDriveClosedLoop = !isDriveEStopped; + driveMotorController.requestVoltage(Volts.of(isDriveEStopped ? 0 : torqueCurrentFOC)); + isDriveClosedLoop = false; isSteerClosedLoop = true; + steerFeedback.setSetpoint(steerAngle.getRadians()); } @Override public void configureDriveControlLoop(MotorControlLoopConfig config) { - // No-op in sim + // No-op in sim — hardware-level PID tuning not applicable } @Override @@ -152,7 +156,7 @@ public void configureSteerControlLoop(MotorControlLoopConfig config) { @Override public void enableDriveEStop() { isDriveEStopped = true; - driveSim.setInputVoltage(0); + driveMotorController.requestVoltage(Volts.of(0)); isDriveClosedLoop = false; } @@ -164,7 +168,7 @@ public void disableDriveEStop() { @Override public void enableSteerEStop() { isSteerEStopped = true; - steerSim.setInputVoltage(0); + steerMotorController.requestVoltage(Volts.of(0)); isSteerClosedLoop = false; } diff --git a/vendordeps/maple-sim-0.4.0-beta.json b/vendordeps/maple-sim-0.4.0-beta.json new file mode 100644 index 0000000..d9bf4ab --- /dev/null +++ b/vendordeps/maple-sim-0.4.0-beta.json @@ -0,0 +1,26 @@ +{ + "fileName": "maple-sim-0.4.0-beta.json", + "name": "maplesim", + "version": "0.4.0-beta", + "frcYear": "2026", + "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", + "mavenUrls": [ + "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/repos/releases", + "https://repo1.maven.org/maven2" + ], + "jsonUrl": "https://shenzhen-robotics-alliance.github.io/maple-sim/vendordep/maple-sim.json", + "javaDependencies": [ + { + "groupId": "org.ironmaple", + "artifactId": "maplesim-java", + "version": "0.4.0-beta" + }, + { + "groupId": "org.dyn4j", + "artifactId": "dyn4j", + "version": "5.0.2" + } + ], + "jniDependencies": [], + "cppDependencies": [] +} \ No newline at end of file