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