Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Change opmodes to purely periodic (#8652)

Browse Source 
1. Make the OpMode interface itself periodic; this means the only
differences between `OpMode` and `PeriodicOpMode` are the latter's
methods to add sideloaded periodic callbacks
2. Make OpModeRobot process callbacks in a similar fashion to TimedRobot
and
3. Add some lifecycle functions (discussed below)
4. Pull the callback priority queue from TimedRobot to a new class
called `PeriodicPriorityQueue` so that `TimedRobot` and `OpModeRobot`
have less duplication
5. Fix a typo in the DriverStationJNI class that causes a memory leak
when certain driver station sim calls
6. Port the C++ OpModeRobot tests to Java 

`OpModeRobot` now possesses some `IterativeRobotBase`-stye lifecycle
functions; these functions
1. `robotPeriodic` 
2. `simulationInit` and `simulationPeriodic` 
3. `disabledInit`, `disabledPeriodic`, and `disabledExit`
(note that `simulationInit` and `disabledInit` may be renamed to match
wpilibsuite#8719)

`OpModeRobot` also now processes `OpMode` changes (by the Driver
Station) in its `loopFunc` method, similar to
`IterativeRobotBase.loopFunc` processing game mode changes; `loopFunc`
is, similarly to `TimedRobot`, provided as a default `Callback`

---------

Signed-off-by: Zach Harel <zach@zharel.me>
Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
This commit is contained in:
Zach Harel
2026-04-10 16:40:17 -04:00
committed by GitHub
parent 84295180cd
commit a8c7f3e3c6
29 changed files with 1954 additions and 1340 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibj/src/main/java/org/wpilib/framework/IterativeRobotBase.java
View File

@@ -235,7 +235,7 @@ public abstract class IterativeRobotBase extends RobotBase {
}
/** Loop function. */
protected void loopFunc() {
protected final void loopFunc() {
DriverStation.refreshData();
DriverStation.refreshControlWordFromCache(m_word);
m_watchdog.reset();

520
wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java
View File

@@ -4,18 +4,22 @@
package org.wpilib.framework;
import static org.wpilib.units.Units.Seconds;
import java.io.File;
import java.io.IOException;
import java.lang.reflect.Modifier;
import java.net.URL;
import java.util.Enumeration;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Map;
import java.util.Optional;
import java.util.concurrent.atomic.AtomicReference;
import java.util.Set;
import java.util.function.Supplier;
import java.util.jar.JarEntry;
import java.util.jar.JarFile;
import org.wpilib.driverstation.Alert;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.driverstation.UserControls;
import org.wpilib.driverstation.UserControlsInstance;
@@ -24,13 +28,19 @@ import org.wpilib.hardware.hal.DriverStationJNI;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.hardware.hal.NotifierJNI;
import org.wpilib.hardware.hal.RobotMode;
import org.wpilib.internal.PeriodicPriorityQueue;
import org.wpilib.internal.PeriodicPriorityQueue.Callback;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.opmode.Autonomous;
import org.wpilib.opmode.OpMode;
import org.wpilib.opmode.PeriodicOpMode;
import org.wpilib.opmode.Teleop;
import org.wpilib.opmode.TestOpMode;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.system.RobotController;
import org.wpilib.system.Watchdog;
import org.wpilib.util.Color;
import org.wpilib.util.ConstructorMatch;
import org.wpilib.util.WPIUtilJNI;
/**
* OpModeRobot implements the opmode-based robot program framework.
@@ -38,13 +48,15 @@ import org.wpilib.util.WPIUtilJNI;
* <p>The OpModeRobot class is intended to be subclassed by a user creating a robot program.
*
* <p>Classes annotated with {@link Autonomous}, {@link Teleop}, and {@link TestOpMode} in the same
* package or subpackages as the user's subclass will be automatically registered as autonomous,
* teleop, and test opmodes respectively.
* package or subpackages as the user's subclass are automatically registered as autonomous, teleop,
* and test opmodes respectively.
*
* <p>Opmodes are constructed when selected on the driver station, and closed/no longer used when
* the robot is disabled after being enabled or a different opmode is selected. When no opmode is
* selected, nonePeriodic() is called. The driverStationConnected() function is called the first
* time the driver station connects to the robot.
* <p>Opmodes are constructed when selected on the driver station. While selected and disabled,
* {@link PeriodicOpMode#disabledPeriodic()} is called. When enabled, {@link PeriodicOpMode#start()}
* is called once and {@link PeriodicOpMode#periodic()} runs at the rate from {@link #getPeriod()}.
* On disable or mode switch while enabled, {@link PeriodicOpMode#end()} is called asynchronously
* and the opmode is then closed and discarded. When no opmode is selected, {@link #nonePeriodic()}
* is called. {@link #driverStationConnected()} is called once when the DS first connects.
*/
public abstract class OpModeRobot extends RobotBase {
private final ControlWord m_word = new ControlWord();
@@ -52,8 +64,22 @@ public abstract class OpModeRobot extends RobotBase {
private record OpModeFactory(String name, Supplier<OpMode> supplier) {}
private final Map<Long, OpModeFactory> m_opModes = new HashMap<>();
private final AtomicReference<OpMode> m_activeOpMode = new AtomicReference<>(null);
private volatile int m_notifier;
// Callback system fields (match C++ architecture)
private final PeriodicPriorityQueue m_callbacks = new PeriodicPriorityQueue();
private int m_notifier;
private final double m_period;
private final long m_startTimeUs;
// OpMode lifecycle state
private long m_lastModeId = -1;
private boolean m_calledDriverStationConnected = false;
private boolean m_lastEnabledState = false;
private OpMode m_currentOpMode;
private Callback m_currentOpModePeriodic;
private final Set<Callback> m_activeOpModeCallbacks = new HashSet<>();
private final Watchdog m_watchdog;
private final Alert m_loopOverrunAlert;
private static void reportAddOpModeError(Class<?> cls, String message) {
DriverStation.reportError("Error adding OpMode " + cls.getSimpleName() + ": " + message, false);
@@ -86,9 +112,12 @@ public abstract class OpModeRobot extends RobotBase {
Optional<ConstructorMatch<T>> ctor;
// try 2-parameter constructor
ctor = ConstructorMatch.findBestConstructor(cls, getClass(), m_userControlsInstance.getClass());
if (ctor.isPresent()) {
return ctor;
if (m_userControlsInstance != null) {
ctor =
ConstructorMatch.findBestConstructor(cls, getClass(), m_userControlsInstance.getClass());
if (ctor.isPresent()) {
return ctor;
}
}
// try 1-parameter constructor with RobotBase parameter
@@ -98,18 +127,16 @@ public abstract class OpModeRobot extends RobotBase {
}
// try 1-parameter constructor with UserControls parameter
ctor = ConstructorMatch.findBestConstructor(cls, m_userControlsInstance.getClass());
if (ctor.isPresent()) {
return ctor;
if (m_userControlsInstance != null) {
ctor = ConstructorMatch.findBestConstructor(cls, m_userControlsInstance.getClass());
if (ctor.isPresent()) {
return ctor;
}
}
// try no-parameter constructor
ctor = ConstructorMatch.findBestConstructor(cls);
if (ctor.isPresent()) {
return ctor;
}
return Optional.empty();
return ctor;
}
private <T extends OpMode> T constructOpModeClass(Class<T> cls) {
@@ -120,7 +147,11 @@ public abstract class OpModeRobot extends RobotBase {
return null;
}
try {
return constructor.get().newInstance(this, m_userControlsInstance);
if (m_userControlsInstance != null) {
return constructor.get().newInstance(this, m_userControlsInstance);
} else {
return constructor.get().newInstance(this);
}
} catch (ReflectiveOperationException e) {
DriverStation.reportError(
"Could not instantiate OpMode " + cls.getSimpleName(), e.getStackTrace());
@@ -226,9 +257,10 @@ public abstract class OpModeRobot extends RobotBase {
/**
* Adds an opmode for an opmode class. The class must be a public, non-abstract subclass of OpMode
* with a public constructor that either takes no arguments or accepts a single argument of this
* class's type (the latter is preferred). It's necessary to call publishOpModes() to make the
* added mode visible to the driver station.
* with a public constructor that either takes no arguments or accepts a single argument
* assignable from this robot class type (the latter is preferred; if multiple match, the most
* specific parameter type is used). It's necessary to call publishOpModes() to make the added
* mode visible to the driver station.
*
* @param cls class to add
* @param mode robot mode
@@ -260,9 +292,10 @@ public abstract class OpModeRobot extends RobotBase {
/**
* Adds an opmode for an opmode class. The class must be a public, non-abstract subclass of OpMode
* with a public constructor that either takes no arguments or accepts a single argument of this
* class's type (the latter is preferred). It's necessary to call publishOpModes() to make the
* added mode visible to the driver station.
* with a public constructor that either takes no arguments or accepts a single argument
* assignable from this robot class type (the latter is preferred; if multiple match, the most
* specific parameter type is used). It's necessary to call publishOpModes() to make the added
* mode visible to the driver station.
*
* @param cls class to add
* @param mode robot mode
@@ -278,9 +311,10 @@ public abstract class OpModeRobot extends RobotBase {
/**
* Adds an opmode for an opmode class. The class must be a public, non-abstract subclass of OpMode
* with a public constructor that either takes no arguments or accepts a single argument of this
* class's type (the latter is preferred). It's necessary to call publishOpModes() to make the
* added mode visible to the driver station.
* with a public constructor that either takes no arguments or accepts a single argument
* assignable from this robot class type (the latter is preferred; if multiple match, the most
* specific parameter type is used). It's necessary to call publishOpModes() to make the added
* mode visible to the driver station.
*
* @param cls class to add
* @param mode robot mode
@@ -294,9 +328,10 @@ public abstract class OpModeRobot extends RobotBase {
/**
* Adds an opmode for an opmode class. The class must be a public, non-abstract subclass of OpMode
* with a public constructor that either takes no arguments or accepts a single argument of this
* class's type (the latter is preferred). It's necessary to call publishOpModes() to make the
* added mode visible to the driver station.
* with a public constructor that either takes no arguments or accepts a single argument
* assignable from this robot class type (the latter is preferred; if multiple match, the most
* specific parameter type is used). It's necessary to call publishOpModes() to make the added
* mode visible to the driver station.
*
* @param cls class to add
* @param mode robot mode
@@ -364,8 +399,9 @@ public abstract class OpModeRobot extends RobotBase {
/**
* Adds an opmode for an opmode class annotated with {@link Autonomous}, {@link Teleop}, or {@link
* TestOpMode}. The class must be a public, non-abstract subclass of OpMode with a public
* constructor that either takes no arguments or accepts a single argument of this class's type.
* It's necessary to call publishOpModes() to make the added mode visible to the driver station.
* constructor that either takes no arguments or accepts a single argument assignable from this
* robot class type (if multiple match, the most specific parameter type is used). It's necessary
* to call publishOpModes() to make the added mode visible to the driver station.
*
* @param cls class to add
* @throws IllegalArgumentException if class does not meet criteria
@@ -485,9 +521,37 @@ public abstract class OpModeRobot extends RobotBase {
m_opModes.clear();
}
/** Constructor. */
/** Default loop period. */
public static final double DEFAULT_PERIOD = 0.02;
/** Constructor with default period. */
@SuppressWarnings("this-escape")
public OpModeRobot() {
this(DEFAULT_PERIOD);
}
/**
* Constructor with specified period.
*
* @param period the period at which to run the robot and opmode periodic callbacks.
*/
@SuppressWarnings("this-escape")
public OpModeRobot(double period) {
m_period = period;
// Create our own notifier and callback queue (match C++)
m_notifier = NotifierJNI.createNotifier();
NotifierJNI.setNotifierName(m_notifier, "OpModeRobot");
m_startTimeUs = RobotController.getMonotonicTime();
m_loopOverrunAlert =
new Alert("Loop time of \"" + m_period + "\"s overrun", Alert.Level.MEDIUM);
m_watchdog = new Watchdog(Seconds.of(m_period), () -> m_loopOverrunAlert.set(true));
// Add LoopFunc as periodic callback (match C++)
addPeriodic(this::loopFunc, period);
// Check to see if we have a DS annotation
UserControlsInstance userControlsAnnotation =
getClass().getAnnotation(UserControlsInstance.class);
@@ -499,18 +563,55 @@ public abstract class OpModeRobot extends RobotBase {
// Scan for annotated opmode classes within the derived class's package and subpackages
addAnnotatedOpModeClasses(getClass().getPackage());
DriverStation.publishOpModes();
HAL.reportUsage("Framework", "OpModeRobot");
}
/**
* Function called exactly once after the DS is connected.
* Add a callback to run at a specific period.
*
* <p>Code that needs to know the DS state should go here.
* @param callback The callback to run.
* @param period The period at which to run the callback.
*/
public void addPeriodic(Runnable callback, double period) {
m_callbacks.add(callback, m_startTimeUs, period);
}
/**
* Get the period at which robot and opmode periodic callbacks are run.
*
* @return The period at which robot and opmode periodic callbacks are run.
*/
public double getPeriod() {
return m_period;
}
/**
* Code that needs to know the DS state should go here.
*
* <p>Users should override this method for initialization that needs to occur after the DS is
* connected, such as needing the alliance information.
*/
public void driverStationConnected() {}
/** Function called periodically every loop, regardless of enabled state or OpMode selection. */
public void robotPeriodic() {}
/** Function called once during robot initialization in simulation. */
public void simulationInit() {}
/** Function called periodically in simulation. */
public void simulationPeriodic() {}
/** Function called once when the robot becomes disabled. */
public void disabledInit() {}
/** Function called periodically while the robot is disabled. */
public void disabledPeriodic() {}
/** Function called once when the robot exits disabled state. */
public void disabledExit() {}
/**
* Function called periodically anytime when no opmode is selected, including when the Driver
* Station is disconnected.
@@ -518,76 +619,142 @@ public abstract class OpModeRobot extends RobotBase {
public void nonePeriodic() {}
/**
* Background monitor thread. On mode/opmode change, this checks to see if the change is actually
* reflected in this class within a reasonable amount of time. If not, that means that the user
* code is stuck and we need to take action to try to get it to exit (up to and including program
* termination).
* Return the system clock time in microseconds for the start of the current periodic loop. This
* is in the same time base as Timer.getMonotonicTimestamp(), but is stable through a loop. It is
* updated at the beginning of every periodic callback (including the normal periodic loop).
*
* @return Robot running time in microseconds, as of the start of the current periodic function.
*/
private void monitorThreadMain(Thread thr, long opmode, int event, int endEvent) {
ControlWord word = new ControlWord();
int[] events = {event, endEvent};
while (true) {
try {
int[] signaled = WPIUtilJNI.waitForObjects(events);
for (int val : signaled) {
if (val < 0) {
return; // handle destroyed
public long getLoopStartTime() {
return m_callbacks.getLoopStartTime();
}
/** Main robot loop function. Handles disabled state logic and opmode management. */
private void loopFunc() {
DriverStation.refreshData();
// Get current enabled state and opmode
DriverStation.refreshControlWordFromCache(m_word);
m_watchdog.reset();
boolean enabled = m_word.isEnabled();
long modeId = m_word.isDSAttached() ? m_word.getOpModeId() : 0;
if (!m_calledDriverStationConnected && m_word.isDSAttached()) {
m_calledDriverStationConnected = true;
driverStationConnected();
m_watchdog.addEpoch("driverStationConnected()");
}
// Handle opmode changes
if (modeId != m_lastModeId) {
// Clean up current opmode
if (m_currentOpMode != null) {
// Remove opmode callbacks
m_callbacks.remove(m_currentOpModePeriodic);
m_callbacks.removeAll(m_activeOpModeCallbacks);
m_activeOpModeCallbacks.clear();
m_currentOpMode.end();
m_currentOpMode.close();
m_currentOpMode = null;
}
// Set up new opmode
if (modeId != 0) {
OpModeFactory factory = m_opModes.get(modeId);
if (factory != null) {
// Instantiate the new opmode
System.out.println("********** Starting OpMode " + factory.name() + " **********");
m_currentOpMode = factory.supplier().get();
if (m_currentOpMode != null) {
// Ensure disabledPeriodic is called at least once
m_currentOpMode.disabledPeriodic();
m_watchdog.addEpoch("opMode.disabledPeriodic()");
// Register the opmode's periodic callbacks
m_currentOpModePeriodic =
m_callbacks.add(m_currentOpMode::periodic, m_startTimeUs, m_period);
m_activeOpModeCallbacks.addAll(m_currentOpMode.getCallbacks());
m_callbacks.addAll(m_activeOpModeCallbacks);
}
}
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
return;
}
// did the opmode or enable state change?
DriverStationJNI.getUncachedControlWord(word);
if (!word.isEnabled() || word.getOpModeId() != opmode) {
break;
}
}
// call opmode stop
OpMode opMode = m_activeOpMode.get();
if (opMode != null) {
opMode.opModeStop();
}
events[0] = m_notifier;
NotifierJNI.setNotifierAlarm(m_notifier, 200000, 0, false, true); // 200 ms
try {
int[] signaled = WPIUtilJNI.waitForObjects(events);
for (int val : signaled) {
if (val < 0 || val == endEvent) {
return; // transitioned, or handle destroyed
} else {
DriverStation.reportError("No OpMode found for mode " + modeId, false);
}
}
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
return;
m_lastModeId = modeId;
}
// if it hasn't transitioned after 200 ms, call thread.interrupt()
DriverStation.reportError("OpMode did not exit, interrupting thread", false);
thr.interrupt();
NotifierJNI.setNotifierAlarm(m_notifier, 800000, 0, false, true); // 800 ms
try {
int[] signaled = WPIUtilJNI.waitForObjects(events);
for (int val : signaled) {
if (val < 0 || val == endEvent) {
return; // transitioned, or handle destroyed
// Handle enabled state changes
boolean justCalledDisabledInit = false;
if (m_lastEnabledState != enabled) {
if (enabled) {
// Transitioning to enabled
disabledExit();
m_watchdog.addEpoch("disabledExit()");
if (m_currentOpMode != null) {
m_currentOpMode.start();
m_watchdog.addEpoch("opMode.start()");
}
} else {
// Transitioning to disabled
if (m_currentOpMode != null && m_lastEnabledState) {
// Was enabled, now disabled
m_currentOpMode.end();
m_watchdog.addEpoch("opMode.end()");
}
disabledInit();
m_watchdog.addEpoch("disabledInit()");
justCalledDisabledInit = true;
}
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
return;
m_lastEnabledState = enabled;
}
// if it hasn't transitioned after 1 second, terminate the program
DriverStation.reportError("OpMode did not exit, terminating program", false);
HAL.terminate();
HAL.shutdown();
System.exit(0);
// Call periodic functions based on current state
if (!enabled) {
// Only call disabledPeriodic if we didn't just call disabledInit
if (!justCalledDisabledInit) {
disabledPeriodic();
m_watchdog.addEpoch("disabledPeriodic()");
}
// Call opmode disabledPeriodic if we have one
if (m_currentOpMode != null) {
m_currentOpMode.disabledPeriodic();
m_watchdog.addEpoch("opMode.disabledPeriodic()");
}
}
// Call nonePeriodic when no opmode is selected
if (DriverStation.getOpModeId() == 0) {
nonePeriodic();
m_watchdog.addEpoch("nonePeriodic()");
}
// Always call robotPeriodic
robotPeriodic();
m_watchdog.addEpoch("robotPeriodic()");
// Always observe user program state
DriverStationJNI.observeUserProgram(m_word.getNative());
SmartDashboard.updateValues();
m_watchdog.addEpoch("SmartDashboard.updateValues()");
// Call simulationPeriodic if in simulation
if (isSimulation()) {
HAL.simPeriodicBefore();
simulationPeriodic();
HAL.simPeriodicAfter();
m_watchdog.addEpoch("simulationPeriodic()");
}
m_watchdog.disable();
// Flush NetworkTables
NetworkTableInstance.getDefault().flushLocal();
// Warn on loop time overruns
if (m_watchdog.isExpired()) {
m_watchdog.printEpochs();
}
}
/** Provide an alternate "main loop" via startCompetition(). */
@@ -595,145 +762,34 @@ public abstract class OpModeRobot extends RobotBase {
public final void startCompetition() {
System.out.println("********** Robot program startup complete **********");
int event = WPIUtilJNI.makeEvent(false, false);
DriverStationJNI.provideNewDataEventHandle(event);
m_notifier = NotifierJNI.createNotifier();
NotifierJNI.setNotifierName(m_notifier, "OpModeRobot");
try {
// Implement the opmode lifecycle
long lastModeId = -1;
boolean calledObserveUserProgramStarting = false;
boolean calledDriverStationConnected = false;
int[] events = {event, m_notifier};
while (true) {
// Wait for new data from the driver station, with 50 ms timeout
NotifierJNI.setNotifierAlarm(m_notifier, 50000, 0, false, true);
// Call observeUserProgramStarting() here as a one-shot to ensure it is called after the
// notifier alarm is set. The notifier alarm is set using relative time, so tests that
// wait on the user program to start and then step time won't work correctly if we call
// this before setting the alarm.
if (!calledObserveUserProgramStarting) {
calledObserveUserProgramStarting = true;
DriverStation.observeUserProgramStarting();
}
try {
int[] signaled = WPIUtilJNI.waitForObjects(events);
for (int val : signaled) {
if (val < 0) {
return; // handle destroyed
}
}
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
break;
}
// Get the latest control word and opmode
DriverStation.refreshData();
DriverStation.refreshControlWordFromCache(m_word);
if (!calledDriverStationConnected && m_word.isDSAttached()) {
calledDriverStationConnected = true;
driverStationConnected();
}
long modeId;
if (!m_word.isDSAttached()) {
modeId = 0;
} else {
modeId = m_word.getOpModeId();
}
OpMode opMode = m_activeOpMode.get();
if (opMode == null || modeId != lastModeId) {
if (opMode != null) {
// no or different opmode selected
m_activeOpMode.set(null);
opMode.opModeClose();
}
if (modeId == 0) {
// no opmode selected
nonePeriodic();
DriverStationJNI.observeUserProgram(m_word.getNative());
continue;
}
OpModeFactory factory = m_opModes.get(modeId);
if (factory == null) {
DriverStation.reportError("No OpMode found for mode " + modeId, false);
m_word.setOpModeId(0);
DriverStationJNI.observeUserProgram(m_word.getNative());
continue;
}
// Instantiate the opmode
System.out.println("********** Starting OpMode " + factory.name() + " **********");
opMode = factory.supplier().get();
if (opMode == null) {
// could not construct
m_word.setOpModeId(0);
DriverStationJNI.observeUserProgram(m_word.getNative());
continue;
}
m_activeOpMode.set(opMode);
lastModeId = modeId;
// Ensure disabledPeriodic is always called at least once
opMode.disabledPeriodic();
}
DriverStationJNI.observeUserProgram(m_word.getNative());
if (m_word.isEnabled()) {
// When enabled, call the opmode run function, then close and clear
int endMonitor = WPIUtilJNI.makeEvent(true, false);
Thread curThread = Thread.currentThread();
Thread monitor =
new Thread(
() -> {
monitorThreadMain(curThread, modeId, event, endMonitor);
});
monitor.start();
try {
opMode.opModeRun(modeId);
} catch (InterruptedException e) {
// ignored
} finally {
Thread.interrupted();
WPIUtilJNI.destroyEvent(endMonitor);
try {
monitor.join();
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
opMode = m_activeOpMode.getAndSet(null);
if (opMode != null) {
opMode.opModeClose();
}
} else {
// When disabled, call the disabledPeriodic function
opMode.disabledPeriodic();
}
}
} finally {
DriverStationJNI.removeNewDataEventHandle(event);
WPIUtilJNI.destroyEvent(event);
NotifierJNI.destroyNotifier(m_notifier);
if (isSimulation()) {
simulationInit();
}
// Tell the DS that the robot is ready to be enabled
DriverStation.observeUserProgramStarting();
// Loop forever, calling the callback system which handles periodic functions
while (true) {
if (!m_callbacks.runCallbacks(m_notifier)) {
break;
}
}
}
@Override
public void close() {
NotifierJNI.destroyNotifier(m_notifier);
}
/** Ends the main loop in startCompetition(). */
@Override
public final void endCompetition() {
NotifierJNI.destroyNotifier(m_notifier);
OpMode opMode = m_activeOpMode.get();
if (opMode != null) {
opMode.opModeStop();
}
}
/** Prints list of epochs added so far and their times. */
public void printWatchdogEpochs() {
m_watchdog.printEpochs();
}
}

125
wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java
View File

@@ -6,14 +6,13 @@ package org.wpilib.framework;
import static org.wpilib.units.Units.Seconds;
import java.util.PriorityQueue;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.hardware.hal.NotifierJNI;
import org.wpilib.internal.PeriodicPriorityQueue;
import org.wpilib.system.RobotController;
import org.wpilib.units.measure.Frequency;
import org.wpilib.units.measure.Time;
import org.wpilib.util.WPIUtilJNI;
/**
* TimedRobot implements the IterativeRobotBase robot program framework.
@@ -23,59 +22,17 @@ import org.wpilib.util.WPIUtilJNI;
* <p>periodic() functions from the base class are called on an interval by a Notifier instance.
*/
public class TimedRobot extends IterativeRobotBase {
@SuppressWarnings("MemberName")
static class Callback implements Comparable<Callback> {
public Runnable func;
public long period;
public long expirationTime;
/**
* Construct a callback container.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling in microseconds.
* @param period The period at which to run the callback in microseconds.
* @param offset The offset from the common starting time in microseconds.
*/
Callback(Runnable func, long startTime, long period, long offset) {
this.func = func;
this.period = period;
this.expirationTime =
startTime
+ offset
+ this.period
+ (RobotController.getMonotonicTime() - startTime) / this.period * this.period;
}
@Override
public boolean equals(Object rhs) {
return rhs instanceof Callback callback && expirationTime == callback.expirationTime;
}
@Override
public int hashCode() {
return Long.hashCode(expirationTime);
}
@Override
public int compareTo(Callback rhs) {
// Elements with sooner expiration times are sorted as lesser. The head of
// Java's PriorityQueue is the least element.
return Long.compare(expirationTime, rhs.expirationTime);
}
}
/** Default loop period. */
@SuppressWarnings("MemberName")
public static final double DEFAULT_PERIOD = 0.02;
// The C pointer to the notifier object. We don't use it directly, it is
// just passed to the JNI bindings.
private final int m_notifier = NotifierJNI.createNotifier();
private long m_startTimeUs;
private long m_loopStartTimeUs;
private final long m_startTimeUs;
private final PriorityQueue<Callback> m_callbacks = new PriorityQueue<>();
private final PeriodicPriorityQueue m_callbackQueue = new PeriodicPriorityQueue();
/** Constructor for TimedRobot. */
protected TimedRobot() {
@@ -87,6 +44,7 @@ public class TimedRobot extends IterativeRobotBase {
*
* @param period The period of the robot loop function.
*/
@SuppressWarnings("this-escape")
protected TimedRobot(double period) {
super(period);
m_startTimeUs = RobotController.getMonotonicTime();
@@ -132,45 +90,9 @@ public class TimedRobot extends IterativeRobotBase {
// Loop forever, calling the appropriate mode-dependent function
while (true) {
// We don't have to check there's an element in the queue first because
// there's always at least one (the constructor adds one). It's reenqueued
// at the end of the loop.
var callback = m_callbacks.poll();
NotifierJNI.setNotifierAlarm(m_notifier, callback.expirationTime, 0, true, true);
try {
WPIUtilJNI.waitForObject(m_notifier);
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
if (!m_callbackQueue.runCallbacks(m_notifier)) {
break;
}
long currentTime = RobotController.getMonotonicTime();
m_loopStartTimeUs = currentTime;
callback.func.run();
// Increment the expiration time by the number of full periods it's behind
// plus one to avoid rapid repeat fires from a large loop overrun. We
// assume currentTime ≥ expirationTime rather than checking for it since
// the callback wouldn't be running otherwise.
callback.expirationTime +=
callback.period
+ (currentTime - callback.expirationTime) / callback.period * callback.period;
m_callbacks.add(callback);
// Process all other callbacks that are ready to run
while (m_callbacks.peek().expirationTime <= currentTime) {
callback = m_callbacks.poll();
callback.func.run();
callback.expirationTime +=
callback.period
+ (currentTime - callback.expirationTime) / callback.period * callback.period;
m_callbacks.add(callback);
}
}
}
@@ -188,7 +110,7 @@ public class TimedRobot extends IterativeRobotBase {
* @return Robot running time in microseconds, as of the start of the current periodic function.
*/
public long getLoopStartTime() {
return m_loopStartTimeUs;
return m_callbackQueue.getLoopStartTime();
}
/**
@@ -201,7 +123,7 @@ public class TimedRobot extends IterativeRobotBase {
* @param period The period at which to run the callback in seconds.
*/
public final void addPeriodic(Runnable callback, double period) {
m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (period * 1e6), 0));
addPeriodic(callback, period, period);
}
/**
@@ -216,35 +138,6 @@ public class TimedRobot extends IterativeRobotBase {
* scheduling a callback in a different timeslot relative to TimedRobot.
*/
public final void addPeriodic(Runnable callback, double period, double offset) {
m_callbacks.add(
new Callback(callback, m_startTimeUs, (long) (period * 1e6), (long) (offset * 1e6)));
}
/**
* Add a callback to run at a specific period.
*
* <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param period The period at which to run the callback.
*/
public final void addPeriodic(Runnable callback, Time period) {
addPeriodic(callback, period.in(Seconds));
}
/**
* Add a callback to run at a specific period with a starting time offset.
*
* <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param period The period at which to run the callback.
* @param offset The offset from the common starting time. This is useful for scheduling a
* callback in a different timeslot relative to TimedRobot.
*/
public final void addPeriodic(Runnable callback, Time period, Time offset) {
addPeriodic(callback, period.in(Seconds), offset.in(Seconds));
m_callbackQueue.add(callback, m_startTimeUs, period, offset);
}
}

312
wpilibj/src/main/java/org/wpilib/internal/PeriodicPriorityQueue.java Normal file
View File

@@ -0,0 +1,312 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.internal;
import java.util.Collection;
import java.util.PriorityQueue;
import org.wpilib.hardware.hal.NotifierJNI;
import org.wpilib.system.RobotController;
import org.wpilib.util.WPIUtilJNI;
/**
* A priority queue for scheduling periodic callbacks based on their next execution time.
*
* <p>This class manages a collection of periodic callbacks that execute at specified intervals.
* Callbacks are scheduled using monotonic timestamps and automatically rescheduled after execution
* to maintain their periodic behavior. The queue uses a priority heap to efficiently determine the
* next callback to execute.
*
* <p>This is an internal scheduling primitive used by robot frameworks like TimedRobot.
*/
public class PeriodicPriorityQueue {
/** Internal priority queue ordered by callback expiration times. */
private final PriorityQueue<Callback> m_queue;
private long m_loopStartTimeMicros;
/** Constructs an empty callback queue. */
public PeriodicPriorityQueue() {
m_queue = new PriorityQueue<>();
}
/**
* Adds a periodic callback to the queue with a specified start time.
*
* @param func The function to call periodically.
* @param timestamp The common starting point for callback scheduling in monotonic timestamp
* microseconds.
* @param periodSeconds The callback period in seconds.
* @param offsetSeconds The offset from the common starting time in seconds.
* @return the callback object
*/
public Callback add(Runnable func, long timestamp, double periodSeconds, double offsetSeconds) {
Callback callback = new Callback(func, timestamp, periodSeconds, offsetSeconds);
add(callback);
return callback;
}
/**
* Adds a periodic callback to the queue with a specified start time.
*
* @param func The function to call periodically.
* @param timestamp The common starting point for callback scheduling in monotonic timestamp
* microseconds.
* @param periodSeconds The callback period in seconds.
* @return the callback object
*/
public Callback add(Runnable func, long timestamp, double periodSeconds) {
Callback callback = new Callback(func, timestamp, periodSeconds);
m_queue.add(callback);
return callback;
}
/**
* Adds a periodic callback to the queue, starting from the current monotonic time.
*
* @param func The function to call periodically.
* @param periodSeconds The callback period in seconds.
* @param offsetSeconds The offset from the current monotonic time in seconds.
* @return the callback object
*/
public Callback add(Runnable func, double periodSeconds, double offsetSeconds) {
return add(func, RobotController.getMonotonicTime(), periodSeconds, offsetSeconds);
}
/**
* Adds a periodic callback to the queue, starting from the current monotonic time.
*
* @param func The function to call periodically.
* @param periodSeconds The callback period in seconds.
* @return the callback object
*/
public Callback add(Runnable func, double periodSeconds) {
return add(func, RobotController.getMonotonicTime(), periodSeconds);
}
/**
* Adds a pre-constructed callback to the queue.
*
* @param callback The callback to add.
*/
public void add(Callback callback) {
m_queue.add(callback);
}
/**
* Adds multiple callbacks to the queue.
*
* @param callbacks The collection of callbacks to add.
*/
public void addAll(Collection<Callback> callbacks) {
m_queue.addAll(callbacks);
}
/**
* Removes all callbacks associated with the given function.
*
* @param func The function whose callbacks should be removed.
*/
public void remove(Runnable func) {
m_queue.removeIf(callback -> callback.m_func.equals(func));
}
/**
* Removes a specific callback from the queue.
*
* @param callback The callback to remove.
*/
public void remove(Callback callback) {
m_queue.remove(callback);
}
/**
* Removes multiple callbacks from the queue.
*
* @param callbacks The collection of callbacks to remove.
*/
public void removeAll(Collection<Callback> callbacks) {
m_queue.removeAll(callbacks);
}
/** Removes all callbacks from the queue. */
public void clear() {
m_queue.clear();
}
/**
* Executes all callbacks that are due, then waits for the next callback's scheduled time.
*
* <p>This method performs the following steps:
*
* <ol>
* <li>Retrieves the callback with the earliest expiration time from the queue
* <li>Sets a hardware notifier alarm to wait until that callback's expiration time
* <li>Blocks until the notifier signals or is interrupted
* <li>Executes the callback and reschedules it for its next period
* <li>Processes any additional callbacks that have become due during execution
* </ol>
*
* <p>When rescheduling callbacks, this method automatically compensates for execution delays by
* advancing the expiration time by the number of full periods that have elapsed, ensuring
* callbacks maintain their scheduled phase over time.
*
* @param notifier The HAL notifier handle to use for timing.
* @return whether the notifier was signaled before the timeout.
* @throws IllegalStateException if the queue is empty when this method is called.
*/
public boolean runCallbacks(int notifier) {
var callback = m_queue.poll();
if (callback == null) {
throw new IllegalStateException(
"No callbacks to run! Did you make sure to call add() first?");
}
NotifierJNI.setNotifierAlarm(notifier, callback.m_expirationTime, 0, true, true);
try {
WPIUtilJNI.waitForObject(notifier);
} catch (InterruptedException ex) {
return false;
}
m_loopStartTimeMicros = RobotController.getMonotonicTime();
callback.m_func.run();
// Increment the expiration time by the number of full periods it's behind
// plus one to avoid rapid repeat fires from a large loop overrun. We
// assume m_loopStartTime ≥ expirationTime rather than checking for it since
// the callback wouldn't be running otherwise.
callback.m_expirationTime +=
callback.m_period
+ (m_loopStartTimeMicros - callback.m_expirationTime)
/ callback.m_period
* callback.m_period;
m_queue.add(callback);
// Process all other callbacks that are ready to run
while (m_queue.peek().m_expirationTime <= m_loopStartTimeMicros) {
callback = m_queue.poll();
callback.m_func.run();
callback.m_expirationTime +=
callback.m_period
+ (m_loopStartTimeMicros - callback.m_expirationTime)
/ callback.m_period
* callback.m_period;
m_queue.add(callback);
}
return true;
}
/**
* Return the system clock time in microseconds for the start of the current periodic loop. This
* is in the same time base as Timer.getMonotonicTimeStamp(), but is stable through a loop. It is
* updated at the beginning of every periodic callback (including the normal periodic loop).
*
* @return Robot running time in microseconds, as of the start of the current periodic function.
*/
public long getLoopStartTime() {
return m_loopStartTimeMicros;
}
/**
* A periodic callback with scheduling metadata.
*
* <p>Each callback tracks its target function, period, and next expiration time. After execution,
* the expiration time is automatically advanced by full periods to maintain precise timing even
* when execution is delayed.
*/
public static class Callback implements Comparable<Callback> {
/** The function to execute when the callback fires. */
public final Runnable m_func;
/** The period at which to run the callback in microseconds. */
public final long m_period;
/** The next scheduled execution time in monotonic timestamp microseconds. */
public long m_expirationTime;
/**
* Construct a callback container.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling in microseconds.
* @param period The period at which to run the callback in microseconds.
* @param offset The offset from the common starting time in microseconds.
*/
public Callback(Runnable func, long startTime, long period, long offset) {
this.m_func = func;
this.m_period = period;
this.m_expirationTime =
startTime
+ offset
+ (1 + (RobotController.getMonotonicTime() - startTime - offset) / this.m_period)
* this.m_period;
}
/**
* Construct a callback container.
*
* @param func The callback to run.
* @param timestamp The common starting point for all callback scheduling in microseconds.
* @param periodSeconds The period at which to run the callback in seconds.
* @param offsetSeconds The offset from the common starting time in seconds.
*/
public Callback(Runnable func, long timestamp, double periodSeconds, double offsetSeconds) {
this(func, timestamp, (long) (periodSeconds * 1e6), (long) (offsetSeconds * 1e6));
}
/**
* Construct a callback container.
*
* @param func The callback to run.
* @param timestamp The common starting point for all callback scheduling in microseconds.
* @param periodSeconds The period at which to run the callback in seconds.
*/
public Callback(Runnable func, long timestamp, double periodSeconds) {
this(func, timestamp, (long) (periodSeconds * 1e6), 0);
}
/**
* Compares callbacks based on expiration time for equality.
*
* @param rhs The object to compare against.
* @return true if rhs is a Callback with the same expiration time.
*/
@Override
public boolean equals(Object rhs) {
return rhs instanceof Callback callback && m_expirationTime == callback.m_expirationTime;
}
/**
* Returns a hash code based on the expiration time.
*
* @return hash code for this callback.
*/
@Override
public int hashCode() {
return Long.hashCode(m_expirationTime);
}
/**
* Compares this callback to another based on expiration time.
*
* <p>Callbacks with earlier expiration times are considered "less than" those with later
* expiration times. This ordering is used by the priority queue to determine execution order.
*
* @param rhs The callback to compare to.
* @return negative if this expires before rhs, positive if after, zero if equal.
*/
@Override
public int compareTo(Callback rhs) {
// Elements with sooner expiration times are sorted as lesser. The head of
// Java's PriorityQueue is the least element.
return Long.compare(m_expirationTime, rhs.m_expirationTime);
}
}
}

85
wpilibj/src/main/java/org/wpilib/opmode/OpMode.java
View File

@@ -4,36 +4,81 @@
package org.wpilib.opmode;
import java.util.Set;
import org.wpilib.framework.OpModeRobot;
import org.wpilib.internal.PeriodicPriorityQueue;
/**
* Top-level interface for opmode classes. Users should generally extend one of the abstract
* implementations of this interface (e.g. {@link PeriodicOpMode}) rather than directly implementing
* this interface.
*
* <p><b>Lifecycle</b>:
*
* <ul>
* <li>constructed when opmode selected on driver station
* <li>disabledPeriodic() called periodically as long as DS is disabled. Note this is not called
* on a set time interval (it does not use the same time interval as periodic())
* <li>when DS transitions from disabled to enabled, start() is called once
* <li>while DS is enabled, periodic() is called periodically at {@link OpModeRobot#getPeriod},
* and additional periodic callbacks added via addPeriodic() are called periodically on their
* set time intervals
* <li>when DS transitions from enabled to disabled, or a different opmode is selected on the
* driver station when the DS is enabled, end() is called, followed by close(); the object is
* not reused
* <li>if a different opmode is selected on the driver station when the DS is disabled, only
* close() is called; the object is not reused
* </ul>
*
* <p>All lifecycle callbacks and periodic callbacks run synchronously on the same thread that
* invokes them. Interactions between opmodes and the robot framework do not require additional
* synchronization.
*
* <p>Additional callbacks can be registered by implementing {@link #getCallbacks()} to return a set
* of {@link PeriodicPriorityQueue.Callback} objects with custom timing. {@link PeriodicOpMode}
* provides a convenient implementation of this method and utility methods for adding periodic
* callbacks.
*/
public interface OpMode {
public interface OpMode extends AutoCloseable {
/**
* This function is called periodically while the opmode is selected on the DS (robot is
* disabled). Code that should only run once when the opmode is selected should go in the opmode
* constructor.
* This function is called periodically while the opmode is selected and the robot is disabled.
* Code that should only run once when the opmode is selected should go in the opmode constructor.
*/
default void disabledPeriodic() {}
/** Called once when this opmode transitions to enabled. */
default void start() {}
/**
* This function is called when the opmode starts (robot is enabled).
* This function is called periodically while the opmode is enabled at the rate returned by {@link
* OpModeRobot#getPeriod()}.
*/
default void periodic() {}
/**
* This function is called asynchronously when the robot disables or switches opmodes while this
* opmode is enabled. Implementations should stop blocking work promptly.
*/
default void end() {}
/**
* This function is called when the opmode is no longer selected on the DS or after an enabled run
* ends. The object will not be reused after this is called.
*/
@Override
default void close() {}
/**
* Returns a set of custom periodic callbacks to be executed while the opmode is enabled.
*
* @param opModeId opmode unique ID
* @throws InterruptedException when interrupted
* <p>This method allows opmodes to register arbitrary periodic callbacks with custom execution
* intervals. The callbacks are executed by the robot framework at their scheduled times, in
* addition to the primary {@link #periodic()} callback.
*
* @return A set of custom callbacks to execute, or an empty set if no custom callbacks are
* needed. The default implementation returns an empty set.
*/
void opModeRun(long opModeId) throws InterruptedException;
/**
* This function is called asynchronously when the robot is disabled, to request the opmode return
* from opModeRun().
*/
void opModeStop();
/**
* This function is called when the opmode is no longer selected on the DS or after opModeRun()
* returns. The object will not be reused after this is called.
*/
void opModeClose();
default Set<PeriodicPriorityQueue.Callback> getCallbacks() {
return Set.of();
}
}

310
wpilibj/src/main/java/org/wpilib/opmode/PeriodicOpMode.java
View File

@@ -4,28 +4,20 @@
package org.wpilib.opmode;
import static org.wpilib.units.Units.Seconds;
import java.util.PriorityQueue;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.ControlWord;
import org.wpilib.hardware.hal.DriverStationJNI;
import java.util.Collections;
import java.util.Set;
import java.util.TreeSet;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.hardware.hal.NotifierJNI;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.internal.PeriodicPriorityQueue;
import org.wpilib.system.RobotController;
import org.wpilib.system.Watchdog;
import org.wpilib.units.measure.Time;
import org.wpilib.util.WPIUtilJNI;
/**
* An opmode structure for periodic operation. This base class implements a loop that runs one or
* more functions periodically (on a set time interval aka loop period). The primary periodic
* callback function is the abstract periodic() function; the time interval for this callback is 20
* ms by default, but may be changed via passing a different time interval to the constructor.
* Additional periodic callbacks with different intervals can be added using the addPeriodic() set
* of functions.
* ms by default, but may be changed via passing a different time interval to OpModeRobot's
* constructor. Additional periodic callbacks with different intervals can be added using the
* addPeriodic() set of functions.
*
* <p>Lifecycle:
*
@@ -34,311 +26,61 @@ import org.wpilib.util.WPIUtilJNI;
* <li>disabledPeriodic() called periodically as long as DS is disabled. Note this is not called
* on a set time interval (it does not use the same time interval as periodic())
* <li>when DS transitions from disabled to enabled, start() is called once
* <li>while DS is enabled, periodic() is called periodically on the time interval set by the
* constructor, and additional periodic callbacks added via addPeriodic() are called
* periodically on their set time intervals
* <li>while DS is enabled, periodic() is called periodically on the time interval set by
* OpModeRobot's constructor, and additional periodic callbacks added via addPeriodic() are
* called periodically on their set time intervals
* <li>when DS transitions from enabled to disabled, or a different opmode is selected on the
* driver station when the DS is enabled, end() is called, followed by close(); the object is
* not reused
* <li>if a different opmode is selected on the driver station when the DS is disabled, only
* close() is called; the object is not reused
* </ul>
*
* <p>All lifecycle callbacks and periodic callbacks run synchronously on the same thread that
* invokes them. Interactions between opmodes and the robot framework do not require additional
* synchronization.
*/
public abstract class PeriodicOpMode implements OpMode {
@SuppressWarnings("MemberName")
static class Callback implements Comparable<Callback> {
public Runnable func;
public long period;
public long expirationTime;
private final Set<PeriodicPriorityQueue.Callback> m_callbacks;
private final long m_startTimeUs = RobotController.getMonotonicTime();
/**
* Construct a callback container.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling in microseconds.
* @param period The period at which to run the callback in microseconds.
* @param offset The offset from the common starting time in microseconds.
*/
Callback(Runnable func, long startTime, long period, long offset) {
this.func = func;
this.period = period;
this.expirationTime =
startTime
+ offset
+ this.period
+ (RobotController.getMonotonicTime() - startTime) / this.period * this.period;
}
@Override
public boolean equals(Object rhs) {
return rhs instanceof Callback callback && expirationTime == callback.expirationTime;
}
@Override
public int hashCode() {
return Long.hashCode(expirationTime);
}
@Override
public int compareTo(Callback rhs) {
// Elements with sooner expiration times are sorted as lesser. The head of
// Java's PriorityQueue is the least element.
return Long.compare(expirationTime, rhs.expirationTime);
}
}
/** Default loop period. */
public static final double DEFAULT_PERIOD = 0.02;
// The C pointer to the notifier object. We don't use it directly, it is
// just passed to the JNI bindings.
private int m_notifier = NotifierJNI.createNotifier();
private long m_startTimeUs;
private long m_loopStartTimeUs;
private final ControlWord m_word = new ControlWord();
private final double m_period;
private final Watchdog m_watchdog;
private long m_opModeId;
private boolean m_running = true;
private final PriorityQueue<Callback> m_callbacks = new PriorityQueue<>();
/**
* Constructor. Periodic opmodes may specify the period used for the periodic() function; the
* no-argument constructor uses a default period of 20 ms.
*/
/** Constructor for PeriodicOpMode. */
@SuppressWarnings("this-escape")
protected PeriodicOpMode() {
this(DEFAULT_PERIOD);
}
/**
* Constructor. Periodic opmodes may specify the period used for the periodic() function.
*
* @param period period (in seconds) for callbacks to the periodic() function
*/
protected PeriodicOpMode(double period) {
m_startTimeUs = RobotController.getMonotonicTime();
m_period = period;
m_watchdog = new Watchdog(period, this::printLoopOverrunMessage);
addPeriodic(this::loopFunc, period);
NotifierJNI.setNotifierName(m_notifier, "PeriodicOpMode");
m_callbacks = new TreeSet<>();
HAL.reportUsage("OpMode", "PeriodicOpMode");
}
/** Called periodically while the opmode is selected on the DS (robot is disabled). */
@Override
public void disabledPeriodic() {}
/**
* Called when the opmode is de-selected on the DS. The object is not reused even if the same
* opmode is selected again (a new object will be created).
*/
public void close() {}
/**
* Called a single time when the robot transitions from disabled to enabled. This is called prior
* to periodic() being called.
*/
public void start() {}
/** Called periodically while the robot is enabled. */
public abstract void periodic();
/**
* Called a single time when the robot transitions from enabled to disabled, or just before
* close() is called if a different opmode is selected while the robot is enabled.
*/
public void end() {}
/**
* Return the system clock time in micrseconds for the start of the current periodic loop. This is
* in the same time base as Timer.getMonotonicTimestamp(), but is stable through a loop. It is
* updated at the beginning of every periodic callback (including the normal periodic loop).
*
* @return Robot running time in microseconds, as of the start of the current periodic function.
*/
public long getLoopStartTime() {
return m_loopStartTimeUs;
public Set<PeriodicPriorityQueue.Callback> getCallbacks() {
return Collections.unmodifiableSet(m_callbacks);
}
/**
* Add a callback to run at a specific period.
*
* <p>This is scheduled on the same Notifier as periodic(), so periodic() and the callback run
* <p>This is scheduled on OpModeRobot's Notifier, so OpModeRobot and the callback run
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param period The period at which to run the callback in seconds.
*/
public final void addPeriodic(Runnable callback, double period) {
m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (period * 1e6), 0));
addPeriodic(callback, period, period);
}
/**
* Add a callback to run at a specific period with a starting time offset.
*
* <p>This is scheduled on the same Notifier as periodic(), so periodic() and the callback run
* <p>This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback run
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param period The period at which to run the callback in seconds.
* @param offset The offset from the common starting time in seconds. This is useful for
* scheduling a callback in a different timeslot relative to PeriodicOpMode.
* scheduling a callback in a different timeslot relative to TimedRobot.
*/
public final void addPeriodic(Runnable callback, double period, double offset) {
m_callbacks.add(
new Callback(callback, m_startTimeUs, (long) (period * 1e6), (long) (offset * 1e6)));
}
/**
* Add a callback to run at a specific period.
*
* <p>This is scheduled on the same Notifier as periodic(), so periodic() and the callback run
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param period The period at which to run the callback.
*/
public final void addPeriodic(Runnable callback, Time period) {
addPeriodic(callback, period.in(Seconds));
}
/**
* Add a callback to run at a specific period with a starting time offset.
*
* <p>This is scheduled on the same Notifier as periodic(), so periodic() and the callback run
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param period The period at which to run the callback.
* @param offset The offset from the common starting time. This is useful for scheduling a
* callback in a different timeslot relative to PeriodicOpMode.
*/
public final void addPeriodic(Runnable callback, Time period, Time offset) {
addPeriodic(callback, period.in(Seconds), offset.in(Seconds));
}
/**
* Gets time period between calls to Periodic() functions.
*
* @return The time period between calls to Periodic() functions.
*/
public double getPeriod() {
return m_period;
}
/** Loop function. */
protected void loopFunc() {
DriverStation.refreshData();
DriverStation.refreshControlWordFromCache(m_word);
m_word.setOpModeId(m_opModeId);
DriverStationJNI.observeUserProgram(m_word.getNative());
if (!DriverStation.isEnabled() || DriverStation.getOpModeId() != m_opModeId) {
m_running = false;
return;
}
m_watchdog.reset();
periodic();
m_watchdog.addEpoch("periodic()");
SmartDashboard.updateValues();
m_watchdog.addEpoch("SmartDashboard.updateValues()");
// if (isSimulation()) {
// HAL.simPeriodicBefore();
// simulationPeriodic();
// HAL.simPeriodicAfter();
// m_watchdog.addEpoch("simulationPeriodic()");
// }
m_watchdog.disable();
// Flush NetworkTables
NetworkTableInstance.getDefault().flushLocal();
// Warn on loop time overruns
if (m_watchdog.isExpired()) {
m_watchdog.printEpochs();
}
}
// implements OpMode interface
@Override
public final void opModeRun(long opModeId) {
m_opModeId = opModeId;
start();
while (m_running) {
// We don't have to check there's an element in the queue first because
// there's always at least one (the constructor adds one). It's reenqueued
// at the end of the loop.
var callback = m_callbacks.poll();
NotifierJNI.setNotifierAlarm(m_notifier, callback.expirationTime, 0, true, true);
try {
WPIUtilJNI.waitForObject(m_notifier);
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
break;
}
long currentTime = RobotController.getMonotonicTime();
m_loopStartTimeUs = RobotController.getMonotonicTime();
callback.func.run();
// Increment the expiration time by the number of full periods it's behind
// plus one to avoid rapid repeat fires from a large loop overrun. We
// assume currentTime ≥ expirationTime rather than checking for it since
// the callback wouldn't be running otherwise.
callback.expirationTime +=
callback.period
+ (currentTime - callback.expirationTime) / callback.period * callback.period;
m_callbacks.add(callback);
// Process all other callbacks that are ready to run
while (m_callbacks.peek().expirationTime <= currentTime) {
callback = m_callbacks.poll();
callback.func.run();
callback.expirationTime +=
callback.period
+ (currentTime - callback.expirationTime) / callback.period * callback.period;
m_callbacks.add(callback);
}
}
end();
}
@Override
public final void opModeStop() {
NotifierJNI.destroyNotifier(m_notifier);
m_notifier = 0;
}
@Override
public final void opModeClose() {
if (m_notifier != 0) {
NotifierJNI.destroyNotifier(m_notifier);
}
close();
}
/** Prints list of epochs added so far and their times. */
public void printWatchdogEpochs() {
m_watchdog.printEpochs();
}
private void printLoopOverrunMessage() {
DriverStation.reportWarning("Loop time of " + m_period + "s overrun\n", false);
m_callbacks.add(new PeriodicPriorityQueue.Callback(callback, m_startTimeUs, period, offset));
}
}

283
wpilibj/src/test/java/org/wpilib/framework/OpModeRobotTest.java Normal file
View File

@@ -0,0 +1,283 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package org.wpilib.framework;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import static org.junit.jupiter.api.Assertions.fail;
import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;
import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.api.parallel.ResourceLock;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.RobotMode;
import org.wpilib.opmode.OpMode;
import org.wpilib.simulation.DriverStationSim;
import org.wpilib.simulation.SimHooks;
import org.wpilib.util.Color;
@ResourceLock("timing")
class OpModeRobotTest {
static final double kPeriod = 0.02;
public static class MockOpMode implements OpMode {
public final AtomicInteger m_disabledPeriodicCount = new AtomicInteger(0);
public final AtomicInteger m_startCount = new AtomicInteger(0);
public final AtomicInteger m_periodicCount = new AtomicInteger(0);
public final AtomicInteger m_endCount = new AtomicInteger(0);
public final AtomicInteger m_closeCount = new AtomicInteger(0);
MockOpMode() {}
@Override
public void close() {
m_closeCount.incrementAndGet();
}
@Override
public void disabledPeriodic() {
m_disabledPeriodicCount.incrementAndGet();
}
@Override
public void start() {
m_startCount.incrementAndGet();
}
@Override
public void periodic() {
m_periodicCount.incrementAndGet();
}
@Override
public void end() {
m_endCount.incrementAndGet();
}
}
public static class OneArgOpMode implements OpMode {
@SuppressWarnings("unused")
OneArgOpMode(MockRobot robot) {}
}
static class MockRobot extends OpModeRobot {
public final AtomicInteger m_driverStationConnectedCount = new AtomicInteger(0);
public final AtomicInteger m_nonePeriodicCount = new AtomicInteger(0);
public final AtomicInteger m_robotPeriodicCount = new AtomicInteger(0);
MockRobot() {
super();
}
@Override
public void driverStationConnected() {
m_driverStationConnectedCount.incrementAndGet();
}
@Override
public void nonePeriodic() {
m_nonePeriodicCount.incrementAndGet();
}
@Override
public void robotPeriodic() {
m_robotPeriodicCount.incrementAndGet();
}
}
@BeforeEach
void setUp() {
SimHooks.pauseTiming();
SimHooks.setProgramStarted(false);
DriverStationSim.resetData();
}
@AfterEach
void tearDown() {
DriverStation.clearOpModes();
SimHooks.resumeTiming();
}
@AfterEach
@SuppressWarnings("PMD.AvoidAccessibilityAlteration")
void resetUserProgramFlag() throws ReflectiveOperationException {
var field = DriverStation.class.getDeclaredField("m_userProgramStarted");
field.setAccessible(true);
field.set(null, false);
}
@Test
void addOpMode() {
class MyMockRobot extends MockRobot {
MyMockRobot() {
addOpModeFactory(
() -> new MockOpMode(),
RobotMode.AUTONOMOUS,
"NoArgOpMode-Auto",
"Group",
"Description",
Color.WHITE,
Color.BLACK);
addOpModeFactory(
() -> new OneArgOpMode(this),
RobotMode.TEST,
"OneArgOpMode-Test",
"Group",
"Description",
Color.WHITE,
Color.BLACK);
addOpModeFactory(() -> new MockOpMode(), RobotMode.TELEOPERATED, "NoArgOpMode");
addOpModeFactory(() -> new OneArgOpMode(this), RobotMode.TELEOPERATED, "OneArgOpMode");
publishOpModes();
}
}
final MyMockRobot robot = new MyMockRobot();
var options = Arrays.asList(DriverStationSim.getOpModeOptions());
assertEquals(4, options.size());
int[] indexes = {-1, -1, -1, -1};
for (int i = 0; i < options.size(); i++) {
String name = options.get(i).name;
switch (name) {
case "NoArgOpMode-Auto" -> indexes[0] = i;
case "OneArgOpMode-Test" -> indexes[1] = i;
case "NoArgOpMode" -> indexes[2] = i;
case "OneArgOpMode" -> indexes[3] = i;
default -> fail("Unexpected op mode: " + name + " at index " + i);
}
}
int i = indexes[0];
assertNotEquals(-1, i);
assertEquals("Group", options.get(i).group);
assertEquals("Description", options.get(i).description);
assertEquals(0xffffff, options.get(i).textColor);
assertEquals(0x000000, options.get(i).backgroundColor);
i = indexes[1];
assertNotEquals(-1, i);
assertEquals("Group", options.get(i).group);
assertEquals("Description", options.get(i).description);
assertEquals(0xffffff, options.get(i).textColor);
assertEquals(0x000000, options.get(i).backgroundColor);
i = indexes[2];
assertNotEquals(-1, i);
assertEquals("", options.get(i).group);
assertEquals("", options.get(i).description);
assertEquals(-1, options.get(i).textColor);
assertEquals(-1, options.get(i).backgroundColor);
i = indexes[3];
assertNotEquals(-1, i);
assertEquals("", options.get(i).group);
assertEquals("", options.get(i).description);
assertEquals(-1, options.get(i).textColor);
assertEquals(-1, options.get(i).backgroundColor);
robot.close();
}
@Test
void clearOpModes() {
class MyMockRobot extends MockRobot {
MyMockRobot() {
addOpModeFactory(() -> new MockOpMode(), RobotMode.TELEOPERATED, "NoArgOpMode");
addOpModeFactory(() -> new OneArgOpMode(this), RobotMode.TELEOPERATED, "OneArgOpMode");
publishOpModes();
}
}
MyMockRobot robot = new MyMockRobot();
robot.clearOpModes();
var options = DriverStationSim.getOpModeOptions();
assertEquals(0, options.length);
robot.close();
}
@Test
void removeOpMode() {
class MyMockRobot extends MockRobot {
MyMockRobot() {
addOpModeFactory(() -> new MockOpMode(), RobotMode.TELEOPERATED, "NoArgOpMode");
addOpModeFactory(() -> new OneArgOpMode(this), RobotMode.TELEOPERATED, "OneArgOpMode");
publishOpModes();
}
}
MyMockRobot robot = new MyMockRobot();
robot.removeOpMode(RobotMode.TELEOPERATED, "NoArgOpMode");
robot.publishOpModes();
var options = DriverStationSim.getOpModeOptions();
assertEquals(1, options.length);
assertEquals("OneArgOpMode", options[0].name);
robot.close();
}
@Test
void nonePeriodic() throws InterruptedException {
class MyMockRobot extends MockRobot {
MyMockRobot() {
addOpModeFactory(() -> new MockOpMode(), RobotMode.TELEOPERATED, "NoArgOpMode");
addOpModeFactory(() -> new OneArgOpMode(this), RobotMode.TELEOPERATED, "OneArgOpMode");
publishOpModes();
}
}
MyMockRobot robot = new MyMockRobot();
Thread robotThread = new Thread(robot::startCompetition);
robotThread.start();
SimHooks.waitForProgramStart();
// Time step to get periodic calls on 20 ms robot loop
SimHooks.stepTiming(0.11); // 110ms
assertEquals(5, robot.m_nonePeriodicCount.get());
robot.endCompetition();
robotThread.join();
robot.close();
}
@Test
void robotPeriodic() throws InterruptedException {
class MyMockRobot extends MockRobot {
MyMockRobot() {
addOpModeFactory(() -> new MockOpMode(), RobotMode.TELEOPERATED, "TestOpMode");
publishOpModes();
}
}
MyMockRobot robot = new MyMockRobot();
Thread robotThread = new Thread(robot::startCompetition);
robotThread.start();
SimHooks.waitForProgramStart();
// RobotPeriodic should be called regardless of state
assertEquals(0, robot.m_robotPeriodicCount.get());
// Step timing to allow callbacks to execute
SimHooks.stepTiming(kPeriod);
assertEquals(1, robot.m_robotPeriodicCount.get());
// Additional time steps should continue calling robotPeriodic
SimHooks.stepTiming(kPeriod);
assertEquals(2, robot.m_robotPeriodicCount.get());
robot.endCompetition();
robotThread.join();
robot.close();
}
}