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