mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-04 03:11:43 +00:00
[wpilib] Change opmodes to purely periodic (#8652)
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
@@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user