From 794ec1875f9baa828b2d879cc4bb7805387795f1 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 14 May 2024 20:14:31 -0700 Subject: [PATCH 01/19] Fix SwerveModuleSim to use "Position" instead of "count" so that in conforms to new MockedEncoder. --- .../org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index cf2bbb73..7ab837b3 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -53,7 +53,7 @@ public SwerveModuleSim(int drivePortNum, double driveGearing, boolean driveInver 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); + driveEncoderSim.getDouble("Position").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); From 5185ea88ba6bb872999970499b85fd76abfc0f09 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 14 May 2024 20:24:19 -0700 Subject: [PATCH 02/19] Fix name of encoder sim device. --- .../org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index 7ab837b3..28bf6723 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -34,7 +34,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", drivePortNum); drivePhysicsSim = new DCMotorSim(DCMotor.getNEO(1), driveGearing, driveMoiKgM2); this.driveGearing = driveGearing; this.driveInversion = driveInversion; From f7a5a0d2090c15e95ac21e444c37f76221e2b137 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 14 May 2024 20:52:21 -0700 Subject: [PATCH 03/19] Fix name of encoder sim device. --- .../org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index 28bf6723..9d4205d8 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -34,7 +34,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(driveMotorSim.getName() + "_RelativeEncoder", drivePortNum); + driveEncoderSim = new SimDeviceSim(driveMotorSim.getName() + "_RelativeEncoder"); drivePhysicsSim = new DCMotorSim(DCMotor.getNEO(1), driveGearing, driveMoiKgM2); this.driveGearing = driveGearing; this.driveInversion = driveInversion; From 4c054914ded648fc09c6302af214ba9367ae3f64 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 14 May 2024 20:52:51 -0700 Subject: [PATCH 04/19] Add basic test of MockSparkMax. --- .../lib199/sim/MockSparkMaxTest.java | 42 +++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java 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..354f00c7 --- /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); + } +} From 31728f94f4b0e0f8ac2a044cdf34b864b781249c Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 14 May 2024 21:09:21 -0700 Subject: [PATCH 05/19] Make SwerveModuleSim more robust. --- .../carlmontrobotics/lib199/swerve/SwerveModuleSim.java | 8 +++++++- .../org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java | 2 +- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index 9d4205d8..35ac7dbf 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -1,5 +1,6 @@ package org.carlmontrobotics.lib199.swerve; +import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Distance; @@ -53,7 +54,12 @@ public SwerveModuleSim(int drivePortNum, double driveGearing, boolean driveInver public void update(double dtSecs) { drivePhysicsSim.setInputVoltage(DriverStation.isEnabled() ? driveMotorSim.getDouble("Motor Output").get()*12.0 : 0.0); drivePhysicsSim.update(dtSecs); - driveEncoderSim.getDouble("Position").set((driveInversion ? -1.0: 1.0) * drivePhysicsSim.getAngularPositionRotations()*4096*driveGearing); + SimDouble positionSim = driveEncoderSim.getDouble("Position"); + if (positionSim == null) { + System.err.println("Could not find Position for " + driveEncoderSim.getName()); + return; + } + positionSim.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); diff --git a/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java b/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java index 354f00c7..0745f075 100644 --- a/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java +++ b/src/test/java/org/carlmontrobotics/lib199/sim/MockSparkMaxTest.java @@ -32,7 +32,7 @@ public class MockSparkMaxTest { @Test public void testHasEncoder() { var mockSpark = new MockSparkMax(0, MotorType.kBrushless); - SimDeviceSim simSpark = new SimDeviceSim("SparkMax[0]"); + SimDeviceSim simSpark = new SimDeviceSim("SparkMax", 0); assertNotNull(simSpark); SimDeviceSim simEncoder = new SimDeviceSim(simSpark.getName() + "_RelativeEncoder"); assertNotNull(simEncoder); From 63fcb12050a93cc795e85741670efe56c3859178 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 14 May 2024 21:18:33 -0700 Subject: [PATCH 06/19] Comment out code that houldn't be needed. --- .../carlmontrobotics/lib199/swerve/SwerveModuleSim.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index 35ac7dbf..c0e76250 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -55,10 +55,10 @@ public void update(double dtSecs) { drivePhysicsSim.setInputVoltage(DriverStation.isEnabled() ? driveMotorSim.getDouble("Motor Output").get()*12.0 : 0.0); drivePhysicsSim.update(dtSecs); SimDouble positionSim = driveEncoderSim.getDouble("Position"); - if (positionSim == null) { - System.err.println("Could not find Position for " + driveEncoderSim.getName()); - return; - } + // if (positionSim == null) { + // System.err.println("Could not find Position for " + driveEncoderSim.getName()); + // return; + // } positionSim.set((driveInversion ? -1.0: 1.0) * drivePhysicsSim.getAngularPositionRotations()*4096*driveGearing); turnPhysicsSim.setInputVoltage(DriverStation.isEnabled() ? turnMotorSim.getDouble("Motor Output").get()*12.0 : 0.0); From 2a010426439e052be9984210fce4b7685ffc1ebd Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Tue, 14 May 2024 21:26:05 -0700 Subject: [PATCH 07/19] Change name of sim value from "Motor Output" to "Speed" to match latest MockSparkMax. --- .../org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index c0e76250..52e5a2a7 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -52,7 +52,7 @@ 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); SimDouble positionSim = driveEncoderSim.getDouble("Position"); // if (positionSim == null) { @@ -61,7 +61,7 @@ public void update(double dtSecs) { // } positionSim.set((driveInversion ? -1.0: 1.0) * drivePhysicsSim.getAngularPositionRotations()*4096*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); } From cbe15dff5f3ef5e6ecd080b44c5dd89d542bda6a Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 15 May 2024 10:28:55 -0700 Subject: [PATCH 08/19] Catch and log exceptions thrown during asyncPeriodic methods. --- .../org/carlmontrobotics/lib199/Lib199Subsystem.java | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) 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); From 90b1bec1bafdf97f1eebfc76e8151a93bbf2007f Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 15 May 2024 10:41:10 -0700 Subject: [PATCH 09/19] Don't start MockSparkMax async periodic calls until after it is fully initialized to avoid race condition where where pidControllerImpl is not set when getRequestedSpeed() is called. --- .../java/org/carlmontrobotics/lib199/sim/MockSparkMax.java | 3 +++ .../java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java | 2 -- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 55d87718..474197cf 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; @@ -64,6 +65,8 @@ public REVLibError setInverted(boolean inverted) { pidController.setFeedbackDevice(encoder); controllers.put(port, this); + + Lib199Subsystem.registerAsyncSimulationPeriodic(this); } @Override diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java index 81de52b1..3d3cea6f 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java @@ -50,8 +50,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); } /** From 5fd1ba9aa8228ce8d667003643db363ae00091bb Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 15 May 2024 10:53:19 -0700 Subject: [PATCH 10/19] Use correct number of encoder counts. --- .../carlmontrobotics/lib199/swerve/SwerveModuleSim.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index 52e5a2a7..f5fcb602 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.hal.SimDouble; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; @@ -59,11 +62,11 @@ public void update(double dtSecs) { // System.err.println("Could not find Position for " + driveEncoderSim.getName()); // return; // } - positionSim.set((driveInversion ? -1.0: 1.0) * drivePhysicsSim.getAngularPositionRotations()*4096*driveGearing); + positionSim.set((driveInversion ? -1.0: 1.0) * drivePhysicsSim.getAngularPositionRotations()*MockedEncoder.NEO_BUILTIN_ENCODER_CPR*driveGearing); 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); + turnEncoderSim.getDouble("count").set(MathUtil.inputModulus((turnInversion ? -1.0: 1.0) * turnPhysicsSim.getAngularPositionRotations(), -0.5, 0.5)*MockedCANCoder.kCANCoderCPR); } /** From 364d2b23d78e84ff4c135e8898fb9f0a679df5fe Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 15 May 2024 11:16:13 -0700 Subject: [PATCH 11/19] Run asyncPeriodic at 1kHz, which is the speed motor controller PID loops run at. --- src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java index efc64d49..20aef427 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java +++ b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java @@ -17,7 +17,7 @@ public class Lib199Subsystem implements Subsystem { private static final Thread asyncPeriodicThread; - public static final long asyncSleepTime = 20; + public static final long asyncSleepTime = 1; static { ensureRegistered(); From a77fd04343475cc916421b3a0678b10b44be608f Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 15 May 2024 12:52:46 -0700 Subject: [PATCH 12/19] Fix double inversion in duty cyle mode. --- src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 474197cf..65366074 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -96,7 +96,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); } From 761f6c928231502d8aaaa9ae646ea29079ddd990 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 15 May 2024 13:51:33 -0700 Subject: [PATCH 13/19] Don't invert the drive motor position based on driveInversion. The drivePhysicsSim should do that properly based on the already possibly inverted Speed of the driveMotorSim. --- .../org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index f5fcb602..eb2b54a6 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -62,7 +62,7 @@ public void update(double dtSecs) { // System.err.println("Could not find Position for " + driveEncoderSim.getName()); // return; // } - positionSim.set((driveInversion ? -1.0: 1.0) * drivePhysicsSim.getAngularPositionRotations()*MockedEncoder.NEO_BUILTIN_ENCODER_CPR*driveGearing); + positionSim.set(drivePhysicsSim.getAngularPositionRotations()*MockedEncoder.NEO_BUILTIN_ENCODER_CPR*driveGearing); turnPhysicsSim.setInputVoltage(DriverStation.isEnabled() ? turnMotorSim.getDouble("Speed").get()*12.0 : 0.0); turnPhysicsSim.update(dtSecs); From 76102fe06bd5c5edcd41102acd0e9358021be164 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 15 May 2024 14:50:02 -0700 Subject: [PATCH 14/19] Don't invert the turnEncoderSim value based on turnInversion. The direction will already be inverted based on the motor sim's Speed being inverted. --- .../org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index eb2b54a6..e3fa194a 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -66,7 +66,7 @@ public void update(double dtSecs) { 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)*MockedCANCoder.kCANCoderCPR); + turnEncoderSim.getDouble("count").set(MathUtil.inputModulus(turnPhysicsSim.getAngularPositionRotations(), -0.5, 0.5)*MockedCANCoder.kCANCoderCPR); } /** From 7018f0ff5ec3e6df57807ca60b2fb2544160572f Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 15 May 2024 16:07:52 -0700 Subject: [PATCH 15/19] Always invert the turnEncoderSim direction because of the way that the CANCoder is mounted. --- .../org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index e3fa194a..74c8a0fe 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -66,7 +66,11 @@ public void update(double dtSecs) { turnPhysicsSim.setInputVoltage(DriverStation.isEnabled() ? turnMotorSim.getDouble("Speed").get()*12.0 : 0.0); turnPhysicsSim.update(dtSecs); - turnEncoderSim.getDouble("count").set(MathUtil.inputModulus(turnPhysicsSim.getAngularPositionRotations(), -0.5, 0.5)*MockedCANCoder.kCANCoderCPR); + // 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); } /** From 75c5c2072b7432fce19a3aba5381682dbcf83856 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 15 May 2024 17:20:19 -0700 Subject: [PATCH 16/19] Update the drive encoder velocity so that we can turn. --- .../carlmontrobotics/lib199/swerve/SwerveModuleSim.java | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index 74c8a0fe..f9523193 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -57,12 +57,8 @@ public SwerveModuleSim(int drivePortNum, double driveGearing, boolean driveInver public void update(double dtSecs) { drivePhysicsSim.setInputVoltage(DriverStation.isEnabled() ? driveMotorSim.getDouble("Speed").get()*12.0 : 0.0); drivePhysicsSim.update(dtSecs); - SimDouble positionSim = driveEncoderSim.getDouble("Position"); - // if (positionSim == null) { - // System.err.println("Could not find Position for " + driveEncoderSim.getName()); - // return; - // } - positionSim.set(drivePhysicsSim.getAngularPositionRotations()*MockedEncoder.NEO_BUILTIN_ENCODER_CPR*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("Speed").get()*12.0 : 0.0); turnPhysicsSim.update(dtSecs); From 99d1d0a76326898a161e26ebaefbe8ae613b6b5f Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 15 May 2024 17:30:07 -0700 Subject: [PATCH 17/19] Move doc of where MockSparkMax's asyncPeriodic registration occrus. --- .../java/org/carlmontrobotics/lib199/sim/MockSparkMax.java | 3 +++ .../java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java | 3 +-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java index 65366074..11e884af 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockSparkMax.java @@ -39,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. diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java index 3d3cea6f..156cf5c4 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java @@ -37,8 +37,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} From 687a2357361795be690e2de19368414a6426269e Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 15 May 2024 17:32:55 -0700 Subject: [PATCH 18/19] Switch back to 50Hz asyncPeriodic for now. --- src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java index 20aef427..efc64d49 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java +++ b/src/main/java/org/carlmontrobotics/lib199/Lib199Subsystem.java @@ -17,7 +17,7 @@ public class Lib199Subsystem implements Subsystem { private static final Thread asyncPeriodicThread; - public static final long asyncSleepTime = 1; + public static final long asyncSleepTime = 20; static { ensureRegistered(); From d71500627ea88d3e05239d89d5bacef8cdca5707 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Wed, 15 May 2024 17:40:00 -0700 Subject: [PATCH 19/19] Remove a couple of unneeded imports. --- .../java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java | 1 - .../java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java | 1 - 2 files changed, 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedMotorBase.java index 156cf5c4..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. diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index f9523193..8ddc1167 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -3,7 +3,6 @@ import org.carlmontrobotics.lib199.sim.MockedCANCoder; import org.carlmontrobotics.lib199.sim.MockedEncoder; -import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Distance;