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

264
design-docs/opmodes.md
View File

@@ -18,7 +18,7 @@ Primary use cases for operator-selectable code include:
Notably these use cases span both matches and off-field testing.
Secondarily, some teams may want to use different code structures for different use cases (e.g. linear for autonomous and iterative for test or teleop).
Secondarily, some teams may want to use different code structures for different use cases.
# Background
@@ -114,11 +114,13 @@ Providing the top-level teleop, autonomous, and test selection available in the
- DS provides drop-down selector(s) for the user-defined opmodes; these are filtered based on the top-level mode selector (e.g. if auto is selected, a single drop-down selector of just the auto opmodes is provided). For match mode or when FMS-attached, two drop downs are provided (one for auto opmode selection and one for teleop opmode selection). The drop-down selector provides grouped categories as specified by the robot program.
- Robot programs are structured to have a top-level Robot class (e.g. motors/sensors/subsystems); this is also where robot-wide initialization is performed (in the class constructor or static initializer block). The Robot class provides overrideable periodic type functions for users to run code when the robot is disabled.
- Robot programs are structured to have a top-level Robot class (e.g. motors/sensors/subsystems); this is also where robot-wide initialization is performed (in the class constructor or static initializer block).
- OpModes are usually registered via annotation of classes. These annotations may specify a name (or default to the class name), group name (for grouping of routines in the selection list), and description. Annotations are used to specify whether the class is a teleop, auto, or test opmode.
- The Robot class has access to comprehensive lifecycle methods that are called at different stages of robot operation, providing behavior that occurs across all opmodes.
- OpModes may also be registered via annotation of functions (in the Robot class only) or via explicit function calls. As C++ does not support annotations, function call registration is the only available method in that language. If manually registered, `publishOpModes()` must be called to publish the list of opmodes to the DS.
- OpModes are usually registered via annotation of classes. These annotations may specify a name (or default to the class name), group name (for grouping of routines in the selection list), description, and optional display colors. Annotations are used to specify whether the class is a teleop, auto, or test opmode.
- OpModes may also be registered via explicit function calls. As C++ does not support annotations, function call registration is the only available method in that language. If manually registered, `publishOpModes()` must be called to publish the list of opmodes to the DS.
- For maximum flexibility, all code in the robot project has access to the enable/disable state, the overall robot teleop/auto/test mode, and the selected opmodes for teleop, auto, and test (even when the robot is disabled).
@@ -146,185 +148,147 @@ Java is used for illustrative purposes. Python and C++ should follow a similar
### OpModeRobot
The `OpModeRobot` class is the base class for the user's `Robot` class. It also implements the private library machinery for robot startup and robot execution (including creating and transitioning between opmodes in accordance with the opmode lifecycle, as described in the following section).
The `OpModeRobot` class is the base class for the user's `Robot` class. It extends `RobotBase` directly and implements the private library machinery for robot startup and robot execution (including creating and transitioning between opmodes in accordance with the opmode lifecycle, as described in the following section).
```java
public abstract class OpModeRobot {
public void nonePeriodic() {
// this code is called when no opmode is selected
public abstract class OpModeRobot extends RobotBase {
// OpMode registration methods
public void addOpModeFactory(Supplier<OpMode> factory, RobotMode mode,
String name, String group, String description,
Color textColor, Color backgroundColor) {...}
// add a particular opmode class (Java only)
public void addOpMode(Class<? extends OpMode> cls, RobotMode mode,
String name, String group, String description,
Color textColor, Color backgroundColor) {...}
public void addAnnotatedOpMode(Class<? extends OpMode> cls) {...}
// add all annotated opmodes in a package and nested packages
public void addAnnotatedOpModeClasses(Package pkg) {...}
public void removeOpMode(RobotMode mode, String name) {...}
public void publishOpModes() {...}
public void clearOpModes() {...}
// Robot lifecycle methods
public void driverStationConnected() {
// called once when the DS first connects
}
// these functions allow users to add opmodes without annotations
public void addAutonomousOpMode(Supplier<OpMode> factory, String name, String group, String description) {...}
public void addTeleoperatedOpMode(Supplier<OpMode> factory, String name, String group, String description) {...}
public void addTestOpMode(Supplier<OpMode> factory, String name, String group, String description) {...}
public void robotPeriodic() {
// called periodically every loop, regardless of enabled state or opmode
}
public void simulationInit() {
// called once during robot initialization in simulation
}
public void simulationPeriodic() {
// called periodically in simulation
}
public void disabledInit() {
// called once when the robot becomes disabled
}
public void disabledPeriodic() {
// called periodically while the robot is disabled
}
public void disabledExit() {
// called once when the robot exits disabled state
}
public void nonePeriodic() {
// called periodically when no opmode is selected
}
}
```
`OpModeRobot` (in Java) automatically scans the user's package and subpackages for `@Autonomous`, `@Teleop`, and `@TestOpMode` classes in its constructor and publishes that list to the DS. When opmodes are added or removed manually after construction, `publishOpModes()` must be called to push changes to the DS.
### OpMode Classes
There are library base classes for different coding styles of routines. The `OpMode` interface serves as the base interface for all opmodes. Most users will use either `LinearOpMode` or `PeriodicOpMode` abstract base class implementations of this interface, but this interface enables users to create more customized opmodes while still utilizing the core robot base class opmode-switching implementation.
The `OpMode` interface serves as the base interface for all opmodes. Most users will extend `PeriodicOpMode`, but users may also implement `OpMode` directly for custom behavior.
The full lifecycle of a opmode is as follows:
- Operator selects opmode on DS -> opmode object is constructed
- If different opmode is selected -> `opModeClose()` is called (which typically just calls `close()`), object is released to GC
- While opmode is selected and robot is disabled, `disabledPeriodic()` is called
- When the robot is enabled in the selected opmode, `opModeRun()` is called; the result of this is different for different opmode base classes:
- `start()` is called once (for `PeriodicOpMode`)
- `run()` is called (for `LinearOpMode`), or `periodic()` is called periodically (for `PeriodicOpMode`)
- When the robot is disabled, `opModeStop()` is called (which results in `end()` being called for `PeriodicOpMode`), followed by `opModeClose()` (which results in `close()` being called for both `PeriodicOpMode` and `LinearOpMode`), object is released to GC
The lifecycle of an opmode is:
- When operator selects opmode on DS, a new opmode object is constructed
- While selected and disabled, `disabledPeriodic()` is called
- On disabled → enabled transition, `start()` is called once
- While enabled, `periodic()` is called at `OpModeRobot#getPeriod()`, and additional callbacks from `getCallbacks()` are run at their own configured rates
- If robot disables or a different opmode is selected while enabled, `end()` is called then `close()` is called (Java), or the object is destroyed (C++/Python); the object is not reused
- If a different opmode is selected while disabled, only `close()` is called (Java), or the object is destroyed (C++); the object is not reused
Following `opModeClose()` being called, a *new* opmode object is constructed based on the DS teleop/auto/test/match selector and selected opmode. In teleop/auto/test, the drop-down selection will be the same as before the previous enable, so the same opmode class is constructed again. In match (or when FMS-connected), only the selected auto opmode object is initially constructed; once auto completes, the selected teleop opmode object is constructed. Thus only zero or one opmode objects will ever be "alive" at any given time.
Following `close()` being called (Java)/the opmode being destroyed (C++), a *new* opmode object is constructed based on the DS teleop/auto/test/match selector and selected opmode. In teleop/auto/test, the drop-down selection will be the same as before the previous enable, so the same opmode class is constructed again. In match (or when FMS-connected), only the selected auto opmode object is initially constructed; once auto completes, the selected teleop opmode object is constructed. Thus only zero or one opmode objects will ever be "alive" at any given time.
For consistency in operation, the library will ensure that `disabledPeriodic()` is always called at least once before `opModeRun()` is called.
For consistency in operation, the library will ensure that `disabledPeriodic()` is always called at least once before `start()` is called.
User implementations of opmode classes may have either a no-parameter constructor or a constructor that accepts the user's `Robot` class type. If available, the library will call the latter and pass the user's `Robot` object to it when constructing the class.
The library will use escalating steps to attempt to terminate a opmode if `opModeRun()` does not return within a reasonable timeframe after `opModeStop()` is called, up to and including termination of the robot executable process (which will result in an automatic restart of it at the system level). User code (particularly in `LinearOpMode` derived classes) should check for `isRunning()` returning false and return as quickly as possible to allow the opmode to terminate gracefully.
User implementations of opmode classes may have either a no-parameter constructor or a constructor that accepts (a subclass of) the user's `Robot` class type. If available, the library will call the latter and pass the user's `Robot` object to it when constructing the class.
```java
public interface OpMode {
// this function is called periodically while the opmode is selected on the DS (robot is disabled)
// Note: it may be called once when the robot is enabled to ensure it's always called once before
// opModeRun() is called
public interface OpMode extends AutoCloseable {
// called periodically while selected and robot is disabled
default void disabledPeriodic() {}
// this function is called when the opmode starts (robot is enabled)
void opModeRun(long opModeId) throws InterruptedException;
// called once when this opmode transitions to enabled
default void start() {}
// this function is called asynchronously when the robot is disabled,
// to request the opmode return from opModeRun()
void opModeStop();
// called periodically while enabled at OpModeRobot#getPeriod()
default void periodic() {}
// this function is called when the opmode is de-selected on the DS or after
// opModeRun() returns
void opModeClose();
}
```
```java
public abstract class LinearOpMode implements OpMode {
// the class is constructed when the opmode is selected on the DS
// called asynchronously when robot disables or switches opmodes while enabled
default void end() {}
// called when this opmode is no longer selected; object is not reused
@Override
public void disabledPeriodic() {
// this code is called periodically while the opmode is selected on the DS (robot is disabled)
}
default void close() {}
public void close() {
// this code is called when the opmode is de-selected on the DS
}
// this function is called once to run the opmode (robot is enabled)
public abstract void run() throws InterruptedException:
public boolean isRunning() {
// returns true until opModeStop() is called
}
// implements OpMode interface
@Override
public final void opModeRun(long opModeId) {
run();
}
@Override
public final void opModeStop() {
// pseudo-code
isRunning = false;
}
@Override
public final void opModeClose() {
close();
// optional additional callbacks with custom timing
default Set<PeriodicPriorityQueue.Callback> getCallbacks() {
return Set.of();
}
}
```
```java
public abstract class PeriodicOpMode implements OpMode {
// the class is constructed when the opmode is selected on the DS
// periodic opmodes may specify their period; if unspecified, a default period of 20 ms is used
protected PeriodicOpMode() {...}
protected PeriodicOpMode(double period) {...}
@Override
public void disabledPeriodic() {
// this code is called periodically while the opmode is selected on the DS (robot is disabled)
}
public Set<PeriodicPriorityQueue.Callback> getCallbacks() {...}
public void close() {
// this code is called when the opmode is de-selected on the DS
}
public void start() {
// this code is called when the opmode starts (robot is enabled)
}
// this function is called periodically while the opmode is running (robot is enabled)
public abstract void periodic();
public void end() {
// this code is called when the opmode ends (robot is disabled)
}
// additional periodic functions can be added using these functions
// additional periodic callbacks with custom rates/offsets
public final void addPeriodic(Runnable callback, double period) {...}
public final void addPeriodic(Runnable callback, double period, double offset) {...}
// returns the start time of the current loop in microseconds
public final long getLoopStartTime() {...}
// implements OpMode interface
@Override
public final void opModeRun(long opModeId) {
// psuedo-code
start();
while (isRunning) {
// wait for next periodic time
// set loop start time
periodic(); // or addPeriodic() callback, as appropriate
}
end();
}
@Override
public final void opModeStop() {
// pseudo-code
isRunning = false;
}
@Override
public final void opModeClose() {
close();
}
}
```
### Annotations
All annotations are class-level. All elements are optional and may be omitted. If name is omitted, the class name is used. If group is emitted, the opmode is listed under a default group in the DS. The description is blank if it is omitted.
All annotations are class-level. All elements are optional and may be omitted. If name is omitted, the class name is used. If group is omitted, the opmode is listed as ungrouped. The description is blank if it is omitted. Color strings are parsed by `Color.fromString()`.
```java
@Autonomous(String name, String group, String description)
@Teleop(String name, String group, String description)
@TestOpMode(String name, String group, String description)
@Autonomous(String name, String group, String description,
String textColor, String backgroundColor)
@Teleop(String name, String group, String description,
String textColor, String backgroundColor)
@TestOpMode(String name, String group, String description,
String textColor, String backgroundColor)
```
Example use cases:
```java
// name will be "MyAuto", default group
// name defaults to class name; ungrouped
@Autonomous
public class MyAuto extends OpModeBaseClass {...}
public class MyAuto extends PeriodicOpMode {...}
// will use default group
@Teleop("my teleop")
public class MyTeleop extends OpModeBaseClass {...}
// custom name and colors
@Teleop(name="Arcade", textColor="#FFFFFF", backgroundColor="#003366")
public class MyTeleop extends PeriodicOpMode {...}
@TestOpMode(name="my test", group="mechanisms", description="tests arm")
public class MyTest extends OpModeBaseClass {...}
@TestOpMode(name="Arm Test", group="mechanisms", description="tests arm")
public class MyTest extends PeriodicOpMode {...}
```
### DriverStation class
@@ -372,7 +336,7 @@ The following example code for non-command-based Java demonstrates the following
- Robot class
- A periodic autonomous opmode
- A periodic teleop opmode
- A linear test opmode
- A periodic test opmode
Robot:
@@ -435,13 +399,26 @@ public class Teleop extends PeriodicOpMode {
Test opmode:
```java
@TestOpMode("Blink dashboard indicator")
public class TestDashboardIndicator extends LinearOpMode {
@TestOpMode(name="Blink dashboard indicator")
public class TestDashboardIndicator extends PeriodicOpMode {
private final Timer timer = new Timer();
private boolean m_logged;
@Override
public void run() {
Telemetry.log("indicator", true);
Timer.sleep(0.5);
Telemetry.log("indicator", false);
public void start() {
timer.start();
m_logged = false;
}
@Override
public void periodic() {
if (!m_logged) {
Telemetry.log("indicator", true);
m_logged = true;
}
if (m_logged && timer.hasElapsed(0.5)) {
Telemetry.log("indicator", false);
}
}
}
```
@@ -498,8 +475,6 @@ Key differences from 2025 FRC:
- The per-opmode overrideable functions in Robot are replaced with separate annotated per-opmode classes (for non-command-based)
- `SendableChooser` is no longer required for selecting autonomous routines; instead this functionality is integrated into opmodes, as users can create/register multiple autonomous opmodes classes, and it's been extended to support multiple teleoperated and multiple test opmodes
- Selection of autonomous opmodes is integrated into the DS instead of being performed by the dashboard
- OpModes are no longer limited to just timed (periodic); a mix of different opmode functionality/approaches across different robot opmodes is possible (e.g. teleop can be periodic and autonomous linear)
- Linear opmodes are now available; these are improved versions of the old SimpleRobot/SampleRobot
# Migration from 2025 FTC (FTC SDK)
@@ -507,13 +482,14 @@ Key differences from 2025 FTC:
- There is no hardware map. Hardware objects are instead directly instantiated inside a top-level Robot class. This Robot class is provided as a parameter to the opmode constructor.
- Init is integrated into the opmode constructor. There is no equivalent to the the enabled init step; the opmode constructor is run as soon as the opmode is selected on the DS, while the robot is still disabled. Match periods will start as soon as the robot is enabled.
- There is no `@Disabled` annotation for opmodes; the opmode annotation can simply be commented out to achieve the same result.
- Opmodes are all periodic. While it may be possible to create linear-like behavior, there is no equivalent of the FTC SDK's `LinearOpMode` class.
# Drawbacks
# Alternatives
Use SendableChooser for more opmodes (teleop and test as well as autonomous); downsides of this:
- Doesn't allow flexibility of mixing linear and periodic opmodes
- Doesn't provide a first-class opmode lifecycle or opmode-specific callback scheduling model
- Harder to use than annotations (generic class)
- No grouping, no filtering by opmode (although these could be added)

2
hal/src/main/native/cpp/jni/HALUtil.cpp
View File

@@ -187,7 +187,7 @@ void ThrowBoundaryException(JNIEnv* env, double value, double lower,
jobject CreateOpModeOption(JNIEnv* env, const HAL_OpModeOption& option) {
static jmethodID constructor = env->GetMethodID(
opModeOptionCls, "<init>",
"(JLjava/lang/String;L/java/lang/String;Ljava/lang/String;II)V");
"(JLjava/lang/String;Ljava/lang/String;Ljava/lang/String;II)V");
JLocal<jstring> name{
env, MakeJString(env, wpi::util::to_string_view(&option.name))};
JLocal<jstring> group{

11
wpilibc/robotpy_pybind_build_info.bzl generated
View File

@@ -750,6 +750,17 @@ def wpilib_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], includ
("wpi::internal::DriverStationModeThread", "wpi__internal__DriverStationModeThread.hpp"),
],
),
struct(
class_name = "PeriodicPriorityQueue",
yml_file = "semiwrap/PeriodicPriorityQueue.yml",
header_root = "$(execpath :robotpy-native-wpilib.copy_headers)",
header_file = "$(execpath :robotpy-native-wpilib.copy_headers)/wpi/internal/PeriodicPriorityQueue.hpp",
tmpl_class_names = [],
trampolines = [
("wpi::internal::PeriodicPriorityQueue", "wpi__internal__PeriodicPriorityQueue.hpp"),
("wpi::internal::PeriodicPriorityQueue::Callback", "wpi__internal__PeriodicPriorityQueue__Callback.hpp"),
],
),
struct(
class_name = "OpMode",
yml_file = "semiwrap/OpMode.yml",

336
wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp
View File

@@ -4,11 +4,11 @@
#include "wpi/framework/OpModeRobot.hpp"
#include <cstdint>
#include <cstdlib>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <fmt/format.h>
@@ -17,206 +17,196 @@
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/HAL.h"
#include "wpi/hal/Notifier.hpp"
#include "wpi/hal/UsageReporting.hpp"
#include "wpi/nt/NetworkTableInstance.hpp"
#include "wpi/opmode/OpMode.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
#include "wpi/system/Errors.hpp"
#include "wpi/system/RobotController.hpp"
#include "wpi/util/SafeThread.hpp"
#include "wpi/util/Synchronization.h"
using namespace wpi;
namespace {
class MonitorThread : public wpi::util::SafeThreadEvent {
public:
MonitorThread(int64_t modeId, wpi::util::Event& dsEvent,
HAL_NotifierHandle notifier, std::weak_ptr<OpMode> activeOpMode)
: m_modeId{modeId},
m_dsEvent{dsEvent.GetHandle()},
m_notifier{notifier},
m_activeOpMode{std::move(activeOpMode)} {}
private:
void Main() override {
// Wait for DS to disable or change modes
WPI_EventHandle events[] = {m_dsEvent, m_stopEvent.GetHandle()};
WPI_Handle signaledBuf[2];
for (;;) {
auto signaled = wpi::util::WaitForObjects(events, signaledBuf);
if (signaled.empty()) {
return; // handles destroyed
}
for (auto signal : signaled) {
if ((signal & 0x80000000) != 0 || signal == m_stopEvent.GetHandle()) {
return; // handle destroyed or transitioned
}
}
// did the opmode or enable state change?
HAL_ControlWord word;
HAL_GetUncachedControlWord(&word);
if (!HAL_ControlWord_IsEnabled(word) ||
HAL_ControlWord_GetOpModeId(word) != m_modeId) {
break;
}
}
// call opmode stop
auto opMode = m_activeOpMode.lock();
if (opMode) {
opMode->OpModeStop();
}
events[0] = m_notifier;
int32_t status = 0;
HAL_SetNotifierAlarm(m_notifier, 1000000, 0, false, true, &status); // 1s
auto signaled = wpi::util::WaitForObjects(events, signaledBuf);
if (signaled.empty()) {
return; // handles destroyed
}
for (auto signal : signaled) {
if ((signal & 0x80000000) != 0 || signal == m_stopEvent.GetHandle()) {
return; // handle destroyed or transitioned
}
}
// if it hasn't transitioned after 1 second, terminate the program
WPILIB_ReportError(err::Error, "OpMode did not exit, terminating program");
HAL_Shutdown();
std::exit(0);
}
int64_t m_modeId;
WPI_EventHandle m_dsEvent;
HAL_NotifierHandle m_notifier;
std::weak_ptr<OpMode> m_activeOpMode;
};
} // namespace
void OpModeRobotBase::StartCompetition() {
fmt::print("********** Robot program startup complete **********\n");
wpi::util::Event event;
struct DSListener {
wpi::util::Event& event;
explicit DSListener(wpi::util::Event& event) : event{event} {
HAL_ProvideNewDataEventHandle(event.GetHandle());
}
~DSListener() { HAL_RemoveNewDataEventHandle(event.GetHandle()); }
} listener{event};
OpModeRobotBase::OpModeRobotBase(wpi::units::second_t period)
: m_period{period},
m_loopOverrunAlert{
fmt::format("Loop time of {:.6f}s overrun", m_period.value()),
Alert::Level::MEDIUM},
m_watchdog{period, [this] { m_loopOverrunAlert.Set(true); }} {
// Create our own notifier and callback queue
int32_t status = 0;
m_notifier = HAL_CreateNotifier(&status);
HAL_SetNotifierName(m_notifier, "OpModeRobot", &status);
int64_t lastModeId = -1;
bool calledObserveUserProgramStarting = false;
bool calledDriverStationConnected = false;
std::shared_ptr<OpMode> opMode;
WPI_EventHandle events[] = {event.GetHandle(),
static_cast<WPI_EventHandle>(m_notifier)};
WPI_Handle signaledBuf[2];
for (;;) {
// Wait for new data from the driver station, with 50 ms timeout
HAL_SetNotifierAlarm(m_notifier, 50000, 0, false, true, &status);
m_startTime = std::chrono::microseconds{RobotController::GetMonotonicTime()};
// Call DriverStation::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();
}
// Add LoopFunc as periodic callback
AddPeriodic([this] { LoopFunc(); }, period);
auto signaled = wpi::util::WaitForObjects(events, signaledBuf);
if (signaled.empty()) {
return; // handles destroyed
}
for (auto signal : signaled) {
if ((signal & 0x80000000) != 0) {
return; // handle destroyed
HAL_ReportUsage("Framework", "OpModeRobot");
}
OpModeRobotBase::OpModeRobotBase() : OpModeRobotBase(DEFAULT_PERIOD) {}
void OpModeRobotBase::AddPeriodic(std::function<void()> callback,
wpi::units::second_t period) {
m_callbacks.Add(std::move(callback), m_startTime, period);
}
void OpModeRobotBase::LoopFunc() {
DriverStation::RefreshData();
// Get current enabled state and opmode
const hal::ControlWord word = DriverStation::GetControlWord();
m_watchdog.Reset();
const bool enabled = word.IsEnabled();
int64_t modeId = word.IsDSAttached() ? word.GetOpModeId() : 0;
if (!m_calledDriverStationConnected && word.IsDSAttached()) {
m_calledDriverStationConnected = true;
DriverStationConnected();
m_watchdog.AddEpoch("DriverStationConnected()");
}
// Handle opmode changes
if (modeId != m_lastModeId) {
// Clean up current opmode
if (m_currentOpMode) {
// Remove opmode callbacks
if (m_opmodePeriodic) {
m_callbacks.Remove(*m_opmodePeriodic);
m_opmodePeriodic.reset();
}
}
// Get the latest control word and opmode
DriverStation::RefreshData();
hal::ControlWord ctlWord = DriverStation::GetControlWord();
if (!calledDriverStationConnected && ctlWord.IsDSAttached()) {
calledDriverStationConnected = true;
DriverStationConnected();
}
int64_t modeId;
if (!ctlWord.IsDSAttached()) {
modeId = 0;
} else {
modeId = ctlWord.GetOpModeId();
}
if (!opMode || modeId != lastModeId) {
if (opMode) {
// no or different opmode selected
opMode.reset();
}
if (modeId == 0) {
// no opmode selected
NonePeriodic();
HAL_ObserveUserProgram(ctlWord.GetValue());
continue;
for (auto& cb : m_activeOpModeCallbacks) {
m_callbacks.Remove(cb);
}
m_activeOpModeCallbacks.clear();
m_currentOpMode.reset();
}
// Set up new opmode
if (modeId != 0) {
auto data = m_opModes.lookup(modeId);
if (!data.factory) {
if (data.factory) {
// Instantiate the new opmode
fmt::print("********** Starting OpMode {} **********\n", data.name);
m_currentOpMode = data.factory();
if (m_currentOpMode) {
// Ensure disabledPeriodic is called at least once
m_currentOpMode->DisabledPeriodic();
m_watchdog.AddEpoch("OpMode::DisabledPeriodic()");
// Register the opmode's periodic callbacks
m_opmodePeriodic = wpi::internal::PeriodicPriorityQueue::Callback{
[op = m_currentOpMode.get()] { op->Periodic(); }, m_startTime,
m_period};
m_callbacks.Add(*m_opmodePeriodic);
m_activeOpModeCallbacks = m_currentOpMode->GetCallbacks();
for (auto& cb : m_activeOpModeCallbacks) {
m_callbacks.Add(cb);
}
}
} else {
WPILIB_ReportError(err::Error, "No OpMode found for mode {}", modeId);
ctlWord.SetOpModeId(0);
HAL_ObserveUserProgram(ctlWord.GetValue());
continue;
}
}
m_lastModeId = modeId;
}
// Instantiate the opmode
fmt::print("********** Starting OpMode {} **********\n", data.name);
opMode = data.factory();
if (!opMode) {
// could not construct
ctlWord.SetOpModeId(0);
HAL_ObserveUserProgram(ctlWord.GetValue());
continue;
// Handle enabled state changes
bool justCalledDisabledInit = false;
if (m_lastEnabledState != enabled) {
if (enabled) {
// Transitioning to enabled
DisabledExit();
m_watchdog.AddEpoch("DisabledExit()");
if (m_currentOpMode) {
m_currentOpMode->Start();
m_watchdog.AddEpoch("OpMode::Start()");
}
{
std::scoped_lock lock(m_opModeMutex);
m_activeOpMode = opMode;
} else {
// Transitioning to disabled
if (m_currentOpMode && m_lastEnabledState) {
// Was enabled, now disabled
m_currentOpMode->End();
m_watchdog.AddEpoch("OpMode::End()");
}
lastModeId = modeId;
// Ensure disabledPeriodic is called at least once
opMode->DisabledPeriodic();
DisabledInit();
m_watchdog.AddEpoch("DisabledInit()");
justCalledDisabledInit = true;
}
m_lastEnabledState = enabled;
}
// 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()");
}
HAL_ObserveUserProgram(ctlWord.GetValue());
// Call opmode DisabledPeriodic if we have one
if (m_currentOpMode) {
m_currentOpMode->DisabledPeriodic();
m_watchdog.AddEpoch("OpMode::DisabledPeriodic()");
}
}
if (ctlWord.IsEnabled()) {
// When enabled, call the opmode run function, then close and clear
wpi::util::SafeThreadOwner<MonitorThread> monitor;
monitor.Start(modeId, event, static_cast<HAL_NotifierHandle>(m_notifier),
opMode);
opMode->OpModeRun(modeId);
opMode.reset();
} else {
// When disabled, call the DisabledPeriodic function
opMode->DisabledPeriodic();
// Call NonePeriodic when no opmode is selected
if (modeId == 0) {
NonePeriodic();
m_watchdog.AddEpoch("NonePeriodic()");
}
// Always call RobotPeriodic
RobotPeriodic();
m_watchdog.AddEpoch("RobotPeriodic()");
// Always observe user program state
HAL_ObserveUserProgram(word.GetValue());
SmartDashboard::UpdateValues();
m_watchdog.AddEpoch("SmartDashboard::UpdateValues()");
if constexpr (IsSimulation()) {
HAL_SimPeriodicBefore();
SimulationPeriodic();
HAL_SimPeriodicAfter();
m_watchdog.AddEpoch("SimulationPeriodic()");
}
m_watchdog.Disable();
// Flush NetworkTables
wpi::nt::NetworkTableInstance::GetDefault().FlushLocal();
// Warn on loop time overruns
if (m_watchdog.IsExpired()) {
m_watchdog.PrintEpochs();
}
}
void OpModeRobotBase::StartCompetition() {
fmt::print("********** Robot program startup complete **********\n");
if constexpr (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;
}
}
}
void OpModeRobotBase::EndCompetition() {
m_notifier = {};
std::shared_ptr<OpMode> opMode;
{
std::scoped_lock lock(m_opModeMutex);
opMode = m_activeOpMode.lock();
}
if (opMode) {
opMode->OpModeStop();
if (m_notifier != HAL_INVALID_HANDLE) {
HAL_DestroyNotifier(m_notifier);
}
}

50
wpilibc/src/main/native/cpp/framework/TimedRobot.cpp
View File

@@ -10,9 +10,10 @@
#include <utility>
#include "wpi/driverstation/DriverStation.hpp"
#include "wpi/hal/Notifier.hpp"
#include "wpi/hal/DriverStation.hpp"
#include "wpi/hal/UsageReporting.hpp"
#include "wpi/system/Errors.hpp"
#include "wpi/system/RobotController.hpp"
using namespace wpi;
@@ -27,45 +28,9 @@ void TimedRobot::StartCompetition() {
// 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.
auto callback = m_callbacks.pop();
int32_t status = 0;
HAL_SetNotifierAlarm(m_notifier, callback.expirationTime.count(), 0, true,
true, &status);
WPILIB_CheckErrorStatus(status, "SetNotifierAlarm");
if (WPI_WaitForObject(m_notifier) == 0) {
if (!m_callbacks.RunCallbacks(m_notifier)) {
break;
}
m_loopStartTimeUs = RobotController::GetMonotonicTime();
std::chrono::microseconds currentTime{m_loopStartTimeUs};
callback.func();
// 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.push(std::move(callback));
// Process all other callbacks that are ready to run
while (m_callbacks.top().expirationTime <= currentTime) {
callback = m_callbacks.pop();
callback.func();
callback.expirationTime +=
callback.period + (currentTime - callback.expirationTime) /
callback.period * callback.period;
m_callbacks.push(std::move(callback));
}
}
}
@@ -96,15 +61,8 @@ TimedRobot::~TimedRobot() {
}
}
uint64_t TimedRobot::GetLoopStartTime() {
return m_loopStartTimeUs;
}
void TimedRobot::AddPeriodic(std::function<void()> callback,
wpi::units::second_t period,
wpi::units::second_t offset) {
m_callbacks.emplace(
callback, m_startTime,
std::chrono::microseconds{static_cast<int64_t>(period.value() * 1e6)},
std::chrono::microseconds{static_cast<int64_t>(offset.value() * 1e6)});
m_callbacks.Add(std::move(callback), m_startTime, period, offset);
}

127
wpilibc/src/main/native/cpp/internal/PeriodicPriorityQueue.cpp Normal file
View File

@@ -0,0 +1,127 @@
// 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.
#include "wpi/internal/PeriodicPriorityQueue.hpp"
#include <utility>
#include "wpi/hal/Notifier.h"
#include "wpi/system/Errors.hpp"
#include "wpi/system/RobotController.hpp"
#include "wpi/util/Synchronization.h"
using namespace wpi::internal;
PeriodicPriorityQueue::Callback::Callback(std::function<void()> func,
std::chrono::microseconds startTime,
std::chrono::microseconds period,
std::chrono::microseconds offset)
: func{std::move(func)},
period{period},
expirationTime(
startTime + offset + period +
(std::chrono::microseconds{RobotController::GetMonotonicTime()} -
startTime) /
period * period) {}
PeriodicPriorityQueue::Callback::Callback(std::function<void()> func,
std::chrono::microseconds startTime,
wpi::units::second_t period,
wpi::units::second_t offset)
: Callback{
std::move(func), startTime,
std::chrono::microseconds{static_cast<int64_t>(period.value() * 1e6)},
std::chrono::microseconds{
static_cast<int64_t>(offset.value() * 1e6)}} {}
PeriodicPriorityQueue::Callback::Callback(std::function<void()> func,
std::chrono::microseconds startTime,
wpi::units::second_t period)
: Callback{std::move(func), startTime, period,
std::chrono::microseconds{0}} {}
void PeriodicPriorityQueue::Add(std::function<void()> func,
std::chrono::microseconds startTime,
std::chrono::microseconds period) {
Add(std::move(func), startTime, period, std::chrono::microseconds{0});
}
void PeriodicPriorityQueue::Add(std::function<void()> func,
std::chrono::microseconds startTime,
std::chrono::microseconds period,
std::chrono::microseconds offset) {
m_queue.emplace(std::move(func), startTime, period, offset);
}
void PeriodicPriorityQueue::Add(std::function<void()> func,
std::chrono::microseconds startTime,
wpi::units::second_t period) {
Add(std::move(func), startTime, period, wpi::units::second_t{0});
}
void PeriodicPriorityQueue::Add(std::function<void()> func,
std::chrono::microseconds startTime,
wpi::units::second_t period,
wpi::units::second_t offset) {
m_queue.emplace(std::move(func), startTime, period, offset);
}
void PeriodicPriorityQueue::Add(Callback callback) {
m_queue.push(std::move(callback));
}
bool PeriodicPriorityQueue::Remove(const Callback& callback) {
return m_queue.remove(callback);
}
void PeriodicPriorityQueue::Clear() {
while (!m_queue.empty()) {
m_queue.pop();
}
}
bool PeriodicPriorityQueue::RunCallbacks(HAL_NotifierHandle notifier) {
// 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.
auto callback = m_queue.pop();
int32_t status = 0;
HAL_SetNotifierAlarm(notifier, callback.expirationTime.count(), 0, true, true,
&status);
WPILIB_CheckErrorStatus(status, "SetNotifierAlarm");
if (WPI_WaitForObject(notifier) == 0) {
return false;
}
const std::chrono::microseconds currentTime{
RobotController::GetMonotonicTime()};
m_loopStartTime = wpi::units::microsecond_t{currentTime};
callback.func();
// 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_queue.push(std::move(callback));
// Process all other callbacks that are ready to run
while (m_queue.top().expirationTime <= currentTime) {
callback = m_queue.pop();
callback.func();
callback.expirationTime +=
callback.period + (currentTime - callback.expirationTime) /
callback.period * callback.period;
m_queue.push(std::move(callback));
}
return true;
}

142
wpilibc/src/main/native/cpp/opmode/PeriodicOpMode.cpp
View File

@@ -6,154 +6,22 @@
#include <utility>
#include "wpi/driverstation/DriverStation.hpp"
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/Notifier.hpp"
#include "wpi/hal/UsageReporting.hpp"
#include "wpi/nt/NetworkTableInstance.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
#include "wpi/system/Errors.hpp"
#include "wpi/system/RobotController.hpp"
using namespace wpi;
PeriodicOpMode::Callback::Callback(std::function<void()> func,
std::chrono::microseconds startTime,
std::chrono::microseconds period,
std::chrono::microseconds offset)
: func{std::move(func)},
period{period},
expirationTime(
startTime + offset + period +
(std::chrono::microseconds{wpi::RobotController::GetMonotonicTime()} -
startTime) /
period * period) {}
PeriodicOpMode::~PeriodicOpMode() {
if (m_notifier != HAL_INVALID_HANDLE) {
HAL_DestroyNotifier(m_notifier);
}
}
PeriodicOpMode::PeriodicOpMode(wpi::units::second_t period)
: m_period{period},
m_watchdog(period, [this] { PrintLoopOverrunMessage(); }) {
m_startTime = std::chrono::microseconds{RobotController::GetMonotonicTime()};
AddPeriodic([=, this] { LoopFunc(); }, period);
int32_t status = 0;
m_notifier = HAL_CreateNotifier(&status);
WPILIB_CheckErrorStatus(status, "CreateNotifier");
HAL_SetNotifierName(m_notifier, "PeriodicOpMode", &status);
PeriodicOpMode::PeriodicOpMode()
: m_startTime{
std::chrono::microseconds{RobotController::GetMonotonicTime()}} {
HAL_ReportUsage("OpMode", "PeriodicOpMode");
}
void PeriodicOpMode::AddPeriodic(std::function<void()> callback,
wpi::units::second_t period,
wpi::units::second_t offset) {
m_callbacks.emplace(
callback, m_startTime,
m_callbacks.emplace_back(
std::move(callback), m_startTime,
std::chrono::microseconds{static_cast<int64_t>(period.value() * 1e6)},
std::chrono::microseconds{static_cast<int64_t>(offset.value() * 1e6)});
}
void PeriodicOpMode::LoopFunc() {
DriverStation::RefreshData();
HAL_ControlWord word;
HAL_GetControlWord(&word);
HAL_ControlWord_SetOpModeId(&word, m_opModeId);
HAL_ObserveUserProgram(word);
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 constexpr (IsSimulation()) {
// HAL_SimPeriodicBefore();
// SimulationPeriodic();
// HAL_SimPeriodicAfter();
// m_watchdog.AddEpoch("SimulationPeriodic()");
// }
m_watchdog.Disable();
// Flush NetworkTables
nt::NetworkTableInstance::GetDefault().FlushLocal();
// Warn on loop time overruns
if (m_watchdog.IsExpired()) {
m_watchdog.PrintEpochs();
}
}
void PeriodicOpMode::OpModeRun(int64_t 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.
auto callback = m_callbacks.pop();
int32_t status = 0;
HAL_SetNotifierAlarm(m_notifier, callback.expirationTime.count(), 0, true,
true, &status);
WPILIB_CheckErrorStatus(status, "SetNotifierAlarm");
if (WPI_WaitForObject(m_notifier) == 0) {
break;
}
m_loopStartTimeUs = RobotController::GetMonotonicTime();
std::chrono::microseconds currentTime{m_loopStartTimeUs};
callback.func();
// 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.push(std::move(callback));
// Process all other callbacks that are ready to run
while (m_callbacks.top().expirationTime <= currentTime) {
callback = m_callbacks.pop();
callback.func();
callback.expirationTime +=
callback.period + (currentTime - callback.expirationTime) /
callback.period * callback.period;
m_callbacks.push(std::move(callback));
}
}
End();
}
void PeriodicOpMode::OpModeStop() {
HAL_DestroyNotifier(m_notifier);
m_notifier = HAL_INVALID_HANDLE;
}
void PeriodicOpMode::PrintLoopOverrunMessage() {
WPILIB_ReportWarning("Loop time of {:.6f}s overrun", m_period.value());
}
void PeriodicOpMode::PrintWatchdogEpochs() {
m_watchdog.PrintEpochs();
}

117
wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp
View File

@@ -5,17 +5,22 @@
#pragma once
#include <concepts>
#include <cstdint>
#include <functional>
#include <memory>
#include <string>
#include <vector>
#include "wpi/driverstation/Alert.hpp"
#include "wpi/framework/RobotBase.hpp"
#include "wpi/hal/DriverStationTypes.hpp"
#include "wpi/hal/Notifier.h"
#include "wpi/hal/Types.hpp"
#include "wpi/internal/PeriodicPriorityQueue.hpp"
#include "wpi/opmode/OpMode.hpp"
#include "wpi/system/Watchdog.hpp"
#include "wpi/units/time.hpp"
#include "wpi/util/DenseMap.hpp"
#include "wpi/util/SafeThread.hpp"
#include "wpi/util/Synchronization.hpp"
#include "wpi/util/mutex.hpp"
namespace wpi::util {
@@ -37,7 +42,7 @@ concept OneArgOpMode = std::constructible_from<T, R&> && OpModeDerived<T>;
/**
* Concept indicating a class is derived from OpMode and has either a
* no-argument constructor or a constructorthat accepts R&.
* no-argument constructor or a constructor that accepts R&.
*
* @tparam T opmode class
* @tparam R robot class
@@ -60,6 +65,9 @@ class OpModeRobotBase : public RobotBase {
public:
using OpModeFactory = std::function<std::unique_ptr<OpMode>()>;
/// Default loop period.
static constexpr auto DEFAULT_PERIOD = 20_ms;
/**
* Provide an alternate "main loop" via StartCompetition().
*/
@@ -72,14 +80,20 @@ class OpModeRobotBase : public RobotBase {
/**
* Constructor.
*
* @param period The period of the robot loop function.
*/
OpModeRobotBase() = default;
explicit OpModeRobotBase(wpi::units::second_t period);
/**
* Constructor for an OpModeRobot with a default loop time of 0.02 seconds.
*/
explicit OpModeRobotBase();
OpModeRobotBase(OpModeRobotBase&&) = delete;
OpModeRobotBase& operator=(OpModeRobotBase&&) = delete;
/**
* Function called exactly once after the DS is connected.
*
* Code that needs to know the DS state should go here.
*
* Users should override this method for initialization that needs to occur
@@ -87,12 +101,65 @@ class OpModeRobotBase : public RobotBase {
*/
virtual void DriverStationConnected() {}
/**
* Function called periodically every loop, regardless of enabled state or
* OpMode selection.
*/
virtual void RobotPeriodic() {}
/**
* Function called once during robot initialization in simulation.
*/
virtual void SimulationInit() {}
/**
* Function called periodically in simulation.
*/
virtual void SimulationPeriodic() {}
/**
* Function called once when the robot becomes disabled.
*/
virtual void DisabledInit() {}
/**
* Function called periodically while the robot is disabled.
*/
virtual void DisabledPeriodic() {}
/**
* Function called once when the robot exits disabled state.
*/
virtual void DisabledExit() {}
/**
* Function called periodically anytime when no opmode is selected, including
* when the Driver Station is disconnected.
*/
virtual void NonePeriodic() {}
/**
* Add a callback to run at a specific period.
*
* @param callback The callback to run.
* @param period The period at which to run the callback.
*/
void AddPeriodic(std::function<void()> callback, wpi::units::second_t period);
/**
* 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.
*/
wpi::units::microsecond_t GetLoopStartTime() const {
return m_callbacks.GetLoopStartTime();
}
/**
* Adds an operating mode option using a factory function that creates the
* opmode. It's necessary to call PublishOpModes() to make the added modes
@@ -147,15 +214,37 @@ class OpModeRobotBase : public RobotBase {
*/
void ClearOpModes();
protected:
/**
* Main robot loop function. Handles disabled state logic.
*/
void LoopFunc();
private:
struct OpModeData {
std::string name;
OpModeFactory factory;
};
wpi::util::DenseMap<int64_t, OpModeData> m_opModes;
wpi::hal::Handle<HAL_NotifierHandle, HAL_DestroyNotifier> m_notifier;
wpi::util::mutex m_opModeMutex;
std::weak_ptr<OpMode> m_activeOpMode;
wpi::internal::PeriodicPriorityQueue m_callbacks;
HAL_NotifierHandle m_notifier;
wpi::units::second_t m_period;
std::chrono::microseconds m_startTime;
Alert m_loopOverrunAlert;
Watchdog m_watchdog;
// OpMode lifecycle state
int64_t m_lastModeId = -1;
bool m_calledDriverStationConnected = false;
bool m_lastEnabledState = false;
std::shared_ptr<OpMode> m_currentOpMode;
std::vector<wpi::internal::PeriodicPriorityQueue::Callback>
m_activeOpModeCallbacks;
std::optional<wpi::internal::PeriodicPriorityQueue::Callback>
m_opmodePeriodic;
};
/**
@@ -176,6 +265,18 @@ class OpModeRobotBase : public RobotBase {
template <typename Derived>
class OpModeRobot : public OpModeRobotBase {
public:
/**
* Constructor.
*
* @param period The period of the robot loop function.
*/
explicit OpModeRobot(wpi::units::second_t period) : OpModeRobotBase{period} {}
/**
* Constructor for an OpModeRobot with a default loop time of 0.02 seconds.
*/
explicit OpModeRobot() : OpModeRobotBase{} {}
/**
* Adds an operating mode option. It's necessary to call PublishOpModes() to
* make the added modes visible to the driver station.

51
wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp
View File

@@ -4,18 +4,14 @@
#pragma once
#include <chrono>
#include <functional>
#include <utility>
#include <vector>
#include "wpi/framework/IterativeRobotBase.hpp"
#include "wpi/hal/Notifier.h"
#include "wpi/hal/Notifier.hpp"
#include "wpi/hal/Types.hpp"
#include "wpi/system/RobotController.hpp"
#include "wpi/internal/PeriodicPriorityQueue.hpp"
#include "wpi/units/frequency.hpp"
#include "wpi/units/time.hpp"
#include "wpi/util/priority_queue.hpp"
namespace wpi {
@@ -64,7 +60,7 @@ class TimedRobot : public IterativeRobotBase {
/**
* Return the system clock time in microseconds for the start of the current
* periodic loop. This is in the same time base as
* 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).
@@ -72,7 +68,9 @@ class TimedRobot : public IterativeRobotBase {
* @return Robot running time in microseconds, as of the start of the current
* periodic function.
*/
uint64_t GetLoopStartTime();
wpi::units::microsecond_t GetLoopStartTime() const {
return m_callbacks.GetLoopStartTime();
}
/**
* Add a callback to run at a specific period with a starting time offset.
@@ -89,43 +87,12 @@ class TimedRobot : public IterativeRobotBase {
void AddPeriodic(std::function<void()> callback, wpi::units::second_t period,
wpi::units::second_t offset = 0_s);
private:
class Callback {
public:
std::function<void()> func;
std::chrono::microseconds period;
std::chrono::microseconds expirationTime;
/**
* Construct a callback container.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling.
* @param period The period at which to run the callback.
* @param offset The offset from the common starting time.
*/
Callback(std::function<void()> func, std::chrono::microseconds startTime,
std::chrono::microseconds period, std::chrono::microseconds offset)
: func{std::move(func)},
period{period},
expirationTime(startTime + offset + period +
(std::chrono::microseconds{
wpi::RobotController::GetMonotonicTime()} -
startTime) /
period * period) {}
bool operator>(const Callback& rhs) const {
return expirationTime > rhs.expirationTime;
}
};
protected:
wpi::hal::Handle<HAL_NotifierHandle, HAL_DestroyNotifier> m_notifier;
std::chrono::microseconds m_startTime;
uint64_t m_loopStartTimeUs = 0;
wpi::util::priority_queue<Callback, std::vector<Callback>,
std::greater<Callback>>
m_callbacks;
private:
wpi::internal::PeriodicPriorityQueue m_callbacks;
};
} // namespace wpi

206
wpilibc/src/main/native/include/wpi/internal/PeriodicPriorityQueue.hpp Normal file
View File

@@ -0,0 +1,206 @@
// 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.
#pragma once
#include <chrono>
#include <functional>
#include <vector>
#include "wpi/hal/Notifier.h"
#include "wpi/units/time.hpp"
#include "wpi/util/priority_queue.hpp"
namespace wpi::internal {
/**
* 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 FPGA 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.
*/
class PeriodicPriorityQueue {
public:
/**
* A periodic callback with scheduling metadata.
*
* 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.
*/
class Callback {
public:
/** The function to execute when the callback fires. */
std::function<void()> func;
/** The period at which to run the callback. */
std::chrono::microseconds period;
/** The next scheduled execution time in FPGA timestamp microseconds. */
std::chrono::microseconds expirationTime;
/**
* Construct a callback container.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling.
* @param period The period at which to run the callback.
* @param offset The offset from the common starting time.
*/
Callback(std::function<void()> func, std::chrono::microseconds startTime,
std::chrono::microseconds period,
std::chrono::microseconds offset);
/**
* Construct a callback container using units-based period and offset.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling.
* @param period The period at which to run the callback.
* @param offset The offset from the common starting time.
*/
Callback(std::function<void()> func, std::chrono::microseconds startTime,
wpi::units::second_t period, wpi::units::second_t offset);
/**
* Construct a callback container using units-based period.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling.
* @param period The period at which to run the callback.
*/
Callback(std::function<void()> func, std::chrono::microseconds startTime,
wpi::units::second_t period);
bool operator>(const Callback& rhs) const {
return expirationTime > rhs.expirationTime;
}
bool operator==(const Callback& rhs) const {
return expirationTime == rhs.expirationTime;
}
};
/**
* Adds a periodic callback to the queue.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling.
* @param period The period at which to run the callback.
*/
void Add(std::function<void()> func, std::chrono::microseconds startTime,
std::chrono::microseconds period);
/**
* Adds a periodic callback to the queue.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling.
* @param period The period at which to run the callback.
* @param offset The offset from the common starting time.
*/
void Add(std::function<void()> func, std::chrono::microseconds startTime,
std::chrono::microseconds period, std::chrono::microseconds offset);
/**
* Adds a periodic callback to the queue.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling in
* FPGA timestamp microseconds.
* @param period The period at which to run the callback.
*/
void Add(std::function<void()> func, std::chrono::microseconds startTime,
wpi::units::second_t period);
/**
* Adds a periodic callback to the queue.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling in
* FPGA timestamp microseconds.
* @param period The period at which to run the callback.
* @param offset The offset from the common starting time.
*/
void Add(std::function<void()> func, std::chrono::microseconds startTime,
wpi::units::second_t period, wpi::units::second_t offset);
/**
* Adds a pre-constructed callback to the queue.
*
* @param callback The callback to add.
*/
void Add(Callback callback);
/**
* Removes a specific callback from the queue.
*
* @param callback The callback to remove.
* @return true if the callback was found and removed, false otherwise.
*/
bool Remove(const Callback& callback);
/**
* Removes all callbacks from the queue.
*/
void Clear();
/**
* Executes all callbacks that are due, then waits for the next callback's
* scheduled time.
*
* This method performs the following steps:
* -# Retrieves the callback with the earliest expiration time from the queue
* -# Sets a hardware notifier alarm to wait until that callback's expiration
* time
* -# Blocks until the notifier signals or is interrupted
* -# Executes the callback and reschedules it for its next period
* -# Processes any additional callbacks that have become due during execution
*
* 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 false if the notifier was destroyed (loop should exit), true
* otherwise.
*/
bool RunCallbacks(HAL_NotifierHandle notifier);
/**
* Returns the underlying priority queue.
*/
wpi::util::priority_queue<Callback, std::vector<Callback>, std::greater<>>&
GetQueue() {
return m_queue;
}
/**
* 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.
*/
wpi::units::microsecond_t GetLoopStartTime() const { return m_loopStartTime; }
private:
wpi::util::priority_queue<Callback, std::vector<Callback>, std::greater<>>
m_queue;
wpi::units::microsecond_t m_loopStartTime{0};
};
} // namespace wpi::internal

63
wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp
View File

@@ -4,7 +4,9 @@
#pragma once
#include <stdint.h>
#include <vector>
#include "wpi/internal/PeriodicPriorityQueue.hpp"
namespace wpi {
@@ -12,34 +14,69 @@ namespace wpi {
* Top-level interface for opmode classes. Users should generally extend one of
* the abstract implementations of this interface (e.g. 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
* <li>when DS transitions from disabled to enabled, Start() is called once
* <li>while DS is enabled, Periodic() is called periodically and additional
* periodic callbacks added via GetCallbacks() are called periodically
* <li>when DS transitions from enabled to disabled, or a different opmode is
* selected while enabled, End() is called, followed by Close(); the
* object is not reused
* <li>if a different opmode is selected while disabled, only Close() is
* called; the object is not reused
* </ul>
*/
class OpMode {
public:
/**
* The object is destroyed when the opmode is no longer selected on the DS or
* after OpModeRun() returns.
* after an enabled run ends. The object will not be reused after the
* destructor is called.
*/
virtual ~OpMode() = default;
/**
* 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
* 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.
*/
virtual void DisabledPeriodic() {}
/**
* This function is called when the opmode starts (robot is enabled).
*
* @param opModeId opmode unique ID
*/
virtual void OpModeRun(int64_t opModeId) = 0;
/** Called once when this opmode transitions to enabled. */
virtual void Start() {}
/**
* This function is called asynchronously when the robot is disabled, to
* request the opmode return from OpModeRun().
* This function is called periodically while the opmode is enabled.
*/
virtual void OpModeStop() = 0;
virtual void Periodic() {}
/**
* This function is called when the robot disables or switches opmodes while
* this opmode is enabled. Implementations should stop blocking work
* promptly.
*/
virtual void End() {}
/**
* Returns a vector of custom periodic callbacks to be executed while the
* opmode is enabled.
*
* <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 Periodic()
* callback.
*
* @return A vector of custom callbacks to execute, or an empty vector if no
* custom callbacks are needed. The default implementation returns an
* empty vector.
*/
virtual std::vector<wpi::internal::PeriodicPriorityQueue::Callback>
GetCallbacks() {
return {};
}
};
} // namespace wpi

145
wpilibc/src/main/native/include/wpi/opmode/PeriodicOpMode.hpp
View File

@@ -4,29 +4,24 @@
#pragma once
#include <stdint.h>
#include <chrono>
#include <functional>
#include <utility>
#include <vector>
#include "wpi/hal/Notifier.h"
#include "wpi/hal/Types.hpp"
#include "wpi/internal/PeriodicPriorityQueue.hpp"
#include "wpi/opmode/OpMode.hpp"
#include "wpi/system/Watchdog.hpp"
#include "wpi/system/RobotController.hpp"
#include "wpi/units/time.hpp"
#include "wpi/util/priority_queue.hpp"
namespace wpi {
/**
* 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 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.
* period). The primary periodic callback function is the abstract Periodic()
* function. Additional periodic callbacks with different intervals can be added
* using the AddPeriodic() set of functions.
*
* Lifecycle:
*
@@ -39,33 +34,25 @@ namespace wpi {
* - When DS transitions from disabled to enabled, Start() is called once
*
* - While DS is enabled, Periodic() is called periodically on the time interval
* set by the constructor, and additional periodic callbacks added via
* set by the robot framework, and additional periodic callbacks added via
* AddPeriodic() are called periodically on their set time intervals
*
* - 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 the object being destroyed; the object is not reused
* followed by Close(); the object is not reused
*
* - If a different opmode is selected on the driver station when the DS is
* disabled, the object is destroyed (without End() being called); the object
* is not reused
* disabled, only Close() is called; the object is not reused
*/
class PeriodicOpMode : public OpMode {
public:
/** Default loop period. */
static constexpr auto DEFAULT_PERIOD = 20_ms;
protected:
/**
* Constructor. Periodic opmodes may specify the period used for the
* Periodic() function.
*
* @param period period for callbacks to the Periodic() function
* Constructor.
*/
explicit PeriodicOpMode(wpi::units::second_t period = DEFAULT_PERIOD);
PeriodicOpMode();
public:
~PeriodicOpMode() override;
~PeriodicOpMode() override = default;
/**
* Called periodically while the opmode is selected on the DS (robot is
@@ -77,35 +64,52 @@ class PeriodicOpMode : public OpMode {
* Called a single time when the robot transitions from disabled to enabled.
* This is called prior to Periodic() being called.
*/
virtual void Start() {}
void Start() override {}
/** Called periodically while the robot is enabled. */
virtual void Periodic() = 0;
/**
* Called periodically while the robot is enabled. The framework calls this
* at OpModeRobot's configured loop period.
*/
void Periodic() override {}
/**
* Called a single time when the robot transitions from enabled to disabled,
* or just before the destructor is called if a different opmode is selected
* while the robot is enabled.
* or just before Close() is called if a different opmode is selected while
* the robot is enabled.
*/
virtual void End() {}
void End() override {}
/**
* 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).
* Returns the set of additional periodic callbacks registered via
* AddPeriodic(). These are executed by the robot framework while the opmode
* is enabled, in addition to the primary Periodic() callback.
*
* @return Robot running time in microseconds, as of the start of the current
* periodic function.
* @return The vector of additional periodic callbacks.
*/
int64_t GetLoopStartTime() const { return m_loopStartTimeUs; }
std::vector<wpi::internal::PeriodicPriorityQueue::Callback> GetCallbacks()
override {
return m_callbacks;
}
/**
* Add a callback to run at a specific period.
*
* 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.
*/
void AddPeriodic(std::function<void()> callback,
wpi::units::second_t period) {
AddPeriodic(std::move(callback), period, period);
}
/**
* Add a callback to run at a specific period with a starting time offset.
*
* This is scheduled on the same Notifier as Periodic(), so Periodic() and the
* callback run synchronously. Interactions between them are thread-safe.
* 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.
@@ -114,66 +118,11 @@ class PeriodicOpMode : public OpMode {
* to TimedRobot.
*/
void AddPeriodic(std::function<void()> callback, wpi::units::second_t period,
wpi::units::second_t offset = 0_s);
/**
* Gets time period between calls to Periodic() functions.
*/
wpi::units::second_t GetPeriod() const { return m_period; }
/**
* Prints list of epochs added so far and their times.
*/
void PrintWatchdogEpochs();
protected:
/** Loop function. */
void LoopFunc();
public:
// implements OpMode interface
void OpModeRun(int64_t opModeId) final;
void OpModeStop() final;
wpi::units::second_t offset);
private:
class Callback {
public:
std::function<void()> func;
std::chrono::microseconds period;
std::chrono::microseconds expirationTime;
/**
* Construct a callback container.
*
* @param func The callback to run.
* @param startTime The common starting point for all callback scheduling.
* @param period The period at which to run the callback.
* @param offset The offset from the common starting time.
*/
Callback(std::function<void()> func, std::chrono::microseconds startTime,
std::chrono::microseconds period,
std::chrono::microseconds offset);
bool operator>(const Callback& rhs) const {
return expirationTime > rhs.expirationTime;
}
};
int64_t m_opModeId;
bool m_running = true;
wpi::hal::Handle<HAL_NotifierHandle, HAL_DestroyNotifier> m_notifier;
std::vector<wpi::internal::PeriodicPriorityQueue::Callback> m_callbacks;
std::chrono::microseconds m_startTime;
int64_t m_loopStartTimeUs = 0;
wpi::units::second_t m_period;
Watchdog m_watchdog;
wpi::util::priority_queue<Callback, std::vector<Callback>,
std::greater<Callback>>
m_callbacks;
void PrintLoopOverrunMessage();
};
} // namespace wpi

1
wpilibc/src/main/python/pyproject.toml
View File

@@ -197,6 +197,7 @@ Encoder = "wpi/hardware/rotation/Encoder.hpp"
# wpi/internal
DriverStationModeThread = "wpi/internal/DriverStationModeThread.hpp"
PeriodicPriorityQueue = "wpi/internal/PeriodicPriorityQueue.hpp"
# wpi/opmode
OpMode = "wpi/opmode/OpMode.hpp"

6
wpilibc/src/main/python/semiwrap/OpMode.yml
View File

@@ -2,5 +2,7 @@ classes:
wpi::OpMode:
methods:
DisabledPeriodic:
OpModeRun:
OpModeStop:
Start:
Periodic:
End:
GetCallbacks:

25
wpilibc/src/main/python/semiwrap/OpModeRobot.yml
View File

@@ -5,8 +5,10 @@ classes:
wpi::OpModeRobotBase:
methods:
StartCompetition:
EndCompetition:
OpModeRobotBase:
overloads:
wpi::units::second_t:
"":
DriverStationConnected:
NonePeriodic:
AddOpModeFactory:
@@ -17,5 +19,26 @@ classes:
RemoveOpMode:
PublishOpModes:
ClearOpModes:
EndCompetition:
RobotPeriodic:
SimulationInit:
SimulationPeriodic:
DisabledInit:
DisabledPeriodic:
DisabledExit:
AddPeriodic:
GetLoopStartTime:
LoopFunc:
attributes:
DEFAULT_PERIOD:
wpi::OpModeRobot:
ignore: true
methods:
OpModeRobot:
overloads:
wpi::units::second_t:
"":
AddOpMode:
overloads:
RobotMode, std::string_view, std::string_view, std::string_view, const wpi::util::Color&, const wpi::util::Color&:
RobotMode, std::string_view, std::string_view, std::string_view:

12
wpilibc/src/main/python/semiwrap/PeriodicOpMode.yml
View File

@@ -1,17 +1,13 @@
classes:
wpi::PeriodicOpMode:
attributes:
DEFAULT_PERIOD:
methods:
DisabledPeriodic:
Start:
Periodic:
End:
GetLoopStartTime:
AddPeriodic:
GetPeriod:
PrintWatchdogEpochs:
OpModeRun:
OpModeStop:
overloads:
std::function<void ()>, wpi::units::second_t:
std::function<void ()>, wpi::units::second_t, wpi::units::second_t:
PeriodicOpMode:
LoopFunc:
GetCallbacks:

28
wpilibc/src/main/python/semiwrap/PeriodicPriorityQueue.yml Normal file
View File

@@ -0,0 +1,28 @@
classes:
wpi::internal::PeriodicPriorityQueue:
methods:
Add:
overloads:
std::function<void ()>, std::chrono::microseconds, std::chrono::microseconds:
std::function<void ()>, std::chrono::microseconds, std::chrono::microseconds, std::chrono::microseconds:
std::function<void ()>, std::chrono::microseconds, wpi::units::second_t:
std::function<void ()>, std::chrono::microseconds, wpi::units::second_t, wpi::units::second_t:
Callback:
Remove:
Clear:
RunCallbacks:
GetQueue:
GetLoopStartTime:
wpi::internal::PeriodicPriorityQueue::Callback:
attributes:
func:
period:
expirationTime:
methods:
Callback:
overloads:
std::function<void ()>, std::chrono::microseconds, std::chrono::microseconds, std::chrono::microseconds:
std::function<void ()>, std::chrono::microseconds, wpi::units::second_t, wpi::units::second_t:
std::function<void ()>, std::chrono::microseconds, wpi::units::second_t:
operator>:
operator==:

2
wpilibc/src/main/python/semiwrap/TimedRobot.yml
View File

@@ -2,6 +2,8 @@ classes:
wpi::TimedRobot:
attributes:
DEFAULT_PERIOD:
m_notifier:
m_startTime:
methods:
StartCompetition:
EndCompetition:

54
wpilibc/src/test/native/cpp/OpModeRobotTest.cpp
View File

@@ -13,6 +13,8 @@
#include "wpi/util/Color.hpp"
#include "wpi/util/string.hpp"
inline constexpr auto kPeriod = 20_ms;
namespace {
class OpModeRobotTest : public ::testing::Test {
protected:
@@ -29,20 +31,24 @@ class MockRobot;
class MockOpMode : public wpi::OpMode {
public:
std::atomic<uint32_t> m_disabledPeriodicCount{0};
std::atomic<uint32_t> m_opModeRunCount{0};
std::atomic<uint32_t> m_opModeStopCount{0};
std::atomic<uint32_t> m_startCount{0};
std::atomic<uint32_t> m_periodicCount{0};
std::atomic<uint32_t> m_endCount{0};
std::atomic<uint32_t> m_closeCount{0};
MockOpMode() = default;
~MockOpMode() override { m_closeCount++; }
void DisabledPeriodic() override { m_disabledPeriodicCount++; }
void OpModeRun(int64_t opModeId) override { m_opModeRunCount++; }
void OpModeStop() override { m_opModeStopCount++; }
void Start() override { m_startCount++; }
void Periodic() override { m_periodicCount++; }
void End() override { m_endCount++; }
};
class OneArgOpMode : public wpi::OpMode {
public:
explicit OneArgOpMode(MockRobot& robot) {}
void OpModeRun(int64_t opModeId) override {}
void OpModeStop() override {}
void Start() override {}
void End() override {}
};
class MockRobot : public wpi::OpModeRobot<MockRobot> {
@@ -50,11 +56,16 @@ class MockRobot : public wpi::OpModeRobot<MockRobot> {
std::atomic<uint32_t> m_driverStationConnectedCount{0};
std::atomic<uint32_t> m_nonePeriodicCount{0};
// RobotPeriodic method counter
std::atomic<uint32_t> m_robotPeriodicCount{0};
MockRobot() = default;
void DriverStationConnected() override { m_driverStationConnectedCount++; }
void NonePeriodic() override { m_nonePeriodicCount++; }
void RobotPeriodic() override { m_robotPeriodicCount++; }
};
} // namespace
@@ -167,9 +178,36 @@ TEST_F(OpModeRobotTest, NonePeriodic) {
std::thread robotThread{[&] { robot.StartCompetition(); }};
wpi::sim::WaitForProgramStart();
// Time step to get periodic calls on 50 ms timeout
// Time step to get periodic calls on 20 ms robot loop
wpi::sim::StepTiming(110_ms);
EXPECT_EQ(robot.m_nonePeriodicCount.load(), 2u);
EXPECT_EQ(robot.m_nonePeriodicCount.load(), 5u);
robot.EndCompetition();
robotThread.join();
}
TEST_F(OpModeRobotTest, RobotPeriodic) {
struct MyMockRobot : public MockRobot {
MyMockRobot() {
AddOpMode<MockOpMode>(wpi::RobotMode::TELEOPERATED, "TestOpMode");
PublishOpModes();
}
};
MyMockRobot robot;
std::thread robotThread{[&] { robot.StartCompetition(); }};
wpi::sim::WaitForProgramStart();
// RobotPeriodic should be called regardless of state
EXPECT_EQ(robot.m_robotPeriodicCount.load(), 0u);
// Step timing to allow callbacks to execute
wpi::sim::StepTiming(kPeriod);
EXPECT_EQ(robot.m_robotPeriodicCount.load(), 1u);
// Additional time steps should continue calling RobotPeriodic
wpi::sim::StepTiming(kPeriod);
EXPECT_EQ(robot.m_robotPeriodicCount.load(), 2u);
robot.EndCompetition();
robotThread.join();

3
wpilibc/src/test/python/test_opmode_robot.py
View File

@@ -146,6 +146,7 @@ def test_none_periodic():
robot = MyMockRobot()
robot_thread = threading.Thread(target=robot.startCompetition)
robot_thread.daemon = True # Make thread daemon so it doesn't block test exit
robot_thread.start()
wsim.waitForProgramStart()
@@ -155,4 +156,4 @@ def test_none_periodic():
assert robot.none_periodic_count == 2
robot.endCompetition()
robot_thread.join()
robot_thread.join(timeout=1.0) # Add timeout to prevent hanging

1
wpilibcExamples/src/main/cpp/snippets/FlywheelBangBangController/cpp/Robot.cpp
View File

@@ -12,6 +12,7 @@
#include "wpi/simulation/EncoderSim.hpp"
#include "wpi/simulation/FlywheelSim.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
#include "wpi/system/RobotController.hpp"
#include "wpi/units/moment_of_inertia.hpp"
/**

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();
}
}

11
wpilibjExamples/src/main/java/org/wpilib/templates/opmode/opmode/MyAuto.java
View File

@@ -15,13 +15,14 @@ public class MyAuto extends PeriodicOpMode {
/** The Robot instance is passed into the opmode via the constructor. */
public MyAuto(Robot robot) {
m_robot = robot;
/*
* Can call super(period) to set a different periodic time interval.
*
* Additional periodic methods may be configured with addPeriodic().
*/
}
/*
* This method runs periodically, using the same period as the Robot instance.
*
* Additional periodic methods may be configured with addPeriodic(),
* which can have periods that differ from the main Robot instance.
*/
@Override
public void periodic() {
// Put custom auto code here