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