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