TimedRobot now uses the Notifier HAL API (#942)

Fixes #941.

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