diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index df1a539a..7a1f6aa5 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -89,6 +89,10 @@ public void setInverted(boolean inverted) { isInverted = inverted; } + public boolean getInverted() { + return isInverted; + } + public REVLibError restoreFactoryDefaults() { return REVLibError.kOk; } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java index 2405acc4..a86af265 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedCANCoder.java @@ -34,7 +34,7 @@ public MockedCANCoder(CANcoder canCoder) { } public void update() { - sim.setRawPosition((int) (position.get() * kCANCoderCPR)); + sim.setRawPosition(position.get() / kCANCoderCPR); } public void setGearing(double gearing) { diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java index 68ff907f..67ea0146 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedSparkEncoder.java @@ -73,7 +73,7 @@ public void run() { double dCount = curCount - lastCount; lastTime = t; lastCount = curCount; - double newVelocity = velocityConversionFactor * ( dCount / dt ) / countsPerRevolution; + double newVelocity = velocityConversionFactor * ( dCount / dt ) / countsPerRevolution * 60; velocity = Double.isNaN(newVelocity) ? 0 : newVelocity; } diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index c05bf0f5..26485eaa 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -1,6 +1,9 @@ package org.carlmontrobotics.lib199.swerve; +import static edu.wpi.first.units.Units.Kilogram; +import static edu.wpi.first.units.Units.Pounds; + import java.util.function.Supplier; import org.mockito.internal.reporting.SmartPrinter; @@ -20,6 +23,8 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Mass; +import edu.wpi.first.units.Measure; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; @@ -449,4 +454,26 @@ public void initSendable(SendableBuilder builder) { builder.addDoubleProperty("Turn FF Output", () -> turnFFVolts, null); builder.addDoubleProperty("Turn Total Output", () -> turnVolts, null); } + + /** + * Create and return a SwerveModuleSim that simulates the physics of this swerve module. + * + * @param massOnWheel the mass on the wheel of this module (typically the mass of the robot divided by the number of modules) + * @param turnGearing the gearing reduction between the turn motor and the module + * @param turnMoiKgM2 the moment of inertia of the part of the module turned by the turn motor (in kg m^2) + * @return a SwerveModuleSim that simulates the physics of this swerve module. + */ + public SwerveModuleSim createSim(Measure massOnWheel, double turnGearing, double turnMoiKgM2) { + double driveMoiKgM2 = massOnWheel.in(Kilogram) * Math.pow(config.wheelDiameterMeters/2, 2); + return new SwerveModuleSim(drive.getDeviceId(), config.driveGearing, drive.getInverted(), driveMoiKgM2, + turn.getDeviceId(), turnEncoder.getDeviceID(), turnGearing, turn.getInverted(), turnMoiKgM2); + } + + /** + * + * @return a SwerveModuleSim that simulates this swerve module assuming it is one of 4 MK4i modules on our 114 lb 2024 robot. + */ + public SwerveModuleSim createSim() { + return createSim(Pounds.of(114.0/4), 150.0/7, 0.0313); + } } diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java new file mode 100644 index 00000000..cf2bbb73 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -0,0 +1,71 @@ +package org.carlmontrobotics.lib199.swerve; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Mass; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Mult; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; + +public class SwerveModuleSim { + private SimDeviceSim driveMotorSim, driveEncoderSim, turnMotorSim, turnEncoderSim; + private DCMotorSim drivePhysicsSim, turnPhysicsSim; + private double driveGearing, turnGearing; + private boolean driveInversion, turnInversion; + private Timer timer = new Timer(); + + /** + * Constructs a SwerveModuleSim that simulates the physics of a swerve module. + * + * @param drivePortNum the port of the SparkMax drive motor + * @param driveGearing the gearing reduction between the drive motor and the wheel + * @param driveInversion whether the drive motor is inverted + * @param driveMoiKgM2 the effective moment of inertia around the wheel axle (typciall the mass of the robot divided the number of modules times the square of the wheel radius) + * @param turnMotorPortNum the port of the SparkMax turn motor + * @param turnEncoderPortNum the port of the CANCoder measuring the module's angle + * @param turnGearing the gearing reduction between the turn motor and the module + * @param turnInversion whether the turn motor is inverted + * @param turnMoiKgM2 the moment of inertia of the part of the module turned by the turn motor (in kg m^2) + */ + public SwerveModuleSim(int drivePortNum, double driveGearing, boolean driveInversion, double driveMoiKgM2, + int turnMotorPortNum, int turnEncoderPortNum, double turnGearing, boolean turnInversion, double turnMoiKgM2) { + driveMotorSim = new SimDeviceSim("SparkMax", drivePortNum); + driveEncoderSim = new SimDeviceSim("RelativeEncoder", drivePortNum); + drivePhysicsSim = new DCMotorSim(DCMotor.getNEO(1), driveGearing, driveMoiKgM2); + this.driveGearing = driveGearing; + this.driveInversion = driveInversion; + + turnMotorSim = new SimDeviceSim("SparkMax", turnMotorPortNum); + turnEncoderSim = new SimDeviceSim("CANCoder", turnEncoderPortNum); + turnPhysicsSim = new DCMotorSim(DCMotor.getNEO(1), turnGearing, turnMoiKgM2); + this.turnGearing = turnGearing; + this.turnInversion = turnInversion; + } + + /** + * Steps the simulation forward by dtSecs seconds. + * @param dtSecs seconds to step the simulation forward. + */ + public void update(double dtSecs) { + drivePhysicsSim.setInputVoltage(DriverStation.isEnabled() ? driveMotorSim.getDouble("Motor Output").get()*12.0 : 0.0); + drivePhysicsSim.update(dtSecs); + driveEncoderSim.getDouble("count").set((driveInversion ? -1.0: 1.0) * drivePhysicsSim.getAngularPositionRotations()*4096*driveGearing); + + turnPhysicsSim.setInputVoltage(DriverStation.isEnabled() ? turnMotorSim.getDouble("Motor Output").get()*12.0 : 0.0); + turnPhysicsSim.update(dtSecs); + turnEncoderSim.getDouble("count").set(MathUtil.inputModulus((turnInversion ? -1.0: 1.0) * turnPhysicsSim.getAngularPositionRotations(), -0.5, 0.5)*4096); + } + + /** + * Steps the simulation forward by the amount of time that has elapsed since this method was last called. + */ + public void update() { + double dtSecs = timer.get(); + timer.restart(); + update(dtSecs); + } +}