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

[wpilib] Support scheduling functions more often than robot loop (#2766)

Browse Source 
Currently, teams have to make a Notifier to run feedback controllers
more often than the TimedRobot loop period of 20ms (running TimedRobot
more often than this is not advised). This lets users add callbacks to
the main robot loop that run at a user-defined period. This allows
running feedback controllers more often, but does so synchronously with
TimedRobot so there aren't any thread safety issues.
This commit is contained in:
Tyler Veness
2020-10-16 17:56:37 -07:00
committed by GitHub
parent 57a97e3fb3
commit 7c8f1cf7af
5 changed files with 361 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
wpilibc/src/main/native/cpp/TimedRobot.cpp
View File

@@ -31,21 +31,37 @@ void TimedRobot::StartCompetition() {
// Tell the DS that the robot is ready to be enabled
HAL_ObserveUserProgramStarting();
m_expirationTime = units::second_t{Timer::GetFPGATimestamp()} + m_period;
UpdateAlarm();
// 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_UpdateNotifierAlarm(
m_notifier, static_cast<uint64_t>(callback.expirationTime * 1e6),
&status);
wpi_setHALError(status);
uint64_t curTime = HAL_WaitForNotifierAlarm(m_notifier, &status);
if (curTime == 0 || status != 0) break;
m_expirationTime += m_period;
callback.func();
UpdateAlarm();
callback.expirationTime += callback.period;
m_callbacks.push(std::move(callback));
// Call callback
LoopFunc();
// Process all other callbacks that are ready to run
while (static_cast<uint64_t>(m_callbacks.top().expirationTime * 1e6) <=
curTime) {
callback = m_callbacks.pop();
callback.func();
callback.expirationTime += callback.period;
m_callbacks.push(std::move(callback));
}
}
}
@@ -61,6 +77,9 @@ units::second_t TimedRobot::GetPeriod() const {
TimedRobot::TimedRobot(double period) : TimedRobot(units::second_t(period)) {}
TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
m_startTime = frc2::Timer::GetFPGATimestamp();
AddPeriodic([=] { LoopFunc(); }, period);
int32_t status = 0;
m_notifier = HAL_InitializeNotifier(&status);
wpi_setHALError(status);
@@ -79,9 +98,7 @@ TimedRobot::~TimedRobot() {
HAL_CleanNotifier(m_notifier, &status);
}
void TimedRobot::UpdateAlarm() {
int32_t status = 0;
HAL_UpdateNotifierAlarm(
m_notifier, static_cast<uint64_t>(m_expirationTime * 1e6), &status);
wpi_setHALError(status);
void TimedRobot::AddPeriodic(std::function<void()> callback,
units::second_t period, units::second_t offset) {
m_callbacks.emplace(callback, m_startTime, period, offset);
}