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