diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp index 0b7b9cb3a2b..538d1b2cf47 100644 --- a/wpilibc/src/main/native/cpp/TimedRobot.cpp +++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp @@ -14,7 +14,6 @@ #include #include "frc/Errors.h" -#include "frc/Timer.h" using namespace frc; @@ -37,29 +36,36 @@ void TimedRobot::StartCompetition() { auto callback = m_callbacks.pop(); int32_t status = 0; - HAL_UpdateNotifierAlarm( - m_notifier, static_cast(callback.expirationTime * 1e6), - &status); + HAL_UpdateNotifierAlarm(m_notifier, callback.expirationTime.count(), + &status); FRC_CheckErrorStatus(status, "UpdateNotifierAlarm"); - uint64_t curTime = HAL_WaitForNotifierAlarm(m_notifier, &status); - if (curTime == 0 || status != 0) { + std::chrono::microseconds currentTime{ + HAL_WaitForNotifierAlarm(m_notifier, &status)}; + if (currentTime.count() == 0 || status != 0) { break; } callback.func(); - callback.expirationTime += callback.period; + // 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 (static_cast(m_callbacks.top().expirationTime * 1e6) <= - curTime) { + while (m_callbacks.top().expirationTime <= currentTime) { callback = m_callbacks.pop(); callback.func(); - callback.expirationTime += callback.period; + callback.expirationTime += + callback.period + (currentTime - callback.expirationTime) / + callback.period * callback.period; m_callbacks.push(std::move(callback)); } } @@ -71,7 +77,7 @@ void TimedRobot::EndCompetition() { } TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) { - m_startTime = Timer::GetFPGATimestamp(); + m_startTime = std::chrono::microseconds{RobotController::GetFPGATime()}; AddPeriodic([=, this] { LoopFunc(); }, period); int32_t status = 0; @@ -94,5 +100,8 @@ TimedRobot::~TimedRobot() { void TimedRobot::AddPeriodic(std::function callback, units::second_t period, units::second_t offset) { - m_callbacks.emplace(callback, m_startTime, period, offset); + m_callbacks.emplace( + callback, m_startTime, + std::chrono::microseconds{static_cast(period.value() * 1e6)}, + std::chrono::microseconds{static_cast(offset.value() * 1e6)}); } diff --git a/wpilibc/src/main/native/include/frc/TimedRobot.h b/wpilibc/src/main/native/include/frc/TimedRobot.h index dcc510af2b8..647285474ad 100644 --- a/wpilibc/src/main/native/include/frc/TimedRobot.h +++ b/wpilibc/src/main/native/include/frc/TimedRobot.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include #include @@ -14,7 +15,7 @@ #include #include "frc/IterativeRobotBase.h" -#include "frc/Timer.h" +#include "frc/RobotController.h" namespace frc { @@ -73,8 +74,8 @@ class TimedRobot : public IterativeRobotBase { class Callback { public: std::function func; - units::second_t period; - units::second_t expirationTime; + std::chrono::microseconds period; + std::chrono::microseconds expirationTime; /** * Construct a callback container. @@ -84,15 +85,15 @@ class TimedRobot : public IterativeRobotBase { * @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) + Callback(std::function func, std::chrono::microseconds startTime, + std::chrono::microseconds period, std::chrono::microseconds offset) : func{std::move(func)}, period{period}, - expirationTime{startTime + offset + - units::math::floor( - (Timer::GetFPGATimestamp() - startTime) / period) * - period + - period} {} + expirationTime( + startTime + offset + period + + (std::chrono::microseconds{frc::RobotController::GetFPGATime()} - + startTime) / + period * period) {} bool operator>(const Callback& rhs) const { return expirationTime > rhs.expirationTime; @@ -100,7 +101,7 @@ class TimedRobot : public IterativeRobotBase { }; hal::Handle m_notifier; - units::second_t m_startTime; + std::chrono::microseconds m_startTime; wpi::priority_queue, std::greater> m_callbacks; 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 180a17b457d..2251d712d9f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -26,26 +26,26 @@ public class TimedRobot extends IterativeRobotBase { @SuppressWarnings("MemberName") static class Callback implements Comparable { public Runnable func; - public double period; - public double expirationTime; + public long period; + public long 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. + * @param startTimeSeconds The common starting point for all callback scheduling in + * microseconds. + * @param periodSeconds The period at which to run the callback in microseconds. + * @param offsetSeconds The offset from the common starting time in microseconds. */ - Callback(Runnable func, double startTimeSeconds, double periodSeconds, double offsetSeconds) { + Callback(Runnable func, long startTimeUs, long periodUs, long offsetUs) { this.func = func; - this.period = periodSeconds; + this.period = periodUs; this.expirationTime = - startTimeSeconds - + offsetSeconds - + Math.floor((Timer.getFPGATimestamp() - startTimeSeconds) / this.period) - * this.period - + this.period; + startTimeUs + + offsetUs + + this.period + + (RobotController.getFPGATime() - startTimeUs) / this.period * this.period; } @Override @@ -62,7 +62,7 @@ public int hashCode() { 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); + return Long.compare(expirationTime, rhs.expirationTime); } } @@ -73,7 +73,7 @@ public int compareTo(Callback rhs) { // just passed to the JNI bindings. private final int m_notifier = NotifierJNI.initializeNotifier(); - private double m_startTime; + private long m_startTimeUs; private final PriorityQueue m_callbacks = new PriorityQueue<>(); @@ -89,7 +89,7 @@ protected TimedRobot() { */ protected TimedRobot(double period) { super(period); - m_startTime = Timer.getFPGATimestamp(); + m_startTimeUs = RobotController.getFPGATime(); addPeriodic(this::loopFunc, period); NotifierJNI.setNotifierName(m_notifier, "TimedRobot"); @@ -122,25 +122,33 @@ public void startCompetition() { // at the end of the loop. var callback = m_callbacks.poll(); - NotifierJNI.updateNotifierAlarm(m_notifier, (long) (callback.expirationTime * 1e6)); + NotifierJNI.updateNotifierAlarm(m_notifier, callback.expirationTime); - long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier); - if (curTime == 0) { + long currentTime = NotifierJNI.waitForNotifierAlarm(m_notifier); + if (currentTime == 0) { break; } callback.func.run(); - callback.expirationTime += callback.period; + // 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 ((long) (m_callbacks.peek().expirationTime * 1e6) <= curTime) { + while (m_callbacks.peek().expirationTime <= currentTime) { callback = m_callbacks.poll(); callback.func.run(); - callback.expirationTime += callback.period; + callback.expirationTime += + callback.period + + (currentTime - callback.expirationTime) / callback.period * callback.period; m_callbacks.add(callback); } } @@ -162,7 +170,7 @@ public void endCompetition() { * @param periodSeconds The period at which to run the callback in seconds. */ public final void addPeriodic(Runnable callback, double periodSeconds) { - m_callbacks.add(new Callback(callback, m_startTime, periodSeconds, 0.0)); + m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (periodSeconds * 1e6), 0)); } /** @@ -177,7 +185,9 @@ public final void addPeriodic(Runnable callback, double periodSeconds) { * scheduling a callback in a different timeslot relative to TimedRobot. */ public final void addPeriodic(Runnable callback, double periodSeconds, double offsetSeconds) { - m_callbacks.add(new Callback(callback, m_startTime, periodSeconds, offsetSeconds)); + m_callbacks.add( + new Callback( + callback, m_startTimeUs, (long) (periodSeconds * 1e6), (long) (offsetSeconds * 1e6))); } /**