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}