Add test of period math, and improve comments.

Signed-off-by: Mike Stitt <[email protected]>

Author: MikeStitt

subprojects/robotpy-wpilib/tests/test_timedrobot.py

@@ -0,0 +1,27 @@
+import pytest
+
+from wpilib.timedrobotpy import _Callback
+
+def test_calcFutureExpirationUs() -> None:
+    cb = _Callback(func=None, periodUs=20_000, expirationUs=100)
+    assert cb.calcFutureExpirationUs(100) == 20_100
+    assert cb.calcFutureExpirationUs(101) == 20_100
+    assert cb.calcFutureExpirationUs(20_099) == 20_100
+    assert cb.calcFutureExpirationUs(20_100) == 40_100
+    assert cb.calcFutureExpirationUs(20_101) == 40_100
+
+    cb = _Callback(func=None, periodUs=40_000, expirationUs=500)
+    assert cb.calcFutureExpirationUs(500) == 40_500
+    assert cb.calcFutureExpirationUs(501) == 40_500
+    assert cb.calcFutureExpirationUs(40_499) == 40_500
+    assert cb.calcFutureExpirationUs(40_500) == 80_500
+    assert cb.calcFutureExpirationUs(40_501) == 80_500
+
+    cb = _Callback(func=None, periodUs=1_000, expirationUs=0)
+    assert cb.calcFutureExpirationUs(1_000_000_000_000_000_000) == 1_000_000_000_000_001_000
+    assert cb.calcFutureExpirationUs(1_000_000_000_000_000_001) == 1_000_000_000_000_001_000
+    assert cb.calcFutureExpirationUs(1_000_000_000_000_000_999) == 1_000_000_000_000_001_000
+    assert cb.calcFutureExpirationUs(1_000_000_000_000_001_000) == 1_000_000_000_000_002_000
+    assert cb.calcFutureExpirationUs(1_000_000_000_000_001_001) == 1_000_000_000_000_002_000
+
+

subprojects/robotpy-wpilib/wpilib/timedrobotpy.py

@@ -55,16 +55,29 @@ def calcFutureExpirationUs(
         self, currentTimeUs: microsecondsAsInt
     ) -> microsecondsAsInt:
         # increment the expiration time by the number of full periods it's behind
-        # plus one to avoid rapid repeat fires from a large loop overrun. We assume
-        # currentTime ≥ startTimeUs rather than checking for it since the
+        # plus one to avoid rapid repeat fires from a large loop overrun.
+        #
+        # This routine is called when either:
+        #   this callback has never ran and self.expirationUs is
+        #   TimedRobot._starttimeUs and we are calculating where the first
+        #   expiration should be relative to TimedRobot._starttimeUs
+        # or:
+        #   this call back has ran and self.expirationUs is when it was scheduled to
+        #   expire and we are calculating when the next expirations should be relative
+        #   to the last scheduled expiration
+        #
+        # We assume currentTime ≥ self.expirationUs rather than checking for it since the
         # callback wouldn't be running otherwise.
-        # todo does this math work?
-        # todo does the "// periodUs * periodUs" do the correct integer math?
-        return (
-            self.expirationUs
-            + self._periodUs
+        #
+        # We take when we previously expired or when we started: self.expirationUs
+        # add + self._periodUs to get at least one period in the future
+        # then calculate how many whole periods we are behind:
+        #   ((currentTimeUs - self.expirationUs) // self._periodUs)
+        # and multiply that by self._periodUs to calculate how much time in full
+        # periods we need to skip to catch up, and add that to the sum to calculate
+        # when we should run again.
+        return self.expirationUs + self._periodUs \
             + ((currentTimeUs - self.expirationUs) // self._periodUs) * self._periodUs
-        )
 
     def setNextStartTimeUs(self, currentTimeUs: microsecondsAsInt) -> None:
         self.expirationUs = self.calcFutureExpirationUs(currentTimeUs)
@@ -129,6 +142,8 @@ def __init__(self, period: wpimath.units.seconds = kDefaultPeriod) -> None:
         """
         super().__init__(period)
 
+        # All periodic functions created by addPeriodic are relative
+        # to this self._startTimeUs
         self._startTimeUs = _getFPGATime()
         self._callbacks = _OrderedList()
         self._loopStartTimeUs = 0