diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp index eef3f5c993..103aed80f1 100644 --- a/wpilibc/src/main/native/cpp/TimedRobot.cpp +++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp @@ -7,10 +7,14 @@ #include "TimedRobot.h" -#include +#include #include +#include "Timer.h" +#include "Utility.h" +#include "WPIErrors.h" + using namespace frc; /** @@ -22,11 +26,23 @@ void TimedRobot::StartCompetition() { // Tell the DS that the robot is ready to be enabled HAL_ObserveUserProgramStarting(); - // Loop forever, calling the appropriate mode-dependent function m_startLoop = true; - m_loop->StartPeriodic(m_period); + + m_expirationTime = Timer::GetFPGATimestamp() + m_period; + UpdateAlarm(); + + // Loop forever, calling the appropriate mode-dependent function while (true) { - std::this_thread::sleep_for(std::chrono::hours(24)); + int32_t status = 0; + uint64_t curTime = HAL_WaitForNotifierAlarm(m_notifier, &status); + if (curTime == 0 || status != 0) break; + + m_expirationTime += m_period; + + UpdateAlarm(); + + // Call callback + LoopFunc(); } } @@ -43,7 +59,8 @@ void TimedRobot::SetPeriod(double period) { m_period = period; if (m_startLoop) { - m_loop->StartPeriodic(period); + m_expirationTime = Timer::GetFPGATimestamp() + period; + UpdateAlarm(); } } @@ -53,7 +70,9 @@ void TimedRobot::SetPeriod(double period) { double TimedRobot::GetPeriod() const { return m_period; } TimedRobot::TimedRobot() { - m_loop = std::make_unique(&TimedRobot::LoopFunc, this); + int32_t status = 0; + m_notifier = HAL_InitializeNotifier(&status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); // HAL_Report(HALUsageReporting::kResourceType_Framework, // HALUsageReporting::kFramework_Periodic); @@ -61,4 +80,21 @@ TimedRobot::TimedRobot() { HALUsageReporting::kFramework_Iterative); } -TimedRobot::~TimedRobot() { m_loop->Stop(); } +TimedRobot::~TimedRobot() { + int32_t status = 0; + + HAL_StopNotifier(m_notifier, &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); + + HAL_CleanNotifier(m_notifier, &status); +} + +/** + * Update the HAL alarm time. + */ +void TimedRobot::UpdateAlarm() { + int32_t status = 0; + HAL_UpdateNotifierAlarm( + m_notifier, static_cast(m_expirationTime * 1e6), &status); + wpi_setErrorWithContext(status, HAL_GetErrorMessage(status)); +} diff --git a/wpilibc/src/main/native/include/TimedRobot.h b/wpilibc/src/main/native/include/TimedRobot.h index ba2f812a5d..c1c4555178 100644 --- a/wpilibc/src/main/native/include/TimedRobot.h +++ b/wpilibc/src/main/native/include/TimedRobot.h @@ -7,11 +7,10 @@ #pragma once -#include -#include +#include +#include "ErrorBase.h" #include "IterativeRobotBase.h" -#include "Notifier.h" namespace frc { @@ -24,7 +23,7 @@ namespace frc { * Periodic() functions from the base class are called on an interval by a * Notifier instance. */ -class TimedRobot : public IterativeRobotBase { +class TimedRobot : public IterativeRobotBase, public ErrorBase { public: static constexpr double kDefaultPeriod = 0.02; @@ -35,15 +34,21 @@ class TimedRobot : public IterativeRobotBase { protected: TimedRobot(); - virtual ~TimedRobot(); + ~TimedRobot() override; private: - std::atomic m_period{kDefaultPeriod}; - // Prevents loop from starting if user calls SetPeriod() in RobotInit() bool m_startLoop = false; - std::unique_ptr m_loop; + HAL_NotifierHandle m_notifier{0}; + + // The absolute expiration time + double m_expirationTime = 0; + + // The relative time + double m_period = kDefaultPeriod; + + void UpdateAlarm(); }; } // namespace frc diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java index bfd261c0eb..1e4f445585 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java @@ -105,11 +105,9 @@ public class Notifier { error = cause; } DriverStation.reportError("Unhandled exception: " + error.toString(), error.getStackTrace()); - DriverStation.reportWarning("Robots should not quit, but yours did!", false); DriverStation.reportError( "The loopFunc() method (or methods called by it) should have handled " + "the exception above.", false); - System.exit(1); }); m_thread.start(); } 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 06aa6165f6..651f8e4d0d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -10,6 +10,7 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; +import edu.wpi.first.wpilibj.hal.NotifierJNI; /** * TimedRobot implements the IterativeRobotBase robot program framework. @@ -21,20 +22,30 @@ import edu.wpi.first.wpilibj.hal.HAL; public class TimedRobot extends IterativeRobotBase { public static final double DEFAULT_PERIOD = 0.02; - private volatile double m_period = DEFAULT_PERIOD; - // Prevents loop from starting if user calls setPeriod() in robotInit() private boolean m_startLoop = false; - private Notifier m_loop = new Notifier(() -> { - loopFunc(); - }); + // 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 = 0; + + private double m_period = DEFAULT_PERIOD; public TimedRobot() { // HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Periodic); HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative); } + @Override + @SuppressWarnings("NoFinalizer") + protected void finalize() { + NotifierJNI.stopNotifier(m_notifier); + NotifierJNI.cleanNotifier(m_notifier); + } + /** * Provide an alternate "main loop" via startCompetition(). */ @@ -44,15 +55,22 @@ public class TimedRobot extends IterativeRobotBase { // Tell the DS that the robot is ready to be enabled HAL.observeUserProgramStarting(); - // Loop forever, calling the appropriate mode-dependent function m_startLoop = true; - m_loop.startPeriodic(m_period); + + m_expirationTime = RobotController.getFPGATime() * 1e-6 + m_period; + updateAlarm(); + + // Loop forever, calling the appropriate mode-dependent function while (true) { - try { - Thread.sleep(1000 * 60 * 60 * 24); - } catch (InterruptedException ex) { - Thread.currentThread().interrupt(); + long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier); + if (curTime == 0) { + break; } + + m_expirationTime += m_period; + updateAlarm(); + + loopFunc(); } } @@ -65,7 +83,8 @@ public class TimedRobot extends IterativeRobotBase { m_period = period; if (m_startLoop) { - m_loop.startPeriodic(m_period); + m_expirationTime = RobotController.getFPGATime() * 1e-6 + period; + updateAlarm(); } } @@ -75,4 +94,11 @@ public class TimedRobot extends IterativeRobotBase { public double getPeriod() { return m_period; } + + /** + * Update the alarm hardware to reflect the next alarm. + */ + private void updateAlarm() { + NotifierJNI.updateNotifierAlarm(m_notifier, (long) (m_expirationTime * 1e6)); + } }