From 551d039b042502a1d3f46fe05af81e101cbedaa1 Mon Sep 17 00:00:00 2001 From: Dean Brettle Date: Fri, 19 Jul 2024 19:16:37 -0700 Subject: [PATCH 1/2] Make tests repeatable and more accurate. - Remove variable delay that could result in periodic methods being run at slightly different times. - Use the actual gearing and flywheel height from the world instead of the theoretical one to avoid rounding errors. --- .../libdeepbluesim/WebotsSimulator.java | 80 +++++++++++++------ .../java/frc/robot/MotorControllerTest.java | 75 ++++++++++------- 2 files changed, 100 insertions(+), 55 deletions(-) diff --git a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java index 66e5d4a..e842572 100644 --- a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java +++ b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java @@ -86,8 +86,7 @@ public class WebotsSimulator implements AutoCloseable { private final StringPublisher simModePublisher; // We'll use this to run all NT listener callbacks sequentially on a single separate thread. - private final ExecutorService listenerCallbackExecutor = - Executors.newSingleThreadExecutor(); + private ExecutorService listenerCallbackExecutor; // Use these to control NetworkTables logging. // - ntLoglevel = 0 means no NT logging @@ -142,6 +141,13 @@ public WebotsSimulator setMaxJitterSecs(double maxJitterSecs) { } } + private void runOnExecutorThread(Runnable r) { + if (listenerCallbackExecutor.isShutdown()) { + return; + } + listenerCallbackExecutor.execute(r); + } + private static synchronized void acquireFileLock() throws InterruptedException, IOException { if (fileLock == null) { @@ -326,7 +332,7 @@ private WebotsSimulator waitForUserToStart(String worldFileAbsPath) inst.addListener(statusSubscriber, EnumSet.of(Kind.kValueRemote, Kind.kImmediate), (event) -> { final var eventValue = event.valueData.value.getString(); - listenerCallbackExecutor.execute(() -> { + runOnExecutorThread(() -> { LOG.log(Level.DEBUG, "In listener, status = {0}", eventValue); if (!eventValue.equals( @@ -433,7 +439,7 @@ private void startTimeSync() { LOG.log(Level.DEBUG, "In simTimeSec value changed callback"); final var eventValue = value.getDouble(); - listenerCallbackExecutor.execute(() -> { + runOnExecutorThread(() -> { LOG.log(Level.DEBUG, "Got simTimeSec value of {0}", eventValue); if (eventValue < 0.0) { @@ -491,7 +497,7 @@ private void startTimeSync() { waitForHALSimWSConnection(); - listenerCallbackExecutor.execute(() -> { + runOnExecutorThread(() -> { simTimeSec = 0.0; sendRobotTime(); }); @@ -506,14 +512,12 @@ private void waitForHALSimWSConnection() { inst.addListener(statusSubscriber, EnumSet.of(Kind.kValueRemote, Kind.kImmediate), (event) -> { final var eventValue = event.valueData.value.getString(); - listenerCallbackExecutor.execute(() -> { - LOG.log(Level.DEBUG, "In listener, status = {0}", - eventValue); - if (!eventValue.equals( - NTConstants.STATUS_HALSIMWS_CONNECTED_VALUE)) - return; - halSimWSConnected.complete(null); - }); + LOG.log(Level.DEBUG, "In listener, status = {0}", + eventValue); + if (!eventValue.equals( + NTConstants.STATUS_HALSIMWS_CONNECTED_VALUE)) + return; + halSimWSConnected.complete(null); }); requestPublisher.set(NTConstants.REQUEST_HALSIMWS_CONNECTION_VERB); @@ -790,6 +794,8 @@ private void endCompetition(TimedRobot robot) { public void run() throws InstantiationException, IllegalAccessException, InvocationTargetException, NoSuchMethodException, SecurityException, TimeoutException { + listenerCallbackExecutor = Executors.newSingleThreadExecutor(); + endCompetitionCalled = false; // HAL must be initialized or SmartDashboard might not work. HAL.initialize(500, 0); @@ -821,13 +827,23 @@ public void run() throws InstantiationException, IllegalAccessException, }); } - // Run the onInited callbacks once. - // Note: the offset is -period so they are run before other WPILib periodic methods + // Schedule the onInited callbacks to be run once. Note: the offset is -period so they + // are run before + // other WPILib periodic methods the 1e-6 is so that it isn't scheduled for a timstamp + // of exactly 0.0 because that is a special value that causes startCompetition() to end. robot.addPeriodic(this::runRobotInitedCallbacksOnce, - robot.getPeriod(), -robot.getPeriod()); + robot.getPeriod(), -robot.getPeriod() + 1e-6); + + // Step forward far enough that the onInited callbacks are run. This needs to happen on + // a separate thread because they won't be run until startCompetition() has been called + // and startCompetition() will block until timing advances. + runOnExecutorThread(() -> { + LOG.log(Level.INFO, "Calling stepTiming()"); + SimHooks.stepTiming(2e-6); + LOG.log(Level.INFO, "Called stepTiming()"); + }); this.endNotifier = endNotifier; - SimHooks.resumeTiming(); isRobotCodeRunning = true; try { robot.startCompetition(); @@ -835,6 +851,7 @@ public void run() throws InstantiationException, IllegalAccessException, // Always call endCompetition() so that WPILib's notifier is stopped. If it isn't // future tests will hang on calls to stepTiming(). endCompetition(robot); + blockWhileShuttingDownExecutor(); } if (runExitThrowable != null) { throw new RuntimeException(runExitThrowable); @@ -850,6 +867,27 @@ public void run() throws InstantiationException, IllegalAccessException, } } + private void blockWhileShuttingDownExecutor() { + if (listenerCallbackExecutor.isShutdown()) { + return; + } + try { + listenerCallbackExecutor.shutdown(); + if (!listenerCallbackExecutor.awaitTermination(10, + TimeUnit.SECONDS)) { + throw new TimeoutException( + "Timed out awaiting termination of callback executor"); + } + // This needs to be done after shutting down the listenerCallbackExecutor + // because some callbacks create Watchers and Watcher is not thread-safe. + Watcher.closeAll(); + } catch (SecurityException | InterruptedException + | TimeoutException ex) { + throw new RuntimeException("Could not shutdown callback executor.", + ex); + } + } + private Notifier endNotifier = null; /** @@ -857,21 +895,15 @@ public void run() throws InstantiationException, IllegalAccessException, */ public void close() { LOG.log(Level.DEBUG, "Closing WebotsSimulator"); + blockWhileShuttingDownExecutor(); requestPublisher.close(); statusSubscriber.close(); - Watcher.closeAll(); inst.close(); inst.stopServer(); pauser.close(); if (timeSyncDevice != null) { timeSyncDevice.close(); } - try { - listenerCallbackExecutor.shutdown(); - } catch (SecurityException ex) { - LOG.log(Level.ERROR, "Could not shutdown callback executor.", ex); - } - LOG.log(Level.DEBUG, "Done closing WebotsSimulator"); } } diff --git a/tests/src/systemTest/java/frc/robot/MotorControllerTest.java b/tests/src/systemTest/java/frc/robot/MotorControllerTest.java index eb06540..3b31ba8 100644 --- a/tests/src/systemTest/java/frc/robot/MotorControllerTest.java +++ b/tests/src/systemTest/java/frc/robot/MotorControllerTest.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Timer; @Timeout(value = 30, unit = TimeUnit.MINUTES) @ResourceLock("WebotsSimulator") @@ -91,6 +92,12 @@ public class MotorControllerTest { // Rearranging gives: // theta(t) = theta_0 + w_f*t + (w_0 - w_f) * t_c * (1-exp(-t/t_c)) + + // The robot autonomous code runs the motor at 0.5 for 2 seconds and then stops it (i.e. + // sets the speed to 0 and lets it stop on it's own). + private static double tMotorStartsSecs = 0.0; + private static double tMotorStopsSecs = timeOfNextStepSecs(2.0); + private static double computeSpeedRadPerSec(DCMotor gearMotor, double moiKgM2, double throttle, double initialSpeedRadPerSec, double tSecs) { @@ -104,8 +111,8 @@ private static double computeSpeedRadPerSec(DCMotor gearMotor, } private static double computeAngleRadians(DCMotor gearMotor, double moiKgM2, - double throttle, double initialSpeedRadPerSec, - double initialAngleRad, double tSecs) { + double throttle, double initialSpeedRadPerSec, + double initialAngleRad, double tSecs) { double timeConstantSecs = computeTimeConstantSecs(gearMotor, moiKgM2); double targetSpeedRadPerSec = throttle * gearMotor.nominalVoltageVolts * gearMotor.KvRadPerSecPerVolt; @@ -124,7 +131,7 @@ private static double computeTimeConstantSecs(DCMotor gearMotor, } private static double computeCylinderMoiKgM2(double radiusMeters, - double heightMeters, double densityKgPerM3) { + double heightMeters, double densityKgPerM3) { double massKg = densityKgPerM3 * Math.PI * radiusMeters * radiusMeters * heightMeters; return massKg * radiusMeters * radiusMeters / 2.0; @@ -136,12 +143,13 @@ private static double timeOfNextStepSecs(double tSecs) { private static double expectedSpeedRadPerSec(DCMotor gearMotor, double moiKgM2, double tSecs) { - // The robot autonomous code runs the motor at 0.5 for 2 seconds and then stops it (i.e. - // sets the speed to 0 and lets it stop on it's own). - double tMotorStopsSecs = timeOfNextStepSecs(2.0); + if (tSecs <= tMotorStartsSecs) { + return 0.0; + } if (tSecs <= tMotorStopsSecs) { - return computeSpeedRadPerSec(gearMotor, moiKgM2, 0.5, 0, tSecs); + return computeSpeedRadPerSec(gearMotor, moiKgM2, 0.5, 0, + tSecs - tMotorStartsSecs); } return computeSpeedRadPerSec(gearMotor, moiKgM2, 0, expectedSpeedRadPerSec(gearMotor, moiKgM2, tMotorStopsSecs), @@ -150,11 +158,12 @@ private static double expectedSpeedRadPerSec(DCMotor gearMotor, private static double expectedAngleRadians(DCMotor gearMotor, double moiKgM2, double tSecs) { - // The robot autonomous code runs the motor at 0.5 for 2 seconds and then stops it (i.e. - // sets the speed to 0 and lets it stop on it's own). - double tMotorStopsSecs = timeOfNextStepSecs(2.0); + if (tSecs <= tMotorStartsSecs) { + return 0.0; + } if (tSecs <= tMotorStopsSecs) { - return computeAngleRadians(gearMotor, moiKgM2, 0.5, 0, 0, tSecs); + return computeAngleRadians(gearMotor, moiKgM2, 0.5, 0, 0, + tSecs - tMotorStartsSecs); } return computeAngleRadians(gearMotor, moiKgM2, 0, expectedSpeedRadPerSec(gearMotor, moiKgM2, tMotorStopsSecs), @@ -168,8 +177,8 @@ private static double computeGearing(DCMotor motor, } private static double computeFlywheelThickness(DCMotor gearMotor, - double flywheelRadiusMeters, double flywheelDensityKgPerM3, - double desiredTimeConstantSecs) { + double flywheelRadiusMeters, double flywheelDensityKgPerM3, + double desiredTimeConstantSecs) { // for a time constant of t_c seconds: // t_c = R*J*k_v/k_T // J = t_c*k_T/(k_v*R) @@ -193,7 +202,8 @@ private static class Model { double moiKgM2; Measurement m1, m2; - Model(String motorModelName, String shaftDefPath) throws Exception { + Model(String motorModelName, String shaftDefPath, double gearing, + double flywheelThicknessMeters) throws Exception { this.motorModelName = motorModelName; this.shaftDefPath = shaftDefPath; // To ensure we the flywheel doesn't spin or accelerate too fast... @@ -207,20 +217,20 @@ private static class Model { var motor = (DCMotor) (DCMotor.class .getDeclaredMethod("get" + motorModelName, int.class) .invoke(null, 1)); - var gearing = computeGearing(motor, desiredFlywheelFreeSpeedRPS); + assertEquals(computeGearing(motor, desiredFlywheelFreeSpeedRPS), + gearing, 0.01 * gearing, + "Incorrect gearing for a free speed of %g RPS when using a %s" + .formatted(desiredFlywheelFreeSpeedRPS, + motorModelName)); gearMotor = motor.withReduction(gearing); - LOG.log(Level.INFO, - "For a free speed of {0} RPS when using a {1}, assuming the gearing is {2}.\n", - desiredFlywheelFreeSpeedRPS, motorModelName, gearing); - var flywheelThicknessMeters = + assertEquals( computeFlywheelThickness(gearMotor, flywheelRadiusMeters, - flywheelDensityKgPerM3, desiredTimeConstantSecs); - LOG.log(Level.INFO, - "For a time constant of {0} second when using a {1} with a gearing of {2} and a flywheel with radius {3} meters and density {4} kg/m^3, assuming the flywheel thickness is {5} meters.\n", - desiredTimeConstantSecs, motorModelName, gearing, - flywheelRadiusMeters, flywheelDensityKgPerM3, - flywheelThicknessMeters); - + flywheelDensityKgPerM3, desiredTimeConstantSecs), + flywheelThicknessMeters, 0.01 * flywheelThicknessMeters, + "Incorrect flywheel thickness for a time constant of %g second when using a %s with a gearing of %g and a flywheel with radius %g meters and density %g kg/m^3" + .formatted(desiredTimeConstantSecs, motorModelName, + gearing, flywheelRadiusMeters, + flywheelDensityKgPerM3)); // With those settings, the flywheel MOI will be: moiKgM2 = computeCylinderMoiKgM2(flywheelRadiusMeters, flywheelThicknessMeters, flywheelDensityKgPerM3); @@ -233,9 +243,7 @@ public String toString() { private void assertShaftCorrectAtSecs(double actualSpeedRadPerSec, double actualAngleRadians, double tSecs) { - // TODO: Make sim accurate enough that this can be less than 1*simStepSizeSecs - // (https://github.com/DeepBlueRobotics/DeepBlueSim/issues/101) - double jitterSecs = 2.5 * simStepSizeSecs; + double jitterSecs = 0.5 * simStepSizeSecs; double expectedEarlierSpeedRadPerSec = expectedSpeedRadPerSec( gearMotor, moiKgM2, tSecs - jitterSecs); double expectedLaterSpeedRadPerSec = expectedSpeedRadPerSec( @@ -300,15 +308,20 @@ private Consumer stateChecker( }; } + // @RepeatedTest(value = 20, failureThreshold = 1) @RepeatedTest(1) void testCorrectRotationInAutonomous() throws Exception { - models.add(new Model("NEO", "CAN_SHAFT")); - models.add(new Model("MiniCIM", "PWM_SHAFT")); + models.add(new Model("NEO", "CAN_SHAFT", 94.6, 0.392)); + models.add(new Model("MiniCIM", "PWM_SHAFT", 97.3333, 0.215)); try (var manager = new WebotsSimulator( "../plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt", MotorControllerRobot::new)) { manager.atSec(0.0, s -> { + // Ensure that the timer didn't step beyond the "microstep" needed to ensure that + // the onInited callbacks get run. + assertEquals(2e-6, Timer.getFPGATimestamp(), + "Timer.getFPGATimestamp() should be 2e-6"); s.enableAutonomous(); }).setMaxJitterSecs(0).everyStep(stateChecker((s, m) -> { LOG.log(Level.DEBUG, From d4079a0f6ce82ece1e5d26077e939b854c6e5c88 Mon Sep 17 00:00:00 2001 From: CoolSpy3 Date: Fri, 19 Jul 2024 23:13:24 -0700 Subject: [PATCH 2/2] formatting --- .../carlmontrobotics/libdeepbluesim/WebotsSimulator.java | 6 +++--- .../src/systemTest/java/frc/robot/MotorControllerTest.java | 7 +++---- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java index e842572..0fea0c5 100644 --- a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java +++ b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java @@ -828,9 +828,9 @@ public void run() throws InstantiationException, IllegalAccessException, } // Schedule the onInited callbacks to be run once. Note: the offset is -period so they - // are run before - // other WPILib periodic methods the 1e-6 is so that it isn't scheduled for a timstamp - // of exactly 0.0 because that is a special value that causes startCompetition() to end. + // are run before other WPILib periodic methods the 1e-6 is so that it isn't scheduled + // for a timestamp of exactly 0.0 because that is a special value that causes + // startCompetition() to end. robot.addPeriodic(this::runRobotInitedCallbacksOnce, robot.getPeriod(), -robot.getPeriod() + 1e-6); diff --git a/tests/src/systemTest/java/frc/robot/MotorControllerTest.java b/tests/src/systemTest/java/frc/robot/MotorControllerTest.java index 3b31ba8..692aa15 100644 --- a/tests/src/systemTest/java/frc/robot/MotorControllerTest.java +++ b/tests/src/systemTest/java/frc/robot/MotorControllerTest.java @@ -112,7 +112,7 @@ private static double computeSpeedRadPerSec(DCMotor gearMotor, private static double computeAngleRadians(DCMotor gearMotor, double moiKgM2, double throttle, double initialSpeedRadPerSec, - double initialAngleRad, double tSecs) { + double initialAngleRad, double tSecs) { double timeConstantSecs = computeTimeConstantSecs(gearMotor, moiKgM2); double targetSpeedRadPerSec = throttle * gearMotor.nominalVoltageVolts * gearMotor.KvRadPerSecPerVolt; @@ -160,8 +160,7 @@ private static double expectedAngleRadians(DCMotor gearMotor, double moiKgM2, double tSecs) { if (tSecs <= tMotorStartsSecs) { return 0.0; - } - if (tSecs <= tMotorStopsSecs) { + } else if (tSecs <= tMotorStopsSecs) { return computeAngleRadians(gearMotor, moiKgM2, 0.5, 0, 0, tSecs - tMotorStartsSecs); } @@ -178,7 +177,7 @@ private static double computeGearing(DCMotor motor, private static double computeFlywheelThickness(DCMotor gearMotor, double flywheelRadiusMeters, double flywheelDensityKgPerM3, - double desiredTimeConstantSecs) { + double desiredTimeConstantSecs) { // for a time constant of t_c seconds: // t_c = R*J*k_v/k_T // J = t_c*k_T/(k_v*R)