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

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