Add NotifierDelay

Add a version of PreciseDelay that uses FPGA interrupts instead of busy
waiting.

Caveat: Users must manually clean up the notifier handle once this
object is no longer in use.

Also change our usage of PreciseDelay to NotifierDelay.

Also use NotifierDelay in magicbot's test mode to avoid drift.

Author: auscompgeek

magicbot/magicrobot.py

@@ -5,7 +5,7 @@
 
 import wpilib
 
-from robotpy_ext.misc import PreciseDelay
+from robotpy_ext.misc import NotifierDelay
 from robotpy_ext.autonomous import AutonomousModeSelector
 
 from robotpy_ext.misc.orderedclass import OrderedClass
@@ -295,7 +295,7 @@ def disabled(self):
         ds_attached = None
         wpilib.LiveWindow.setEnabled(False)
 
-        delay = PreciseDelay(self.control_loop_wait_time)
+        delay = NotifierDelay(self.control_loop_wait_time)
 
         self._on_mode_disable_components()
         try:
@@ -318,6 +318,8 @@ def disabled(self):
             self.robotPeriodic()
             delay.wait()
 
+        delay.free()
+
     def operatorControl(self):
         """
             This function is called in teleoperated mode. You should not
@@ -333,7 +335,7 @@ def operatorControl(self):
         self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
         wpilib.LiveWindow.setEnabled(False)
 
-        delay = PreciseDelay(self.control_loop_wait_time)
+        delay = NotifierDelay(self.control_loop_wait_time)
 
         # initialize things
         self._on_mode_enable_components()
@@ -356,6 +358,7 @@ def operatorControl(self):
 
             delay.wait()
 
+        delay.free()
         self._on_mode_disable_components()
 
     def test(self):
@@ -365,6 +368,8 @@ def test(self):
         self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
         wpilib.LiveWindow.setEnabled(True)
 
+        delay = NotifierDelay(self.control_loop_wait_time)
+
         try:
             self.testInit()
         except:
@@ -378,7 +383,9 @@ def test(self):
 
             self._update_feedback()
             self.robotPeriodic()
-            wpilib.Timer.delay(self.control_loop_wait_time)
+            delay.wait()
+
+        delay.free()
 
     def _on_mode_enable_components(self):
         # initialize things

robotpy_ext/autonomous/selector.py

@@ -7,7 +7,7 @@
 import logging
 logger = logging.getLogger('autonomous')
 
-from ..misc.precise_delay import PreciseDelay
+from ..misc.precise_delay import NotifierDelay
 
 import wpilib
 
@@ -227,7 +227,7 @@ def run(self, control_loop_wait_time=0.020, iter_fn=None, on_exception=None):
         # Autonomous control loop
         #
 
-        delay = PreciseDelay(control_loop_wait_time)
+        delay = NotifierDelay(control_loop_wait_time)
 
         while self.ds.isAutonomous() and self.ds.isEnabled():
 
@@ -254,6 +254,7 @@ def run(self, control_loop_wait_time=0.020, iter_fn=None, on_exception=None):
             on_exception(forceReport=True)
 
         logger.info("Autonomous mode ended")
+        delay.free()
 
     #
     #   Internal methods used to implement autonomous mode switching, and

robotpy_ext/misc/__init__.py

@@ -1,3 +1,3 @@
 
-from .precise_delay import PreciseDelay
+from .precise_delay import NotifierDelay, PreciseDelay
 from .periodic_filter import PeriodicFilter

robotpy_ext/misc/precise_delay.py

@@ -77,5 +77,44 @@ def _wait_unit_tests(self):
         # - TODO: should we just always use this in simulated mode?
 
         wpilib.Timer.delay(self.delay_period)
-        
-        
+
+
+class NotifierDelay:
+    """Synchronizes a timing loop against interrupts from the FPGA.
+
+    This will delay so that the next invocation of your loop happens at
+    precisely the same period, assuming that your loop does not take longer
+    than the specified period.
+    """
+
+    def __init__(self, delay_period: float) -> None:
+        """:param delay_period: The period's amount of time (in seconds)."""
+        if delay_period < 0.001:
+            raise ValueError("You probably don't want to delay less than 1ms!")
+
+        # Convert the delay period to microseconds, as FPGA timestamps are microseconds
+        self.delay_period = int(delay_period * 1e6)
+        self._notifier = hal.initializeNotifier()
+        self._expiry_time = wpilib.RobotController.getFPGATime() + self.delay_period
+        self._update_alarm()
+
+        wpilib.Resource._add_global_resource(self)
+
+    def free(self) -> None:
+        """Clean up the notifier.
+
+        Call this method once you are done using this object.
+        Do not use this object after this method is called.
+        """
+        handle = self._notifier
+        hal.stopNotifier(handle)
+        hal.cleanNotifier(handle)
+
+    def wait(self) -> None:
+        """Wait until the delay period has passed."""
+        hal.waitForNotifierAlarm(self._notifier)
+        self._expiry_time += self.delay_period
+        self._update_alarm()
+
+    def _update_alarm(self) -> None:
+        hal.updateNotifierAlarm(self._notifier, self._expiry_time)