22
22
reportWarning ,
23
23
)
24
24
from wpilib .shuffleboard import Shuffleboard
25
+ import wpimath .units
25
26
26
27
_kResourceType_SmartDashboard = tResourceType .kResourceType_SmartDashboard
27
28
_kSmartDashboard_LiveWindow = tInstances .kSmartDashboard_LiveWindow
@@ -35,9 +36,55 @@ class IterativeRobotMode(Enum):
35
36
kTest = 4
36
37
37
38
39
+ # todo should this be IterativeRobotPy or IterativeRobotBase (replacing) or IterativeRobotBasePy
38
40
class IterativeRobotPy (RobotBase ):
41
+ """
42
+ IterativeRobotPy implements a specific type of robot program framework,
43
+ extending the RobotBase class. It provides similar functionality as
44
+ IterativeRobotBase does in a python wrapper of C++, but in pure python.
39
45
40
- def __init__ (self , period : float ):
46
+ The IterativeRobotPy class does not implement StartCompetition(), so it
47
+ should not be used by teams directly.
48
+
49
+ This class provides the following functions which are called by the main
50
+ loop, StartCompetition(), at the appropriate times:
51
+
52
+ robotInit() -- provide for initialization at robot power-on
53
+
54
+ DriverStationConnected() -- provide for initialization the first time the DS
55
+ is connected
56
+
57
+ Init() functions -- each of the following functions is called once when the
58
+ appropriate mode is entered:
59
+
60
+ - disabledInit() -- called each and every time disabled is entered from another mode
61
+ - autonomousInit() -- called each and every time autonomous is entered from another mode
62
+ - teleopInit() -- called each and every time teleop is entered from another mode
63
+ - testInit() -- called each and every time test is entered from another mode
64
+
65
+ Periodic() functions -- each of these functions is called on an interval:
66
+
67
+ - robotPeriodic()
68
+ - disabledPeriodic()
69
+ - autonomousPeriodic()
70
+ - teleopPeriodic()
71
+ - testPeriodic()
72
+
73
+ Exit() functions -- each of the following functions is called once when the
74
+ appropriate mode is exited:
75
+
76
+ - disabledExit() -- called each and every time disabled is exited
77
+ - autonomousExit() -- called each and every time autonomous is exited
78
+ - teleopExit() -- called each and every time teleop is exited
79
+ - testExit() -- called each and every time test is exited
80
+ """
81
+
82
+ def __init__ (self , period : wpimath .units .seconds ) -> None :
83
+ """
84
+ Constructor for IterativeRobotPy.
85
+
86
+ :param period: period of the main robot periodic loop in seconds.
87
+ """
41
88
super ().__init__ ()
42
89
self ._periodS = period
43
90
self .watchdog = Watchdog (self ._periodS , self .printLoopOverrunMessage )
@@ -56,73 +103,200 @@ def __init__(self, period: float):
56
103
self ._testPeriodicHasRun : bool = False
57
104
58
105
def robotInit (self ) -> None :
106
+ """
107
+ Robot-wide initialization code should go here.
108
+
109
+ Users should override this method for default Robot-wide initialization
110
+ which will be called when the robot is first powered on. It will be called
111
+ exactly one time.
112
+
113
+ Note: This method is functionally identical to the class constructor so
114
+ that should be used instead.
115
+ """
59
116
pass
60
117
61
118
def driverStationConnected (self ) -> None :
119
+ """
120
+ Code that needs to know the DS state should go here.
121
+
122
+ Users should override this method for initialization that needs to occur
123
+ after the DS is connected, such as needing the alliance information.
124
+ """
62
125
pass
63
126
64
127
def simulationInit (self ) -> None :
128
+ """
129
+ Robot-wide simulation initialization code should go here.
130
+
131
+ Users should override this method for default Robot-wide simulation
132
+ related initialization which will be called when the robot is first
133
+ started. It will be called exactly one time after robotInit is called
134
+ only when the robot is in simulation.
135
+ """
65
136
pass
66
137
67
138
def disabledInit (self ) -> None :
139
+ """
140
+ Initialization code for disabled mode should go here.
141
+
142
+ Users should override this method for initialization code which will be
143
+ called each time
144
+ the robot enters disabled mode.
145
+ """
68
146
pass
69
147
70
148
def autonomousInit (self ) -> None :
149
+ """
150
+ Initialization code for autonomous mode should go here.
151
+
152
+ Users should override this method for initialization code which will be
153
+ called each time the robot enters autonomous mode.
154
+ """
71
155
pass
72
156
73
157
def teleopInit (self ) -> None :
158
+ """
159
+ Initialization code for teleop mode should go here.
160
+
161
+ Users should override this method for initialization code which will be
162
+ called each time the robot enters teleop mode.
163
+ """
74
164
pass
75
165
76
166
def testInit (self ) -> None :
167
+ """
168
+ Initialization code for test mode should go here.
169
+
170
+ Users should override this method for initialization code which will be
171
+ called each time the robot enters test mode.
172
+ """
77
173
pass
78
174
79
175
def robotPeriodic (self ) -> None :
176
+ """
177
+ Periodic code for all modes should go here.
178
+
179
+ This function is called each time a new packet is received from the driver
180
+ station.
181
+ """
80
182
if not self ._robotPeriodicHasRun :
81
- print (f"Default RobotPeriodic () method...Override me!" )
183
+ print (f"Default robotPeriodic () method...Override me!" )
82
184
self ._robotPeriodicHasRun = True
83
185
186
+ # todo why is this _simulationPeriodic in previous code? Can it be simulationPeriodic and still work?
84
187
def simulationPeriodic (self ) -> None :
188
+ """
189
+ Periodic simulation code should go here.
190
+
191
+ This function is called in a simulated robot after user code executes.
192
+ """
85
193
if not self ._simulationPeriodicHasRun :
86
194
print (f"Default simulationPeriodic() method...Override me!" )
87
195
self ._simulationPeriodicHasRun = True
88
196
89
197
def disabledPeriodic (self ) -> None :
198
+ """
199
+ Periodic code for disabled mode should go here.
200
+
201
+ Users should override this method for code which will be called each time a
202
+ new packet is received from the driver station and the robot is in disabled
203
+ mode.
204
+ """
90
205
if not self ._disabledPeriodicHasRun :
91
206
print (f"Default disabledPeriodic() method...Override me!" )
92
207
self ._disabledPeriodicHasRun = True
93
208
94
209
def autonomousPeriodic (self ) -> None :
210
+ """
211
+ Periodic code for autonomous mode should go here.
212
+
213
+ Users should override this method for code which will be called each time a
214
+ new packet is received from the driver station and the robot is in
215
+ autonomous mode.
216
+ """
95
217
if not self ._autonomousPeriodicHasRun :
96
218
print (f"Default autonomousPeriodic() method...Override me!" )
97
219
self ._autonomousPeriodicHasRun = True
98
220
99
221
def teleopPeriodic (self ) -> None :
222
+ """
223
+ Periodic code for teleop mode should go here.
224
+
225
+ Users should override this method for code which will be called each time a
226
+ new packet is received from the driver station and the robot is in teleop
227
+ mode.
228
+ """
100
229
if not self ._teleopPeriodicHasRun :
101
230
print (f"Default teleopPeriodic() method...Override me!" )
102
231
self ._teleopPeriodicHasRun = True
103
232
104
233
def testPeriodic (self ) -> None :
234
+ """
235
+ Periodic code for test mode should go here.
236
+
237
+ Users should override this method for code which will be called each time a
238
+ new packet is received from the driver station and the robot is in test
239
+ mode.
240
+ """
105
241
if not self ._testPeriodicHasRun :
106
242
print (f"Default testPeriodic() method...Override me!" )
107
- self ._teleopPeriodicHasRun = True
243
+ self ._testPeriodicHasRun = True
108
244
109
245
def disabledExit (self ) -> None :
246
+ """
247
+ Exit code for disabled mode should go here.
248
+
249
+ Users should override this method for code which will be called each time
250
+ the robot exits disabled mode.
251
+ """
110
252
pass
111
253
112
254
def autonomousExit (self ) -> None :
255
+ """
256
+ Exit code for autonomous mode should go here.
257
+
258
+ Users should override this method for code which will be called each time
259
+ the robot exits autonomous mode.
260
+ """
113
261
pass
114
262
115
263
def teleopExit (self ) -> None :
264
+ """
265
+ Exit code for teleop mode should go here.
266
+
267
+ Users should override this method for code which will be called each time
268
+ the robot exits teleop mode.
269
+ """
116
270
pass
117
271
118
272
def testExit (self ) -> None :
273
+ """
274
+ Exit code for test mode should go here.
275
+
276
+ Users should override this method for code which will be called each time
277
+ the robot exits test mode.
278
+ """
119
279
pass
120
280
121
- # todo @Deprecated(forRemoval=true, since="2025")
122
281
def setNetworkTablesFlushEnabled (self , enabled : bool ) -> None :
282
+ """
283
+ Enables or disables flushing NetworkTables every loop iteration.
284
+ By default, this is enabled.
285
+
286
+ :deprecated: Deprecated without replacement.
287
+
288
+ :param enabled: True to enable, false to disable.
289
+ """
290
+
123
291
self ._ntFlushEnabled = enabled
124
292
125
293
def enableLiveWindowInTest (self , testLW : bool ) -> None :
294
+ """
295
+ Sets whether LiveWindow operation is enabled during test mode.
296
+
297
+ :param testLW: True to enable, false to disable. Defaults to false.
298
+ @throws if called in test mode.
299
+ """
126
300
if self .isTestEnabled ():
127
301
raise RuntimeError ("Can't configure test mode while in test mode!" )
128
302
if not self ._reportedLw and testLW :
@@ -131,12 +305,21 @@ def enableLiveWindowInTest(self, testLW: bool) -> None:
131
305
self ._lwEnabledInTest = testLW
132
306
133
307
def isLiveWindowEnabledInTest (self ) -> bool :
308
+ """
309
+ Whether LiveWindow operation is enabled during test mode.
310
+ """
134
311
return self ._lwEnabledInTest
135
312
136
- def getPeriod (self ) -> float :
313
+ def getPeriod (self ) -> wpimath .units .seconds :
314
+ """
315
+ Gets time period between calls to Periodic() functions.
316
+ """
137
317
return self ._periodS
138
318
139
- def loopFunc (self ) -> None :
319
+ def _loopFunc (self ) -> None :
320
+ """
321
+ Loop function.
322
+ """
140
323
DriverStation .refreshData ()
141
324
self .watchdog .reset ()
142
325
@@ -186,36 +369,36 @@ def loopFunc(self) -> None:
186
369
187
370
if self ._mode is IterativeRobotMode .kDisabled :
188
371
self .disabledInit ()
189
- self .watchdog .addEpoch ("DisabledInit ()" )
372
+ self .watchdog .addEpoch ("disabledInit ()" )
190
373
elif self ._mode is IterativeRobotMode .kAutonomous :
191
374
self .autonomousInit ()
192
- self .watchdog .addEpoch ("AutonomousInit ()" )
375
+ self .watchdog .addEpoch ("autonomousInit ()" )
193
376
elif self ._mode is IterativeRobotMode .kTeleop :
194
377
self .teleopInit ()
195
- self .watchdog .addEpoch ("TeleopInit ()" )
378
+ self .watchdog .addEpoch ("teleopInit ()" )
196
379
elif self ._mode is IterativeRobotMode .kTest :
197
380
if self ._lwEnabledInTest :
198
381
LiveWindow .setEnabled (True )
199
382
Shuffleboard .enableActuatorWidgets ()
200
383
self .testInit ()
201
- self .watchdog .addEpoch ("TestInit ()" )
384
+ self .watchdog .addEpoch ("testInit ()" )
202
385
"""
203
386
match self._mode:
204
387
case IterativeRobotMode.kDisabled:
205
388
self.disabledInit()
206
- self._watchdog.addEpoch("DisabledInit ()")
389
+ self._watchdog.addEpoch("disabledInit ()")
207
390
case IterativeRobotMode.kAutonomous:
208
391
self.autonomousInit()
209
- self._watchdog.addEpoch("AutonomousInit ()")
392
+ self._watchdog.addEpoch("autonomousInit ()")
210
393
case IterativeRobotMode.kTeleop:
211
394
self.teleopInit()
212
- self._watchdog.addEpoch("TeleopInit ()")
395
+ self._watchdog.addEpoch("teleopInit ()")
213
396
case IterativeRobotMode.kTest:
214
397
if self._lwEnabledInTest:
215
398
LiveWindow.setEnabled(True)
216
399
Shuffleboard.enableActuatorWidgets()
217
400
self.testInit()
218
- self._watchdog.addEpoch("TestInit ()")
401
+ self._watchdog.addEpoch("testInit ()")
219
402
"""
220
403
self ._lastMode = self ._mode
221
404
@@ -224,22 +407,22 @@ def loopFunc(self) -> None:
224
407
case IterativeRobotMode .kDisabled :
225
408
observeUserProgramDisabled ()
226
409
self .disabledPeriodic ()
227
- self .watchdog .addEpoch ("DisabledPeriodic ()" )
410
+ self .watchdog .addEpoch ("disabledPeriodic ()" )
228
411
case IterativeRobotMode .kAutonomous :
229
412
observeUserProgramAutonomous ()
230
413
self .autonomousPeriodic ()
231
- self .watchdog .addEpoch ("AutonomousPeriodic ()" )
414
+ self .watchdog .addEpoch ("autonomousPeriodic ()" )
232
415
case IterativeRobotMode .kTeleop :
233
416
observeUserProgramTeleop ()
234
417
self .teleopPeriodic ()
235
- self .watchdog .addEpoch ("TeleopPeriodic ()" )
418
+ self .watchdog .addEpoch ("teleopPeriodic ()" )
236
419
case IterativeRobotMode .kTest :
237
420
observeUserProgramTest ()
238
421
self .testPeriodic ()
239
- self .watchdog .addEpoch ("TestPeriodic ()" )
422
+ self .watchdog .addEpoch ("testPeriodic ()" )
240
423
241
424
self .robotPeriodic ()
242
- self .watchdog .addEpoch ("RobotPeriodic ()" )
425
+ self .watchdog .addEpoch ("robotPeriodic ()" )
243
426
244
427
SmartDashboard .updateValues ()
245
428
self .watchdog .addEpoch ("SmartDashboard::UpdateValues()" )
@@ -270,4 +453,7 @@ def printLoopOverrunMessage(self) -> None:
270
453
reportWarning (f"Loop time of { self .watchdog .getTimeout ()} s overrun" , False )
271
454
272
455
def printWatchdogEpochs (self ) -> None :
456
+ """
457
+ Prints list of epochs added so far and their times.
458
+ """
273
459
self .watchdog .printEpochs ()
0 commit comments