Source code

001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj;
006
007import edu.wpi.first.hal.DriverStationJNI;
008import edu.wpi.first.hal.HAL;
009import edu.wpi.first.networktables.NetworkTableInstance;
010import edu.wpi.first.wpilibj.livewindow.LiveWindow;
011import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
012import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
013import java.util.ConcurrentModificationException;
014
015/**
016 * IterativeRobotBase implements a specific type of robot program framework, extending the RobotBase
017 * class.
018 *
019 * <p>The IterativeRobotBase class does not implement startCompetition(), so it should not be used
020 * by teams directly.
021 *
022 * <p>This class provides the following functions which are called by the main loop,
023 * startCompetition(), at the appropriate times:
024 *
025 * <p>robotInit() -- provide for initialization at robot power-on
026 *
027 * <p>init() functions -- each of the following functions is called once when the appropriate mode
028 * is entered:
029 *
030 * <ul>
031 *   <li>disabledInit() -- called each and every time disabled is entered from another mode
032 *   <li>autonomousInit() -- called each and every time autonomous is entered from another mode
033 *   <li>teleopInit() -- called each and every time teleop is entered from another mode
034 *   <li>testInit() -- called each and every time test is entered from another mode
035 * </ul>
036 *
037 * <p>periodic() functions -- each of these functions is called on an interval:
038 *
039 * <ul>
040 *   <li>robotPeriodic()
041 *   <li>disabledPeriodic()
042 *   <li>autonomousPeriodic()
043 *   <li>teleopPeriodic()
044 *   <li>testPeriodic()
045 * </ul>
046 *
047 * <p>exit() functions -- each of the following functions is called once when the appropriate mode
048 * is exited:
049 *
050 * <ul>
051 *   <li>disabledExit() -- called each and every time disabled is exited
052 *   <li>autonomousExit() -- called each and every time autonomous is exited
053 *   <li>teleopExit() -- called each and every time teleop is exited
054 *   <li>testExit() -- called each and every time test is exited
055 * </ul>
056 */
057public abstract class IterativeRobotBase extends RobotBase {
058  private enum Mode {
059    kNone,
060    kDisabled,
061    kAutonomous,
062    kTeleop,
063    kTest
064  }
065
066  private final DSControlWord m_word = new DSControlWord();
067  private Mode m_lastMode = Mode.kNone;
068  private final double m_period;
069  private final Watchdog m_watchdog;
070  private boolean m_ntFlushEnabled = true;
071  private boolean m_lwEnabledInTest = true;
072
073  /**
074   * Constructor for IterativeRobotBase.
075   *
076   * @param period Period in seconds.
077   */
078  protected IterativeRobotBase(double period) {
079    m_period = period;
080    m_watchdog = new Watchdog(period, this::printLoopOverrunMessage);
081  }
082
083  /** Provide an alternate "main loop" via startCompetition(). */
084  @Override
085  public abstract void startCompetition();
086
087  /* ----------- Overridable initialization code ----------------- */
088
089  /**
090   * Robot-wide initialization code should go here.
091   *
092   * <p>Users should override this method for default Robot-wide initialization which will be called
093   * when the robot is first powered on. It will be called exactly one time.
094   *
095   * <p>Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" indicators will be off
096   * until RobotInit() exits. Code in RobotInit() that waits for enable will cause the robot to
097   * never indicate that the code is ready, causing the robot to be bypassed in a match.
098   */
099  public void robotInit() {}
100
101  /**
102   * Robot-wide simulation initialization code should go here.
103   *
104   * <p>Users should override this method for default Robot-wide simulation related initialization
105   * which will be called when the robot is first started. It will be called exactly one time after
106   * RobotInit is called only when the robot is in simulation.
107   */
108  public void simulationInit() {}
109
110  /**
111   * Initialization code for disabled mode should go here.
112   *
113   * <p>Users should override this method for initialization code which will be called each time the
114   * robot enters disabled mode.
115   */
116  public void disabledInit() {}
117
118  /**
119   * Initialization code for autonomous mode should go here.
120   *
121   * <p>Users should override this method for initialization code which will be called each time the
122   * robot enters autonomous mode.
123   */
124  public void autonomousInit() {}
125
126  /**
127   * Initialization code for teleop mode should go here.
128   *
129   * <p>Users should override this method for initialization code which will be called each time the
130   * robot enters teleop mode.
131   */
132  public void teleopInit() {}
133
134  /**
135   * Initialization code for test mode should go here.
136   *
137   * <p>Users should override this method for initialization code which will be called each time the
138   * robot enters test mode.
139   */
140  public void testInit() {}
141
142  /* ----------- Overridable periodic code ----------------- */
143
144  private boolean m_rpFirstRun = true;
145
146  /** Periodic code for all robot modes should go here. */
147  public void robotPeriodic() {
148    if (m_rpFirstRun) {
149      System.out.println("Default robotPeriodic() method... Override me!");
150      m_rpFirstRun = false;
151    }
152  }
153
154  private boolean m_spFirstRun = true;
155
156  /**
157   * Periodic simulation code should go here.
158   *
159   * <p>This function is called in a simulated robot after user code executes.
160   */
161  public void simulationPeriodic() {
162    if (m_spFirstRun) {
163      System.out.println("Default simulationPeriodic() method... Override me!");
164      m_spFirstRun = false;
165    }
166  }
167
168  private boolean m_dpFirstRun = true;
169
170  /** Periodic code for disabled mode should go here. */
171  public void disabledPeriodic() {
172    if (m_dpFirstRun) {
173      System.out.println("Default disabledPeriodic() method... Override me!");
174      m_dpFirstRun = false;
175    }
176  }
177
178  private boolean m_apFirstRun = true;
179
180  /** Periodic code for autonomous mode should go here. */
181  public void autonomousPeriodic() {
182    if (m_apFirstRun) {
183      System.out.println("Default autonomousPeriodic() method... Override me!");
184      m_apFirstRun = false;
185    }
186  }
187
188  private boolean m_tpFirstRun = true;
189
190  /** Periodic code for teleop mode should go here. */
191  public void teleopPeriodic() {
192    if (m_tpFirstRun) {
193      System.out.println("Default teleopPeriodic() method... Override me!");
194      m_tpFirstRun = false;
195    }
196  }
197
198  private boolean m_tmpFirstRun = true;
199
200  /** Periodic code for test mode should go here. */
201  public void testPeriodic() {
202    if (m_tmpFirstRun) {
203      System.out.println("Default testPeriodic() method... Override me!");
204      m_tmpFirstRun = false;
205    }
206  }
207
208  /**
209   * Exit code for disabled mode should go here.
210   *
211   * <p>Users should override this method for code which will be called each time the robot exits
212   * disabled mode.
213   */
214  public void disabledExit() {}
215
216  /**
217   * Exit code for autonomous mode should go here.
218   *
219   * <p>Users should override this method for code which will be called each time the robot exits
220   * autonomous mode.
221   */
222  public void autonomousExit() {}
223
224  /**
225   * Exit code for teleop mode should go here.
226   *
227   * <p>Users should override this method for code which will be called each time the robot exits
228   * teleop mode.
229   */
230  public void teleopExit() {}
231
232  /**
233   * Exit code for test mode should go here.
234   *
235   * <p>Users should override this method for code which will be called each time the robot exits
236   * test mode.
237   */
238  public void testExit() {}
239
240  /**
241   * Enables or disables flushing NetworkTables every loop iteration. By default, this is enabled.
242   *
243   * @param enabled True to enable, false to disable
244   */
245  public void setNetworkTablesFlushEnabled(boolean enabled) {
246    m_ntFlushEnabled = enabled;
247  }
248
249  /**
250   * Sets whether LiveWindow operation is enabled during test mode. Calling
251   *
252   * @param testLW True to enable, false to disable. Defaults to true.
253   * @throws ConcurrentModificationException if this is called during test mode.
254   */
255  public void enableLiveWindowInTest(boolean testLW) {
256    if (isTestEnabled()) {
257      throw new ConcurrentModificationException("Can't configure test mode while in test mode!");
258    }
259    m_lwEnabledInTest = testLW;
260  }
261
262  /**
263   * Whether LiveWindow operation is enabled during test mode.
264   *
265   * @return whether LiveWindow should be enabled in test mode.
266   */
267  public boolean isLiveWindowEnabledInTest() {
268    return m_lwEnabledInTest;
269  }
270
271  /**
272   * Gets time period between calls to Periodic() functions.
273   *
274   * @return The time period between calls to Periodic() functions.
275   */
276  public double getPeriod() {
277    return m_period;
278  }
279
280  protected void loopFunc() {
281    DriverStation.refreshData();
282    m_watchdog.reset();
283
284    m_word.refresh();
285
286    // Get current mode
287    Mode mode = Mode.kNone;
288    if (m_word.isDisabled()) {
289      mode = Mode.kDisabled;
290    } else if (m_word.isAutonomous()) {
291      mode = Mode.kAutonomous;
292    } else if (m_word.isTeleop()) {
293      mode = Mode.kTeleop;
294    } else if (m_word.isTest()) {
295      mode = Mode.kTest;
296    }
297
298    // If mode changed, call mode exit and entry functions
299    if (m_lastMode != mode) {
300      // Call last mode's exit function
301      if (m_lastMode == Mode.kDisabled) {
302        disabledExit();
303      } else if (m_lastMode == Mode.kAutonomous) {
304        autonomousExit();
305      } else if (m_lastMode == Mode.kTeleop) {
306        teleopExit();
307      } else if (m_lastMode == Mode.kTest) {
308        if (m_lwEnabledInTest) {
309          LiveWindow.setEnabled(false);
310          Shuffleboard.disableActuatorWidgets();
311        }
312        testExit();
313      }
314
315      // Call current mode's entry function
316      if (mode == Mode.kDisabled) {
317        disabledInit();
318        m_watchdog.addEpoch("disabledInit()");
319      } else if (mode == Mode.kAutonomous) {
320        autonomousInit();
321        m_watchdog.addEpoch("autonomousInit()");
322      } else if (mode == Mode.kTeleop) {
323        teleopInit();
324        m_watchdog.addEpoch("teleopInit()");
325      } else if (mode == Mode.kTest) {
326        if (m_lwEnabledInTest) {
327          LiveWindow.setEnabled(true);
328          Shuffleboard.enableActuatorWidgets();
329        }
330        testInit();
331        m_watchdog.addEpoch("testInit()");
332      }
333
334      m_lastMode = mode;
335    }
336
337    // Call the appropriate function depending upon the current robot mode
338    if (mode == Mode.kDisabled) {
339      DriverStationJNI.observeUserProgramDisabled();
340      disabledPeriodic();
341      m_watchdog.addEpoch("disabledPeriodic()");
342    } else if (mode == Mode.kAutonomous) {
343      DriverStationJNI.observeUserProgramAutonomous();
344      autonomousPeriodic();
345      m_watchdog.addEpoch("autonomousPeriodic()");
346    } else if (mode == Mode.kTeleop) {
347      DriverStationJNI.observeUserProgramTeleop();
348      teleopPeriodic();
349      m_watchdog.addEpoch("teleopPeriodic()");
350    } else {
351      DriverStationJNI.observeUserProgramTest();
352      testPeriodic();
353      m_watchdog.addEpoch("testPeriodic()");
354    }
355
356    robotPeriodic();
357    m_watchdog.addEpoch("robotPeriodic()");
358
359    SmartDashboard.updateValues();
360    m_watchdog.addEpoch("SmartDashboard.updateValues()");
361    LiveWindow.updateValues();
362    m_watchdog.addEpoch("LiveWindow.updateValues()");
363    Shuffleboard.update();
364    m_watchdog.addEpoch("Shuffleboard.update()");
365
366    if (isSimulation()) {
367      HAL.simPeriodicBefore();
368      simulationPeriodic();
369      HAL.simPeriodicAfter();
370      m_watchdog.addEpoch("simulationPeriodic()");
371    }
372
373    m_watchdog.disable();
374
375    // Flush NetworkTables
376    if (m_ntFlushEnabled) {
377      NetworkTableInstance.getDefault().flushLocal();
378    }
379
380    // Warn on loop time overruns
381    if (m_watchdog.isExpired()) {
382      m_watchdog.printEpochs();
383    }
384  }
385
386  private void printLoopOverrunMessage() {
387    DriverStation.reportWarning("Loop time of " + m_period + "s overrun\n", false);
388  }
389}