Merge pull request #103 from auscompgeek/notifier-delay

Add NotifierDelay

Author: virtuald

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,28 +295,26 @@ def disabled(self):
         ds_attached = None
         wpilib.LiveWindow.setEnabled(False)
 
-        delay = PreciseDelay(self.control_loop_wait_time)
-
         self._on_mode_disable_components()
         try:
             self.disabledInit()
         except:
             self.onException(forceReport=True)
 
-        while self.isDisabled():
-            
-            if ds_attached != self.ds.isDSAttached():
-                ds_attached = not ds_attached
-                self.__nt.putBoolean('is_ds_attached', ds_attached)
-            
-            try:
-                self.disabledPeriodic()
-            except:
-                self.onException()
+        with NotifierDelay(self.control_loop_wait_time) as delay:
+            while self.isDisabled():
+                if ds_attached != self.ds.isDSAttached():
+                    ds_attached = not ds_attached
+                    self.__nt.putBoolean('is_ds_attached', ds_attached)
 
-            self._update_feedback()
-            self.robotPeriodic()
-            delay.wait()
+                try:
+                    self.disabledPeriodic()
+                except:
+                    self.onException()
+
+                self._update_feedback()
+                self.robotPeriodic()
+                delay.wait()
 
     def operatorControl(self):
         """
@@ -333,8 +331,6 @@ def operatorControl(self):
         self.__nt.putBoolean('is_ds_attached', self.ds.isDSAttached())
         wpilib.LiveWindow.setEnabled(False)
 
-        delay = PreciseDelay(self.control_loop_wait_time)
-
         # initialize things
         self._on_mode_enable_components()
 
@@ -343,18 +339,18 @@ def operatorControl(self):
         except:
             self.onException(forceReport=True)
 
-        while self.isOperatorControl() and self.isEnabled():
-            
-            try:
-                self.teleopPeriodic()
-            except:
-                self.onException()
+        with NotifierDelay(self.control_loop_wait_time) as delay:
+            while self.isOperatorControl() and self.isEnabled():
+                try:
+                    self.teleopPeriodic()
+                except:
+                    self.onException()
 
-            self._execute_components()
-            self._update_feedback()
-            self.robotPeriodic()
+                self._execute_components()
+                self._update_feedback()
+                self.robotPeriodic()
 
-            delay.wait()
+                delay.wait()
 
         self._on_mode_disable_components()
 
@@ -369,16 +365,17 @@ def test(self):
             self.testInit()
         except:
             self.onException(forceReport=True)
-        
-        while self.isTest() and self.isEnabled():
-            try:
-                self.testPeriodic()
-            except:
-                self.onException()
 
-            self._update_feedback()
-            self.robotPeriodic()
-            wpilib.Timer.delay(self.control_loop_wait_time)
+        with NotifierDelay(self.control_loop_wait_time) as delay:
+            while self.isTest() and self.isEnabled():
+                try:
+                    self.testPeriodic()
+                except:
+                    self.onException()
+
+                self._update_feedback()
+                self.robotPeriodic()
+                delay.wait()
 
     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
 
@@ -226,24 +226,22 @@ def run(self, control_loop_wait_time=0.020, iter_fn=None, on_exception=None):
         #
         # Autonomous control loop
         #
-        
-        delay = PreciseDelay(control_loop_wait_time)
-        
-        while self.ds.isAutonomous() and self.ds.isEnabled():
- 
-            try:            
-                self._on_iteration(timer.get())
-            except:
-                on_exception()
-            
-            if isinstance(iter_fn, (list, tuple)):
-                for fn in iter_fn:
-                    fn()
-            else:
-                iter_fn()
-             
-            delay.wait()
-            
+
+        with NotifierDelay(control_loop_wait_time) as delay:
+            while self.ds.isAutonomous() and self.ds.isEnabled():
+                try:
+                    self._on_iteration(timer.get())
+                except:
+                    on_exception()
+
+                if isinstance(iter_fn, (list, tuple)):
+                    for fn in iter_fn:
+                        fn()
+                else:
+                    iter_fn()
+
+                delay.wait()
+
         #
         # Done with autonomous, finish up
         #
@@ -254,6 +252,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,62 @@ 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.
+
+    Example::
+
+        with NotifierDelay(0.02) as delay:
+            while something:
+                # do things here
+                delay.wait()
+    """
+
+    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 __del__(self):
+        self.free()
+
+    def __enter__(self) -> 'NotifierDelay':
+        return self
+
+    def __exit__(self, exc_type, exc_val, exc_tb):
+        self.free()
+
+    def free(self) -> None:
+        """Clean up the notifier.
+
+        Do not use this object after this method is called.
+        """
+        handle = self._notifier
+        if handle is None:
+            return
+        hal.stopNotifier(handle)
+        hal.cleanNotifier(handle)
+        self._notifier = None
+
+    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)