forked from wpilibsuite/allwpilib
-
Notifications
You must be signed in to change notification settings - Fork 0
/
IterativeRobotBase.java
405 lines (355 loc) · 12.2 KB
/
IterativeRobotBase.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.ConcurrentModificationException;
/**
* IterativeRobotBase implements a specific type of robot program framework, extending the RobotBase
* class.
*
* <p>The IterativeRobotBase class does not implement startCompetition(), so it should not be used
* by teams directly.
*
* <p>This class provides the following functions which are called by the main loop,
* startCompetition(), at the appropriate times:
*
* <p>robotInit() -- provide for initialization at robot power-on
*
* <p>driverStationConnected() -- provide for initialization the first time the DS is connected
*
* <p>init() functions -- each of the following functions is called once when the appropriate mode
* is entered:
*
* <ul>
* <li>disabledInit() -- called each and every time disabled is entered from another mode
* <li>autonomousInit() -- called each and every time autonomous is entered from another mode
* <li>teleopInit() -- called each and every time teleop is entered from another mode
* <li>testInit() -- called each and every time test is entered from another mode
* </ul>
*
* <p>periodic() functions -- each of these functions is called on an interval:
*
* <ul>
* <li>robotPeriodic()
* <li>disabledPeriodic()
* <li>autonomousPeriodic()
* <li>teleopPeriodic()
* <li>testPeriodic()
* </ul>
*
* <p>exit() functions -- each of the following functions is called once when the appropriate mode
* is exited:
*
* <ul>
* <li>disabledExit() -- called each and every time disabled is exited
* <li>autonomousExit() -- called each and every time autonomous is exited
* <li>teleopExit() -- called each and every time teleop is exited
* <li>testExit() -- called each and every time test is exited
* </ul>
*/
public abstract class IterativeRobotBase extends RobotBase {
private enum Mode {
kNone,
kDisabled,
kAutonomous,
kTeleop,
kTest
}
private final DSControlWord m_word = new DSControlWord();
private Mode m_lastMode = Mode.kNone;
private final double m_period;
private final Watchdog m_watchdog;
private boolean m_ntFlushEnabled = true;
private boolean m_lwEnabledInTest;
private boolean m_calledDsConnected;
/**
* Constructor for IterativeRobotBase.
*
* @param period Period in seconds.
*/
protected IterativeRobotBase(double period) {
m_period = period;
m_watchdog = new Watchdog(period, this::printLoopOverrunMessage);
}
/** Provide an alternate "main loop" via startCompetition(). */
@Override
public abstract void startCompetition();
/* ----------- Overridable initialization code ----------------- */
/**
* Robot-wide initialization code should go here.
*
* <p>Users should override this method for default Robot-wide initialization which will be called
* when the robot is first powered on. It will be called exactly one time.
*
* <p>Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" indicators will be off
* until RobotInit() exits. Code in RobotInit() that waits for enable will cause the robot to
* never indicate that the code is ready, causing the robot to be bypassed in a match.
*/
public void robotInit() {}
/**
* Code that needs to know the DS state should go here.
*
* <p>Users should override this method for initialization that needs to occur after the DS is
* connected, such as needing the alliance information.
*/
public void driverStationConnected() {}
/**
* Robot-wide simulation initialization code should go here.
*
* <p>Users should override this method for default Robot-wide simulation related initialization
* which will be called when the robot is first started. It will be called exactly one time after
* RobotInit is called only when the robot is in simulation.
*/
public void simulationInit() {}
/**
* Initialization code for disabled mode should go here.
*
* <p>Users should override this method for initialization code which will be called each time the
* robot enters disabled mode.
*/
public void disabledInit() {}
/**
* Initialization code for autonomous mode should go here.
*
* <p>Users should override this method for initialization code which will be called each time the
* robot enters autonomous mode.
*/
public void autonomousInit() {}
/**
* Initialization code for teleop mode should go here.
*
* <p>Users should override this method for initialization code which will be called each time the
* robot enters teleop mode.
*/
public void teleopInit() {}
/**
* Initialization code for test mode should go here.
*
* <p>Users should override this method for initialization code which will be called each time the
* robot enters test mode.
*/
public void testInit() {}
/* ----------- Overridable periodic code ----------------- */
private boolean m_rpFirstRun = true;
/** Periodic code for all robot modes should go here. */
public void robotPeriodic() {
if (m_rpFirstRun) {
System.out.println("Default robotPeriodic() method... Override me!");
m_rpFirstRun = false;
}
}
private boolean m_spFirstRun = true;
/**
* Periodic simulation code should go here.
*
* <p>This function is called in a simulated robot after user code executes.
*/
public void simulationPeriodic() {
if (m_spFirstRun) {
System.out.println("Default simulationPeriodic() method... Override me!");
m_spFirstRun = false;
}
}
private boolean m_dpFirstRun = true;
/** Periodic code for disabled mode should go here. */
public void disabledPeriodic() {
if (m_dpFirstRun) {
System.out.println("Default disabledPeriodic() method... Override me!");
m_dpFirstRun = false;
}
}
private boolean m_apFirstRun = true;
/** Periodic code for autonomous mode should go here. */
public void autonomousPeriodic() {
if (m_apFirstRun) {
System.out.println("Default autonomousPeriodic() method... Override me!");
m_apFirstRun = false;
}
}
private boolean m_tpFirstRun = true;
/** Periodic code for teleop mode should go here. */
public void teleopPeriodic() {
if (m_tpFirstRun) {
System.out.println("Default teleopPeriodic() method... Override me!");
m_tpFirstRun = false;
}
}
private boolean m_tmpFirstRun = true;
/** Periodic code for test mode should go here. */
public void testPeriodic() {
if (m_tmpFirstRun) {
System.out.println("Default testPeriodic() method... Override me!");
m_tmpFirstRun = false;
}
}
/**
* Exit code for disabled mode should go here.
*
* <p>Users should override this method for code which will be called each time the robot exits
* disabled mode.
*/
public void disabledExit() {}
/**
* Exit code for autonomous mode should go here.
*
* <p>Users should override this method for code which will be called each time the robot exits
* autonomous mode.
*/
public void autonomousExit() {}
/**
* Exit code for teleop mode should go here.
*
* <p>Users should override this method for code which will be called each time the robot exits
* teleop mode.
*/
public void teleopExit() {}
/**
* Exit code for test mode should go here.
*
* <p>Users should override this method for code which will be called each time the robot exits
* test mode.
*/
public void testExit() {}
/**
* Enables or disables flushing NetworkTables every loop iteration. By default, this is enabled.
*
* @param enabled True to enable, false to disable
*/
public void setNetworkTablesFlushEnabled(boolean enabled) {
m_ntFlushEnabled = enabled;
}
/**
* Sets whether LiveWindow operation is enabled during test mode. Calling
*
* @param testLW True to enable, false to disable. Defaults to false.
* @throws ConcurrentModificationException if this is called during test mode.
*/
public void enableLiveWindowInTest(boolean testLW) {
if (isTestEnabled()) {
throw new ConcurrentModificationException("Can't configure test mode while in test mode!");
}
m_lwEnabledInTest = testLW;
}
/**
* Whether LiveWindow operation is enabled during test mode.
*
* @return whether LiveWindow should be enabled in test mode.
*/
public boolean isLiveWindowEnabledInTest() {
return m_lwEnabledInTest;
}
/**
* Gets time period between calls to Periodic() functions.
*
* @return The time period between calls to Periodic() functions.
*/
public double getPeriod() {
return m_period;
}
protected void loopFunc() {
DriverStation.refreshData();
m_watchdog.reset();
m_word.refresh();
// Get current mode
Mode mode = Mode.kNone;
if (m_word.isDisabled()) {
mode = Mode.kDisabled;
} else if (m_word.isAutonomous()) {
mode = Mode.kAutonomous;
} else if (m_word.isTeleop()) {
mode = Mode.kTeleop;
} else if (m_word.isTest()) {
mode = Mode.kTest;
}
if (!m_calledDsConnected && m_word.isDSAttached()) {
m_calledDsConnected = true;
driverStationConnected();
}
// If mode changed, call mode exit and entry functions
if (m_lastMode != mode) {
// Call last mode's exit function
if (m_lastMode == Mode.kDisabled) {
disabledExit();
} else if (m_lastMode == Mode.kAutonomous) {
autonomousExit();
} else if (m_lastMode == Mode.kTeleop) {
teleopExit();
} else if (m_lastMode == Mode.kTest) {
if (m_lwEnabledInTest) {
LiveWindow.setEnabled(false);
Shuffleboard.disableActuatorWidgets();
}
testExit();
}
// Call current mode's entry function
if (mode == Mode.kDisabled) {
disabledInit();
m_watchdog.addEpoch("disabledInit()");
} else if (mode == Mode.kAutonomous) {
autonomousInit();
m_watchdog.addEpoch("autonomousInit()");
} else if (mode == Mode.kTeleop) {
teleopInit();
m_watchdog.addEpoch("teleopInit()");
} else if (mode == Mode.kTest) {
if (m_lwEnabledInTest) {
LiveWindow.setEnabled(true);
Shuffleboard.enableActuatorWidgets();
}
testInit();
m_watchdog.addEpoch("testInit()");
}
m_lastMode = mode;
}
// Call the appropriate function depending upon the current robot mode
if (mode == Mode.kDisabled) {
DriverStationJNI.observeUserProgramDisabled();
disabledPeriodic();
m_watchdog.addEpoch("disabledPeriodic()");
} else if (mode == Mode.kAutonomous) {
DriverStationJNI.observeUserProgramAutonomous();
autonomousPeriodic();
m_watchdog.addEpoch("autonomousPeriodic()");
} else if (mode == Mode.kTeleop) {
DriverStationJNI.observeUserProgramTeleop();
teleopPeriodic();
m_watchdog.addEpoch("teleopPeriodic()");
} else {
DriverStationJNI.observeUserProgramTest();
testPeriodic();
m_watchdog.addEpoch("testPeriodic()");
}
robotPeriodic();
m_watchdog.addEpoch("robotPeriodic()");
SmartDashboard.updateValues();
m_watchdog.addEpoch("SmartDashboard.updateValues()");
LiveWindow.updateValues();
m_watchdog.addEpoch("LiveWindow.updateValues()");
Shuffleboard.update();
m_watchdog.addEpoch("Shuffleboard.update()");
if (isSimulation()) {
HAL.simPeriodicBefore();
simulationPeriodic();
HAL.simPeriodicAfter();
m_watchdog.addEpoch("simulationPeriodic()");
}
m_watchdog.disable();
// Flush NetworkTables
if (m_ntFlushEnabled) {
NetworkTableInstance.getDefault().flushLocal();
}
// Warn on loop time overruns
if (m_watchdog.isExpired()) {
m_watchdog.printEpochs();
}
}
private void printLoopOverrunMessage() {
DriverStation.reportWarning("Loop time of " + m_period + "s overrun\n", false);
}
}