From 7c8f1cf7af8c30a03453a1a6055c9069f2ed4873 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 16 Oct 2020 17:56:37 -0700 Subject: [PATCH] [wpilib] Support scheduling functions more often than robot loop (#2766) 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. --- wpilibc/src/main/native/cpp/TimedRobot.cpp | 41 +++++-- .../src/main/native/include/frc/TimedRobot.h | 63 ++++++++-- .../src/test/native/cpp/TimedRobotTest.cpp | 83 +++++++++++++ .../edu/wpi/first/wpilibj/TimedRobot.java | 97 ++++++++++++++-- .../edu/wpi/first/wpilibj/TimedRobotTest.java | 109 ++++++++++++++++++ 5 files changed, 361 insertions(+), 32 deletions(-) diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp index 144cdbe2e8..74e658aeac 100644 --- a/wpilibc/src/main/native/cpp/TimedRobot.cpp +++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp @@ -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(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(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(m_expirationTime * 1e6), &status); - wpi_setHALError(status); +void TimedRobot::AddPeriodic(std::function callback, + units::second_t period, units::second_t offset) { + m_callbacks.emplace(callback, m_startTime, period, offset); } diff --git a/wpilibc/src/main/native/include/frc/TimedRobot.h b/wpilibc/src/main/native/include/frc/TimedRobot.h index 66f960b473..9dbc3f2a41 100644 --- a/wpilibc/src/main/native/include/frc/TimedRobot.h +++ b/wpilibc/src/main/native/include/frc/TimedRobot.h @@ -7,12 +7,18 @@ #pragma once +#include +#include + #include +#include #include #include +#include #include "frc/ErrorBase.h" #include "frc/IterativeRobotBase.h" +#include "frc2/Timer.h" namespace frc { @@ -67,16 +73,57 @@ class TimedRobot : public IterativeRobotBase, public ErrorBase { TimedRobot(TimedRobot&&) = default; TimedRobot& operator=(TimedRobot&&) = default; - private: - hal::Handle m_notifier; - - // The absolute expiration time - units::second_t m_expirationTime{0}; - /** - * Update the HAL alarm time. + * Add a callback to run at a specific period with a starting time offset. + * + * 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. */ - void UpdateAlarm(); + void AddPeriodic(std::function callback, units::second_t period, + units::second_t offset = 0_s); + + private: + class Callback { + public: + std::function func; + units::second_t period; + units::second_t 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 func, units::second_t startTime, + units::second_t period, units::second_t offset) + : func{func}, + period{period}, + expirationTime{ + startTime + offset + + units::math::floor((frc2::Timer::GetFPGATimestamp() - startTime) / + period) * + period + + period} {} + + bool operator>(const Callback& rhs) const { + return expirationTime > rhs.expirationTime; + } + }; + + hal::Handle m_notifier; + units::second_t m_startTime; + + wpi::priority_queue, std::greater> + m_callbacks; }; } // namespace frc diff --git a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp index 6fe11b4481..f91efb84a4 100644 --- a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp +++ b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp @@ -309,3 +309,86 @@ TEST_F(TimedRobotTest, Test) { robot.EndCompetition(); robotThread.join(); } + +TEST_F(TimedRobotTest, AddPeriodic) { + MockRobot robot; + + std::atomic callbackCount{0}; + robot.AddPeriodic([&] { callbackCount++; }, 10_ms); + + std::thread robotThread{[&] { robot.StartCompetition(); }}; + + frc::sim::DriverStationSim::SetEnabled(false); + frc::sim::DriverStationSim::NotifyNewData(); + frc::sim::StepTiming(0_ms); // Wait for Notifiers + + EXPECT_EQ(0u, robot.m_disabledInitCount); + EXPECT_EQ(0u, robot.m_disabledPeriodicCount); + EXPECT_EQ(0u, callbackCount); + + frc::sim::StepTiming(10_ms); + + EXPECT_EQ(0u, robot.m_disabledInitCount); + EXPECT_EQ(0u, robot.m_disabledPeriodicCount); + EXPECT_EQ(1u, callbackCount); + + frc::sim::StepTiming(10_ms); + + EXPECT_EQ(1u, robot.m_disabledInitCount); + EXPECT_EQ(1u, robot.m_disabledPeriodicCount); + EXPECT_EQ(2u, callbackCount); + + robot.EndCompetition(); + robotThread.join(); +} + +TEST_F(TimedRobotTest, AddPeriodicWithOffset) { + MockRobot robot; + + std::atomic callbackCount{0}; + robot.AddPeriodic([&] { callbackCount++; }, 10_ms, 5_ms); + + // Expirations in this test (ms) + // + // Robot | Callback + // ================ + // 20 | 15 + // 40 | 25 + + std::thread robotThread{[&] { robot.StartCompetition(); }}; + + frc::sim::DriverStationSim::SetEnabled(false); + frc::sim::DriverStationSim::NotifyNewData(); + frc::sim::StepTiming(0_ms); // Wait for Notifiers + + EXPECT_EQ(0u, robot.m_disabledInitCount); + EXPECT_EQ(0u, robot.m_disabledPeriodicCount); + EXPECT_EQ(0u, callbackCount); + + frc::sim::StepTiming(7.5_ms); + + EXPECT_EQ(0u, robot.m_disabledInitCount); + EXPECT_EQ(0u, robot.m_disabledPeriodicCount); + EXPECT_EQ(0u, callbackCount); + + frc::sim::StepTiming(7.5_ms); + + EXPECT_EQ(0u, robot.m_disabledInitCount); + EXPECT_EQ(0u, robot.m_disabledPeriodicCount); + EXPECT_EQ(1u, callbackCount); + + frc::sim::StepTiming(5_ms); + + EXPECT_EQ(1u, robot.m_disabledInitCount); + EXPECT_EQ(1u, robot.m_disabledPeriodicCount); + EXPECT_EQ(1u, callbackCount); + + frc::sim::StepTiming(5_ms); + + EXPECT_EQ(1u, robot.m_disabledInitCount); + EXPECT_EQ(1u, robot.m_disabledPeriodicCount); + EXPECT_EQ(2u, callbackCount); + + robot.EndCompetition(); + robotThread.join(); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java index 63952896cf..99bce3d91c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -7,6 +7,8 @@ package edu.wpi.first.wpilibj; +import java.util.PriorityQueue; + import edu.wpi.first.hal.FRCNetComm.tInstances; import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; @@ -20,14 +22,48 @@ import edu.wpi.first.hal.NotifierJNI; *

periodic() functions from the base class are called on an interval by a Notifier instance. */ public class TimedRobot extends IterativeRobotBase { + @SuppressWarnings("MemberName") + class Callback implements Comparable { + public Runnable func; + public double period; + public double expirationTime; + + /** + * Construct a callback container. + * + * @param func The callback to run. + * @param startTimeSeconds The common starting point for all callback + * scheduling in seconds. + * @param periodSeconds The period at which to run the callback in + * seconds. + * @param offsetSeconds The offset from the common starting time in + * seconds. + */ + Callback(Runnable func, double startTimeSeconds, double periodSeconds, double offsetSeconds) { + this.func = func; + this.period = periodSeconds; + this.expirationTime = startTimeSeconds + offsetSeconds + + Math.floor((Timer.getFPGATimestamp() - startTimeSeconds) + / this.period) * this.period + this.period; + } + + @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 Double.compare(expirationTime, rhs.expirationTime); + } + } + public static final double kDefaultPeriod = 0.02; // The C pointer to the notifier object. We don't use it directly, it is // just passed to the JNI bindings. private final int m_notifier = NotifierJNI.initializeNotifier(); - // The absolute expiration time - private double m_expirationTime; + private double m_startTime; + + private final PriorityQueue m_callbacks = new PriorityQueue<>(); /** * Constructor for TimedRobot. @@ -43,6 +79,8 @@ public class TimedRobot extends IterativeRobotBase { */ protected TimedRobot(double period) { super(period); + m_startTime = Timer.getFPGATimestamp(); + addPeriodic(this::loopFunc, period); NotifierJNI.setNotifierName(m_notifier, "TimedRobot"); HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Timed); @@ -70,20 +108,34 @@ public class TimedRobot extends IterativeRobotBase { // Tell the DS that the robot is ready to be enabled HAL.observeUserProgramStarting(); - m_expirationTime = RobotController.getFPGATime() * 1e-6 + 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. + var callback = m_callbacks.poll(); + + NotifierJNI.updateNotifierAlarm(m_notifier, (long) (callback.expirationTime * 1e6)); + long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier); if (curTime == 0) { break; } - m_expirationTime += m_period; - updateAlarm(); + callback.func.run(); - loopFunc(); + callback.expirationTime += callback.period; + m_callbacks.add(callback); + + // Process all other callbacks that are ready to run + while ((long) (m_callbacks.peek().expirationTime * 1e6) <= curTime) { + callback = m_callbacks.poll(); + + callback.func.run(); + + callback.expirationTime += callback.period; + m_callbacks.add(callback); + } } } @@ -103,10 +155,31 @@ public class TimedRobot extends IterativeRobotBase { } /** - * Update the alarm hardware to reflect the next alarm. + * Add a callback to run at a specific period. + * + *

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 periodSeconds The period at which to run the callback in seconds. */ - @SuppressWarnings("UnsafeFinalization") - private void updateAlarm() { - NotifierJNI.updateNotifierAlarm(m_notifier, (long) (m_expirationTime * 1e6)); + public void addPeriodic(Runnable callback, double periodSeconds) { + m_callbacks.add(new Callback(callback, m_startTime, periodSeconds, 0.0)); + } + + /** + * Add a callback to run at a specific period with a starting time offset. + * + *

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 periodSeconds The period at which to run the callback in seconds. + * @param offsetSeconds The offset from the common starting time in seconds. + * This is useful for scheduling a callback in a + * different timeslot relative to TimedRobot. + */ + public void addPeriodic(Runnable callback, double periodSeconds, double offsetSeconds) { + m_callbacks.add(new Callback(callback, m_startTime, periodSeconds, offsetSeconds)); } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java index 47ffaa09d4..84b1edc0bb 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java @@ -392,4 +392,113 @@ class TimedRobotTest { } robot.close(); } + + @Test + @ResourceLock("timing") + void addPeriodicTest() { + MockRobot robot = new MockRobot(); + + final AtomicInteger callbackCount = new AtomicInteger(0); + robot.addPeriodic(() -> { + callbackCount.addAndGet(1); + }, 0.01); + + Thread robotThread = new Thread(() -> { + robot.startCompetition(); + }); + robotThread.start(); + + DriverStationSim.setEnabled(false); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(0.0); // Wait for Notifiers + + assertEquals(0, robot.m_disabledInitCount.get()); + assertEquals(0, robot.m_disabledPeriodicCount.get()); + assertEquals(0, callbackCount.get()); + + SimHooks.stepTiming(0.01); + + assertEquals(0, robot.m_disabledInitCount.get()); + assertEquals(0, robot.m_disabledPeriodicCount.get()); + assertEquals(1, callbackCount.get()); + + SimHooks.stepTiming(0.01); + + assertEquals(1, robot.m_disabledInitCount.get()); + assertEquals(1, robot.m_disabledPeriodicCount.get()); + assertEquals(2, callbackCount.get()); + + robot.endCompetition(); + try { + robotThread.interrupt(); + robotThread.join(); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + robot.close(); + } + + @Test + @ResourceLock("timing") + void addPeriodicWithOffsetTest() { + MockRobot robot = new MockRobot(); + + final AtomicInteger callbackCount = new AtomicInteger(0); + robot.addPeriodic(() -> { + callbackCount.addAndGet(1); + }, 0.01, 0.005); + + // Expirations in this test (ms) + // + // Robot | Callback + // ================ + // 20 | 15 + // 40 | 25 + + Thread robotThread = new Thread(() -> { + robot.startCompetition(); + }); + robotThread.start(); + + DriverStationSim.setEnabled(false); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(0.0); // Wait for Notifiers + + assertEquals(0, robot.m_disabledInitCount.get()); + assertEquals(0, robot.m_disabledPeriodicCount.get()); + assertEquals(0, callbackCount.get()); + + SimHooks.stepTiming(0.0075); + + assertEquals(0, robot.m_disabledInitCount.get()); + assertEquals(0, robot.m_disabledPeriodicCount.get()); + assertEquals(0, callbackCount.get()); + + SimHooks.stepTiming(0.0075); + + assertEquals(0, robot.m_disabledInitCount.get()); + assertEquals(0, robot.m_disabledPeriodicCount.get()); + assertEquals(1, callbackCount.get()); + + SimHooks.stepTiming(0.005); + + assertEquals(1, robot.m_disabledInitCount.get()); + assertEquals(1, robot.m_disabledPeriodicCount.get()); + assertEquals(1, callbackCount.get()); + + SimHooks.stepTiming(0.005); + + assertEquals(1, robot.m_disabledInitCount.get()); + assertEquals(1, robot.m_disabledPeriodicCount.get()); + assertEquals(2, callbackCount.get()); + + robot.endCompetition(); + try { + robotThread.interrupt(); + robotThread.join(); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + robot.close(); + } }