mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[hal, wpilib] Add OpMode support (#7744)
User code: - OpModeRobot used as the robot base class - LinearOpMode and PeriodicOpMode are provided opmode base classes - In Java, annotations can be used to automatically register opmode classes Additional user code functionality: - OpMode (string) is available in addition to the overall auto/teleop/test robot mode - OpMode does not indicate enable (enable/disable is still separate) - The HAL API uses integer UIDs; these are exposed at the user API level as well for faster checks - User code creates opmodes on startup (these have name, category, description, etc). DS: - DS will present opmode selection lists for auto and teleop for match/practice. During a match, the DS will automatically activate the selected opmode in the corresponding match period. - For testing, an overall mode is selected (e.g. teleop/auto/test) and a single opmode is selected Future work: - Command framework support/integration - Python annotation support - Unit tests (needs race-free DS sim updates) - Porting of examples Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
This commit is contained in:
Peter Johnson
344
wpilibj/src/main/java/org/wpilib/opmode/PeriodicOpMode.java
Normal file
344
wpilibj/src/main/java/org/wpilib/opmode/PeriodicOpMode.java
Normal file
@@ -0,0 +1,344 @@
|
||||
// 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.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 org.wpilib.hardware.hal.HAL;
|
||||
import org.wpilib.hardware.hal.NotifierJNI;
|
||||
import org.wpilib.networktables.NetworkTableInstance;
|
||||
import org.wpilib.smartdashboard.SmartDashboard;
|
||||
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.
|
||||
*
|
||||
* <p>Lifecycle:
|
||||
*
|
||||
* <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 on the time interval set by the
|
||||
* 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>
|
||||
*/
|
||||
public abstract class PeriodicOpMode implements OpMode {
|
||||
@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.getFPGATime() - 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 kDefaultPeriod = 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.
|
||||
*/
|
||||
protected PeriodicOpMode() {
|
||||
this(kDefaultPeriod);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.getFPGATime();
|
||||
m_period = period;
|
||||
m_watchdog = new Watchdog(period, this::printLoopOverrunMessage);
|
||||
|
||||
addPeriodic(this::loopFunc, period);
|
||||
NotifierJNI.setNotifierName(m_notifier, "PeriodicOpMode");
|
||||
|
||||
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.getFPGATimestamp(), 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;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 in seconds.
|
||||
*/
|
||||
public final void addPeriodic(Runnable callback, double period) {
|
||||
m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (period * 1e6), 0));
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 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.
|
||||
*/
|
||||
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.getFPGATime();
|
||||
m_loopStartTimeUs = RobotController.getFPGATime();
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user