Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Make tests repeatable and more accurate. #124

Merged
merged 2 commits into from
Jul 20, 2024
Merged

Make tests repeatable and more accurate. #124

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
551d039
Make tests repeatable and more accurate.
brettle Jul 20, 2024
d4079a0
formatting
CoolSpy3 Jul 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
80 changes: 56 additions & 24 deletions plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -491,7 +497,7 @@ private void startTimeSync() {

waitForHALSimWSConnection();

listenerCallbackExecutor.execute(() -> {
runOnExecutorThread(() -> {
simTimeSec = 0.0;
sendRobotTime();
});
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -821,20 +827,31 @@ 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 timestamp 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();
} finally {
// 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);
Expand All @@ -850,28 +867,43 @@ 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;

/**
* Closes this instance, freeing any resources that in holds.
*/
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");
}
}
72 changes: 42 additions & 30 deletions tests/src/systemTest/java/frc/robot/MotorControllerTest.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand Down Expand Up @@ -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) {
Expand All @@ -104,7 +111,7 @@ private static double computeSpeedRadPerSec(DCMotor gearMotor,
}

private static double computeAngleRadians(DCMotor gearMotor, double moiKgM2,
double throttle, double initialSpeedRadPerSec,
double throttle, double initialSpeedRadPerSec,
double initialAngleRad, double tSecs) {
double timeConstantSecs = computeTimeConstantSecs(gearMotor, moiKgM2);
double targetSpeedRadPerSec = throttle * gearMotor.nominalVoltageVolts
Expand All @@ -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;
Expand All @@ -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),
Expand All @@ -150,11 +158,11 @@ 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 <= tMotorStopsSecs) {
return computeAngleRadians(gearMotor, moiKgM2, 0.5, 0, 0, tSecs);
if (tSecs <= tMotorStartsSecs) {
return 0.0;
} else if (tSecs <= tMotorStopsSecs) {
return computeAngleRadians(gearMotor, moiKgM2, 0.5, 0, 0,
tSecs - tMotorStartsSecs);
}
return computeAngleRadians(gearMotor, moiKgM2, 0,
expectedSpeedRadPerSec(gearMotor, moiKgM2, tMotorStopsSecs),
Expand All @@ -168,7 +176,7 @@ private static double computeGearing(DCMotor motor,
}

private static double computeFlywheelThickness(DCMotor gearMotor,
double flywheelRadiusMeters, double flywheelDensityKgPerM3,
double flywheelRadiusMeters, double flywheelDensityKgPerM3,
double desiredTimeConstantSecs) {
// for a time constant of t_c seconds:
// t_c = R*J*k_v/k_T
Expand All @@ -193,7 +201,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...
Expand All @@ -207,20 +216,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);
Expand All @@ -233,9 +242,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(
Expand Down Expand Up @@ -300,15 +307,20 @@ private Consumer<SimulationState> 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,
Expand Down