TimedRobot now uses the Notifier HAL API (#942)

Fixes #941.

wpilibc/src/main/native/cpp/TimedRobot.cpp

@@ -7,10 +7,14 @@
 
 #include "TimedRobot.h"
 
-#include <chrono>
+#include <stdint.h>
 
 #include <HAL/HAL.h>
 
+#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<Notifier>(&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<uint64_t>(m_expirationTime * 1e6), &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}

wpilibc/src/main/native/include/TimedRobot.h

@@ -7,11 +7,10 @@
 
 #pragma once
 
-#include <atomic>
-#include <memory>
+#include <HAL/Notifier.h>
 
+#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<double> m_period{kDefaultPeriod};
-
   // Prevents loop from starting if user calls SetPeriod() in RobotInit()
   bool m_startLoop = false;
 
-  std::unique_ptr<Notifier> 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

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

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