11from  typing  import  Any , Callable , Iterable 
22from  heapq  import  heappush , heappop 
3- from  hal  import  report , initializeNotifier , setNotifierName , observeUserProgramStarting , updateNotifierAlarm , \
4-     waitForNotifierAlarm , stopNotifier , tResourceType , tInstances 
3+ from  hal  import  (
4+     report ,
5+     initializeNotifier ,
6+     setNotifierName ,
7+     observeUserProgramStarting ,
8+     updateNotifierAlarm ,
9+     waitForNotifierAlarm ,
10+     stopNotifier ,
11+     tResourceType ,
12+     tInstances ,
13+ )
514from  wpilib  import  RobotController 
615
716from  .iterativerobotpy  import  IterativeRobotPy 
1019_kResourceType_Framework  =  tResourceType .kResourceType_Framework 
1120_kFramework_Timed  =  tInstances .kFramework_Timed 
1221
22+ 
1323class  _Callback :
14-     def  __init__ (self , func : Callable [[],None ], periodUs : int , expirationUs : int ):
24+     def  __init__ (self , func : Callable [[],  None ], periodUs : int , expirationUs : int ):
1525        self .func  =  func 
1626        self ._periodUs  =  periodUs 
1727        self .expirationUs  =  expirationUs 
1828
1929    @classmethod  
20-     def  makeCallBack (cls ,
21-                      func : Callable [[],None ],
22-                      startTimeUs : int ,
23-                      periodUs : int ,
24-                      offsetUs : int ) ->  "_Callback" :
25- 
26-         callback  =  _Callback (
27-             func = func ,
28-             periodUs = periodUs ,
29-             expirationUs = startTimeUs 
30-         )
30+     def  makeCallBack (
31+         cls , func : Callable [[], None ], startTimeUs : int , periodUs : int , offsetUs : int 
32+     ) ->  "_Callback" :
33+ 
34+         callback  =  _Callback (func = func , periodUs = periodUs , expirationUs = startTimeUs )
3135
3236        currentTimeUs  =  _getFPGATime ()
33-         callback .expirationUs  =  offsetUs  +  callback .calcFutureExpirationUs (currentTimeUs )
37+         callback .expirationUs  =  offsetUs  +  callback .calcFutureExpirationUs (
38+             currentTimeUs 
39+         )
3440        return  callback 
3541
3642    def  calcFutureExpirationUs (self , currentTimeUs : int ) ->  int :
@@ -40,8 +46,11 @@ def calcFutureExpirationUs(self, currentTimeUs: int) -> int:
4046        # callback wouldn't be running otherwise. 
4147        # todo does this math work? 
4248        # todo does the "// periodUs * periodUs" do the correct integer math? 
43-         return  self .expirationUs  +  self ._periodUs  +  \
44-             ((currentTimeUs  -  self .expirationUs ) //  self ._periodUs ) *  self ._periodUs 
49+         return  (
50+             self .expirationUs 
51+             +  self ._periodUs 
52+             +  ((currentTimeUs  -  self .expirationUs ) //  self ._periodUs ) *  self ._periodUs 
53+         )
4554
4655    def  setNextStartTimeUs (self , currentTimeUs : int ) ->  None :
4756        self .expirationUs  =  self .calcFutureExpirationUs (currentTimeUs )
@@ -63,7 +72,7 @@ def add(self, item: Any) -> None:
6372    def  pop (self ) ->  Any :
6473        return  heappop (self ._data )
6574
66-     def  peek (self ) ->  Any | None :
75+     def  peek (self ) ->  Any   |   None :
6776        if  self ._data :
6877            return  self ._data [0 ]
6978        else :
@@ -94,7 +103,9 @@ def __init__(self, period: float = 0.020):
94103
95104        self ._notifier , status  =  initializeNotifier ()
96105        if  status  !=  0 :
97-             raise  RuntimeError (f"initializeNotifier() returned { self ._notifier }  , { status }  " )
106+             raise  RuntimeError (
107+                 f"initializeNotifier() returned { self ._notifier }  , { status }  " 
108+             )
98109
99110        status  =  setNotifierName (self ._notifier , "TimedRobot" )
100111        if  status  !=  0 :
@@ -126,7 +137,9 @@ def startCompetition(self) -> None:
126137
127138            currentTimeUs , status  =  waitForNotifierAlarm (self ._notifier )
128139            if  status  !=  0 :
129-                 raise  RuntimeError (f"waitForNotifierAlarm() returned currentTimeUs={ currentTimeUs }   status={ status }  " )
140+                 raise  RuntimeError (
141+                     f"waitForNotifierAlarm() returned currentTimeUs={ currentTimeUs }   status={ status }  " 
142+                 )
130143
131144            if  currentTimeUs  ==  0 :
132145                # when HAL_StopNotifier(self.notifier) is called the above waitForNotifierAlarm 
@@ -142,7 +155,9 @@ def startCompetition(self) -> None:
142155                callback  =  self ._callbacks .pop ()
143156                self ._runCallbackAndReschedule (callback , currentTimeUs )
144157
145-     def  _runCallbackAndReschedule (self , callback : Callable [[],None ], currentTimeUs : int ) ->  None :
158+     def  _runCallbackAndReschedule (
159+         self , callback : Callable [[], None ], currentTimeUs : int 
160+     ) ->  None :
146161        callback .func ()
147162        callback .setNextStartTimeUs (currentTimeUs )
148163        self ._callbacks .add (callback )
@@ -151,16 +166,16 @@ def endCompetition(self) -> None:
151166        stopNotifier (self ._notifier )
152167
153168    def  getLoopStartTime (self ) ->  float :
154-         return  self .loopStartTimeUs / 1e6   # units are seconds 
155- 
156-     def  addPeriodic (self ,
157-                     callback : Callable [[],None ],
158-                     period : float , # units are seconds 
159-                     offset : float  =  0.0  # units are seconds 
160-                     ) ->  None :
169+         return  self .loopStartTimeUs  /  1e6   # units are seconds 
170+ 
171+     def  addPeriodic (
172+         self ,
173+         callback : Callable [[], None ],
174+         period : float ,  # units are seconds 
175+         offset : float  =  0.0 ,  # units are seconds 
176+     ) ->  None :
161177        self ._callbacks .add (
162178            _Callback .makeCallBack (
163-                 callback ,
164-                 self ._startTimeUs , int (period  *  1e6 ), int (offset  *  1e6 )
179+                 callback , self ._startTimeUs , int (period  *  1e6 ), int (offset  *  1e6 )
165180            )
166181        )
0 commit comments