diff --git a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java index ad9beaf9..efc64d49 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java +++ b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java @@ -24,10 +24,15 @@ public class Lib199Subsystem implements Subsystem { asyncPeriodicThread = new Thread(() -> { while(true) { - INSTANCE.asyncPeriodic(); try { - Thread.sleep(asyncSleepTime); - } catch(InterruptedException e) {} + INSTANCE.asyncPeriodic(); + try { + Thread.sleep(asyncSleepTime); + } catch(InterruptedException e) {} + } catch (Exception ex) { + System.err.println("Lib199 error running ayncPeriodic() methods: " + ex); + ex.printStackTrace(System.err); + } } }); asyncPeriodicThread.setDaemon(true); diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 55d87718..11e884af 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -3,6 +3,7 @@ import java.util.concurrent.ConcurrentHashMap; import org.carlmontrobotics.lib199.DummySparkMaxAnswer; +import org.carlmontrobotics.lib199.Lib199Subsystem; import org.carlmontrobotics.lib199.Mocks; import org.carlmontrobotics.lib199.REVLibErrorAnswer; @@ -38,6 +39,9 @@ public class MockSparkMax extends MockedMotorBase { private SparkAnalogSensor analogSensor = null; /** + * Initializes a new {@link SimDevice} with the given parameters and creates the necessary sim values, and + * registers this class's {@link #run()} method to be called asynchronously via {@link Lib199Subsystem#registerAsyncSimulationPeriodic(Runnable)}. + * * @param port the port to associate this {@code MockSparkMax} with. Will be used to create the {@link SimDevice} and facilitate motor following. * @param type the type of the simulated motor. If this is set to {@link MotorType#kBrushless}, the builtin encoder simulation will be configured * to follow the inversion state of the motor and its {@code setInverted} method will be disabled. @@ -64,6 +68,8 @@ public REVLibError setInverted(boolean inverted) { pidController.setFeedbackDevice(encoder); controllers.put(port, this); + + Lib199Subsystem.registerAsyncSimulationPeriodic(this); } @Override @@ -93,7 +99,6 @@ public static CANSparkMax createMockSparkMax(int port, MotorType type) { @Override public void set(double speed) { speed *= voltageCompensationNominalVoltage / defaultNominalVoltage; - speed = (isInverted ? -1.0 : 1.0) * speed; pidControllerImpl.setDutyCycle(speed); } diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java index 81de52b1..40b9e490 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java @@ -6,7 +6,6 @@ import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.wpilibj.motorcontrol.MotorController; -import org.carlmontrobotics.lib199.Lib199Subsystem; /** * Represents a base encoder class which can connect to a DeepBlueSim SimDeviceMotorMediator. @@ -37,8 +36,7 @@ public abstract class MockedMotorBase implements AutoCloseable, MotorController, private double requestedSpeedPercent = 0.0; /** - * Initializes a new {@link SimDevice} with the given parameters, creates the necessary sim values, and - * registers this class's {@link #run()} method to be called asynchronously via {@link Lib199Subsystem#registerAsyncSimulationPeriodic(Runnable)}. + * Initializes a new {@link SimDevice} with the given parameters and creates the necessary sim values. * * @param type the device type name to pass to {@link SimDevice#create} * @param port the device port to pass to {@link SimDevice#create} @@ -50,8 +48,6 @@ public MockedMotorBase(String type, int port) { neutralDeadband = device.createDouble("Neutral Deadband", Direction.kOutput, 0.04); brakeModeEnabled = device.createBoolean("Brake Mode", Direction.kOutput, true); currentDraw = device.createDouble("Current Draw", Direction.kInput, 0.0); - - Lib199Subsystem.registerAsyncSimulationPeriodic(this); } /** diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index cf2bbb73..8ddc1167 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -1,5 +1,8 @@ package org.carlmontrobotics.lib199.swerve; +import org.carlmontrobotics.lib199.sim.MockedCANCoder; +import org.carlmontrobotics.lib199.sim.MockedEncoder; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Distance; @@ -34,7 +37,7 @@ public class SwerveModuleSim { 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); + driveEncoderSim = new SimDeviceSim(driveMotorSim.getName() + "_RelativeEncoder"); drivePhysicsSim = new DCMotorSim(DCMotor.getNEO(1), driveGearing, driveMoiKgM2); this.driveGearing = driveGearing; this.driveInversion = driveInversion; @@ -51,13 +54,18 @@ public SwerveModuleSim(int drivePortNum, double driveGearing, boolean driveInver * @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.setInputVoltage(DriverStation.isEnabled() ? driveMotorSim.getDouble("Speed").get()*12.0 : 0.0); drivePhysicsSim.update(dtSecs); - driveEncoderSim.getDouble("count").set((driveInversion ? -1.0: 1.0) * drivePhysicsSim.getAngularPositionRotations()*4096*driveGearing); + driveEncoderSim.getDouble("Position").set(drivePhysicsSim.getAngularPositionRotations()*MockedEncoder.NEO_BUILTIN_ENCODER_CPR*driveGearing); + driveEncoderSim.getDouble("Velocity").set(drivePhysicsSim.getAngularVelocityRPM()*MockedEncoder.NEO_BUILTIN_ENCODER_CPR*driveGearing); - turnPhysicsSim.setInputVoltage(DriverStation.isEnabled() ? turnMotorSim.getDouble("Motor Output").get()*12.0 : 0.0); + turnPhysicsSim.setInputVoltage(DriverStation.isEnabled() ? turnMotorSim.getDouble("Speed").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); + // The -1.0 below is to account for the fact that the CANCoder is mounted such that turning the wheel CCW (as viewed from above) causes + // the encoder value to decrease. However the turnPhysicsSim's angular position will *increase* under those circumstances. + // Note that this is independent of turnInversion. turnInversion controls which direction a positive voltage will cause the turn motor + // to spin. turnInversion should be used to set the motor's inversion so that a positive voltage will spin the wheel CCW (as viewed from above). + turnEncoderSim.getDouble("count").set(MathUtil.inputModulus(-1.0 * turnPhysicsSim.getAngularPositionRotations(), -0.5, 0.5)*MockedCANCoder.kCANCoderCPR); } /** diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java new file mode 100644 index 00000000..0745f075 --- /dev/null +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java @@ -0,0 +1,42 @@ +package org.carlmontrobotics.lib199.sim; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertFalse; +import static org.junit.Assert.assertNotNull; +import static org.junit.Assert.assertTrue; + +import java.util.stream.Stream; + +import com.revrobotics.REVLibError; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.CANSparkLowLevel.MotorType; + +import org.carlmontrobotics.lib199.Mocks; +import org.carlmontrobotics.lib199.REVLibErrorAnswer; +import org.carlmontrobotics.lib199.testUtils.SafelyClosable; +import org.carlmontrobotics.lib199.testUtils.TestRules; +import org.junit.ClassRule; +import org.junit.Rule; +import org.junit.Test; + +import edu.wpi.first.hal.SimDevice; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; + +public class MockSparkMaxTest { + @ClassRule + public static TestRules.InitializeHAL simClassRule = new TestRules.InitializeHAL(); + @Rule + public TestRules.ResetSimDeviceSimData simTestRule = new TestRules.ResetSimDeviceSimData(); + + @Test + public void testHasEncoder() { + var mockSpark = new MockSparkMax(0, MotorType.kBrushless); + SimDeviceSim simSpark = new SimDeviceSim("SparkMax", 0); + assertNotNull(simSpark); + SimDeviceSim simEncoder = new SimDeviceSim(simSpark.getName() + "_RelativeEncoder"); + assertNotNull(simEncoder); + SimDouble simPosition = simEncoder.getDouble("Position"); + assertNotNull(simPosition); + } +}