Merge pull request #451 from robotpy/timed-robot

TimedRobot now uses the HAL Notifier API directly
robotpy · Feb 16, 2018 · 2304efc · 2304efc
2 parents 5f535a2 + 35118db
commit 2304efc
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 7 deletions.
diff --git a/hal-sim/hal_impl/functions.py b/hal-sim/hal_impl/functions.py
@@ -1335,11 +1335,22 @@ def waitForNotifierAlarm(notifierHandle, status):
     with notifierHandle.lock:
         while notifierHandle.active:
             if not notifierHandle.running:
-                waitTime = 1000.0
+                # TODO: switch to longer wait once pyfrc is fixed
+                #waitTime = 1000.0
+                waitTime = 0.010
             else:
-                waitTime = (notifierHandle.waitTime - hooks.getFPGATime()) * 1e-6
+                waitTime = max((notifierHandle.waitTime - hooks.getFPGATime()) * 1e-6, 0)
+
+            notifierHandle.updatedAlarm = False
+
+            # TODO: fix pyfrc to make this work with the condition instead
+            #notifierHandle.lock.wait(timeout=waitTime)
+            try:
+                notifierHandle.lock.release()
+                hooks.delaySeconds(waitTime)
+            finally:
+                notifierHandle.lock.acquire()
 
-            notifierHandle.lock.wait(timeout=waitTime)
             if notifierHandle.updatedAlarm:
                 notifierHandle.updatedAlarm = False
                 continue

diff --git a/wpilib/wpilib/timedrobot.py b/wpilib/wpilib/timedrobot.py
@@ -10,6 +10,8 @@
 
 from .iterativerobotbase import IterativeRobotBase
 from .notifier import Notifier
+from .resource import Resource
+from .robotcontroller import RobotController
 from .timer import Timer
 
 
@@ -33,7 +35,16 @@ def __init__(self):
         self.period = TimedRobot.DEFAULT_PERIOD
         # Prevents loop from starting if user calls setPeriod() in robotInit()
         self.startLoop = False
-        self.loop = Notifier(self.loopFunc)
+
+        self._expirationTime = 0
+        self._notifier = hal.initializeNotifier()
+
+        Resource._add_global_resource(self)
+
+    # python-specific
+    def free(self) -> None:
+        hal.stopNotifier(self._notifier)
+        hal.cleanNotifier(self._notifier)
 
     def startCompetition(self) -> None:
         """Provide an alternate "main loop" via startCompetition()"""
@@ -42,10 +53,19 @@ def startCompetition(self) -> None:
         hal.observeUserProgramStarting()
 
         self.startLoop = True
-        self.loop.startPeriodic(self.period)
+
+        self._expirationTime = RobotController.getFPGATime() * 1e-6 + self.period
+        self._updateAlarm()
 
+        # Loop forever, calling the appropriate mode-dependent function
         while True:
-            Timer.delay(60*60*24)
+            if hal.waitForNotifierAlarm(self._notifier) == 0:
+                break
+
+            self._expirationTime += self.period
+            self._updateAlarm()
+
+            self.loopFunc()
 
 
     def setPeriod(self, period: float) -> None:
@@ -56,8 +76,12 @@ def setPeriod(self, period: float) -> None:
         self.period = period
 
         if self.startLoop:
-            self.loop.startPeriodic(self.period)
+            self._expirationTime = RobotController.getFPGATime() * 1e-6 + self.period
+            self._updateAlarm()
 
     def getPeriod(self):
         """Get time period between calls to Periodic() functions."""
         return self.period
+
+    def _updateAlarm(self) -> None:
+        hal.updateNotifierAlarm(self._notifier, int(self._expirationTime * 1e6))