diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp index 414bfb23b6..5a74927773 100644 --- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp +++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp @@ -94,6 +94,14 @@ void IterativeRobotBase::TestPeriodic() { } } +void IterativeRobotBase::DisabledExit() {} + +void IterativeRobotBase::AutonomousExit() {} + +void IterativeRobotBase::TeleopExit() {} + +void IterativeRobotBase::TestExit() {} + void IterativeRobotBase::SetNetworkTablesFlushEnabled(bool enabled) { m_ntFlushEnabled = enabled; } @@ -105,60 +113,67 @@ units::second_t IterativeRobotBase::GetPeriod() const { void IterativeRobotBase::LoopFunc() { m_watchdog.Reset(); - // Call the appropriate function depending upon the current robot mode + // Get current mode + Mode mode = Mode::kNone; if (IsDisabled()) { - // Call DisabledInit() if we are now just entering disabled mode from - // either a different mode or from power-on. - if (m_lastMode != Mode::kDisabled) { + mode = Mode::kDisabled; + } else if (IsAutonomous()) { + mode = Mode::kAutonomous; + } else if (IsOperatorControl()) { + mode = Mode::kTeleop; + } else if (IsTest()) { + mode = Mode::kTest; + } + + // If mode changed, call mode exit and entry functions + if (m_lastMode != mode) { + // Call last mode's exit function + if (m_lastMode == Mode::kDisabled) { + DisabledExit(); + } else if (m_lastMode == Mode::kAutonomous) { + AutonomousExit(); + } else if (m_lastMode == Mode::kTeleop) { + TeleopExit(); + } else if (m_lastMode == Mode::kTest) { LiveWindow::SetEnabled(false); Shuffleboard::DisableActuatorWidgets(); + TestExit(); + } + + // Call current mode's entry function + if (mode == Mode::kDisabled) { DisabledInit(); m_watchdog.AddEpoch("DisabledInit()"); - m_lastMode = Mode::kDisabled; - } - - HAL_ObserveUserProgramDisabled(); - DisabledPeriodic(); - m_watchdog.AddEpoch("DisabledPeriodic()"); - } else if (IsAutonomous()) { - // Call AutonomousInit() if we are now just entering autonomous mode from - // either a different mode or from power-on. - if (m_lastMode != Mode::kAutonomous) { - LiveWindow::SetEnabled(false); - Shuffleboard::DisableActuatorWidgets(); + } else if (mode == Mode::kAutonomous) { AutonomousInit(); m_watchdog.AddEpoch("AutonomousInit()"); - m_lastMode = Mode::kAutonomous; - } - - HAL_ObserveUserProgramAutonomous(); - AutonomousPeriodic(); - m_watchdog.AddEpoch("AutonomousPeriodic()"); - } else if (IsOperatorControl()) { - // Call TeleopInit() if we are now just entering teleop mode from - // either a different mode or from power-on. - if (m_lastMode != Mode::kTeleop) { - LiveWindow::SetEnabled(false); - Shuffleboard::DisableActuatorWidgets(); + } else if (mode == Mode::kTeleop) { TeleopInit(); m_watchdog.AddEpoch("TeleopInit()"); - m_lastMode = Mode::kTeleop; - } - - HAL_ObserveUserProgramTeleop(); - TeleopPeriodic(); - m_watchdog.AddEpoch("TeleopPeriodic()"); - } else { - // Call TestInit() if we are now just entering test mode from - // either a different mode or from power-on. - if (m_lastMode != Mode::kTest) { + } else if (mode == Mode::kTest) { LiveWindow::SetEnabled(true); Shuffleboard::EnableActuatorWidgets(); TestInit(); m_watchdog.AddEpoch("TestInit()"); - m_lastMode = Mode::kTest; } + m_lastMode = mode; + } + + // Call the appropriate function depending upon the current robot mode + if (mode == Mode::kDisabled) { + HAL_ObserveUserProgramDisabled(); + DisabledPeriodic(); + m_watchdog.AddEpoch("DisabledPeriodic()"); + } else if (mode == Mode::kAutonomous) { + HAL_ObserveUserProgramAutonomous(); + AutonomousPeriodic(); + m_watchdog.AddEpoch("AutonomousPeriodic()"); + } else if (mode == Mode::kTeleop) { + HAL_ObserveUserProgramTeleop(); + TeleopPeriodic(); + m_watchdog.AddEpoch("TeleopPeriodic()"); + } else if (mode == Mode::kTest) { HAL_ObserveUserProgramTest(); TestPeriodic(); m_watchdog.AddEpoch("TestPeriodic()"); diff --git a/wpilibc/src/main/native/include/frc/IterativeRobotBase.h b/wpilibc/src/main/native/include/frc/IterativeRobotBase.h index beb7f4cbe1..c4253efdc3 100644 --- a/wpilibc/src/main/native/include/frc/IterativeRobotBase.h +++ b/wpilibc/src/main/native/include/frc/IterativeRobotBase.h @@ -25,22 +25,32 @@ namespace frc { * RobotInit() -- provide for initialization at robot power-on * * Init() functions -- each of the following functions is called once when the - * appropriate mode is entered: - * - DisabledInit() -- called each and every time disabled is entered from - * another mode - * - AutonomousInit() -- called each and every time autonomous is entered from - * another mode - * - TeleopInit() -- called each and every time teleop is entered from - * another mode - * - TestInit() -- called each and every time test is entered from - * another mode + * appropriate mode is entered: + * + * \li DisabledInit() -- called each and every time disabled is entered from + * another mode + * \li AutonomousInit() -- called each and every time autonomous is entered from + * another mode + * \li TeleopInit() -- called each and every time teleop is entered from another + * mode + * \li TestInit() -- called each and every time test is entered from another + * mode * * Periodic() functions -- each of these functions is called on an interval: - * - RobotPeriodic() - * - DisabledPeriodic() - * - AutonomousPeriodic() - * - TeleopPeriodic() - * - TestPeriodic() + * + * \li RobotPeriodic() + * \li DisabledPeriodic() + * \li AutonomousPeriodic() + * \li TeleopPeriodic() + * \li TestPeriodic() + * + * Exit() functions -- each of the following functions is called once when the + * appropriate mode is exited: + * + * \li DisabledExit() -- called each and every time disabled is exited + * \li AutonomousExit() -- called each and every time autonomous is exited + * \li TeleopExit() -- called each and every time teleop is exited + * \li TestExit() -- called each and every time test is exited */ class IterativeRobotBase : public RobotBase { public: @@ -152,6 +162,38 @@ class IterativeRobotBase : public RobotBase { */ virtual void TestPeriodic(); + /** + * Exit code for disabled mode should go here. + * + * Users should override this method for code which will be called each time + * the robot exits disabled mode. + */ + virtual void DisabledExit(); + + /** + * Exit code for autonomous mode should go here. + * + * Users should override this method for code which will be called each time + * the robot exits autonomous mode. + */ + virtual void AutonomousExit(); + + /** + * Exit code for teleop mode should go here. + * + * Users should override this method for code which will be called each time + * the robot exits teleop mode. + */ + virtual void TeleopExit(); + + /** + * Exit code for test mode should go here. + * + * Users should override this method for code which will be called each time + * the robot exits test mode. + */ + virtual void TestExit(); + /** * Enables or disables flushing NetworkTables every loop iteration. * By default, this is disabled. diff --git a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp index 575112d5f2..8bbf0b3507 100644 --- a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp +++ b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp @@ -32,6 +32,11 @@ class MockRobot : public TimedRobot { std::atomic m_teleopInitCount{0}; std::atomic m_testInitCount{0}; + std::atomic m_disabledExitCount{0}; + std::atomic m_autonomousExitCount{0}; + std::atomic m_teleopExitCount{0}; + std::atomic m_testExitCount{0}; + std::atomic m_robotPeriodicCount{0}; std::atomic m_simulationPeriodicCount{0}; std::atomic m_disabledPeriodicCount{0}; @@ -62,6 +67,14 @@ class MockRobot : public TimedRobot { void TeleopPeriodic() override { m_teleopPeriodicCount++; } void TestPeriodic() override { m_testPeriodicCount++; } + + void DisabledExit() override { m_disabledExitCount++; } + + void AutonomousExit() override { m_autonomousExitCount++; } + + void TeleopExit() override { m_teleopExitCount++; } + + void TestExit() override { m_testExitCount++; } }; } // namespace @@ -88,6 +101,11 @@ TEST_F(TimedRobotTest, Disabled) { EXPECT_EQ(0u, robot.m_teleopPeriodicCount); EXPECT_EQ(0u, robot.m_testPeriodicCount); + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + frc::sim::StepTiming(20_ms); EXPECT_EQ(1u, robot.m_robotInitCount); @@ -104,6 +122,11 @@ TEST_F(TimedRobotTest, Disabled) { EXPECT_EQ(0u, robot.m_teleopPeriodicCount); EXPECT_EQ(0u, robot.m_testPeriodicCount); + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + frc::sim::StepTiming(20_ms); EXPECT_EQ(1u, robot.m_robotInitCount); @@ -120,6 +143,11 @@ TEST_F(TimedRobotTest, Disabled) { EXPECT_EQ(0u, robot.m_teleopPeriodicCount); EXPECT_EQ(0u, robot.m_testPeriodicCount); + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + robot.EndCompetition(); robotThread.join(); } @@ -149,6 +177,11 @@ TEST_F(TimedRobotTest, Autonomous) { EXPECT_EQ(0u, robot.m_teleopPeriodicCount); EXPECT_EQ(0u, robot.m_testPeriodicCount); + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + frc::sim::StepTiming(20_ms); EXPECT_EQ(1u, robot.m_robotInitCount); @@ -165,6 +198,11 @@ TEST_F(TimedRobotTest, Autonomous) { EXPECT_EQ(0u, robot.m_teleopPeriodicCount); EXPECT_EQ(0u, robot.m_testPeriodicCount); + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + frc::sim::StepTiming(20_ms); EXPECT_EQ(1u, robot.m_robotInitCount); @@ -181,6 +219,11 @@ TEST_F(TimedRobotTest, Autonomous) { EXPECT_EQ(0u, robot.m_teleopPeriodicCount); EXPECT_EQ(0u, robot.m_testPeriodicCount); + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + robot.EndCompetition(); robotThread.join(); } @@ -210,6 +253,11 @@ TEST_F(TimedRobotTest, Teleop) { EXPECT_EQ(0u, robot.m_teleopPeriodicCount); EXPECT_EQ(0u, robot.m_testPeriodicCount); + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + frc::sim::StepTiming(20_ms); EXPECT_EQ(1u, robot.m_robotInitCount); @@ -226,6 +274,11 @@ TEST_F(TimedRobotTest, Teleop) { EXPECT_EQ(1u, robot.m_teleopPeriodicCount); EXPECT_EQ(0u, robot.m_testPeriodicCount); + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + frc::sim::StepTiming(20_ms); EXPECT_EQ(1u, robot.m_robotInitCount); @@ -242,6 +295,11 @@ TEST_F(TimedRobotTest, Teleop) { EXPECT_EQ(2u, robot.m_teleopPeriodicCount); EXPECT_EQ(0u, robot.m_testPeriodicCount); + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + robot.EndCompetition(); robotThread.join(); } @@ -271,6 +329,11 @@ TEST_F(TimedRobotTest, Test) { EXPECT_EQ(0u, robot.m_teleopPeriodicCount); EXPECT_EQ(0u, robot.m_testPeriodicCount); + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + frc::sim::StepTiming(20_ms); EXPECT_EQ(1u, robot.m_robotInitCount); @@ -287,6 +350,11 @@ TEST_F(TimedRobotTest, Test) { EXPECT_EQ(0u, robot.m_teleopPeriodicCount); EXPECT_EQ(1u, robot.m_testPeriodicCount); + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + frc::sim::StepTiming(20_ms); EXPECT_EQ(1u, robot.m_robotInitCount); @@ -303,6 +371,117 @@ TEST_F(TimedRobotTest, Test) { EXPECT_EQ(0u, robot.m_teleopPeriodicCount); EXPECT_EQ(2u, robot.m_testPeriodicCount); + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + + robot.EndCompetition(); + robotThread.join(); +} + +TEST_F(TimedRobotTest, ModeChange) { + MockRobot robot; + + std::thread robotThread{[&] { robot.StartCompetition(); }}; + + // Start in disabled + frc::sim::DriverStationSim::SetEnabled(false); + frc::sim::DriverStationSim::SetAutonomous(false); + frc::sim::DriverStationSim::SetTest(false); + frc::sim::DriverStationSim::NotifyNewData(); + frc::sim::StepTiming(0_ms); // Wait for Notifiers + + EXPECT_EQ(0u, robot.m_disabledInitCount); + EXPECT_EQ(0u, robot.m_autonomousInitCount); + EXPECT_EQ(0u, robot.m_teleopInitCount); + EXPECT_EQ(0u, robot.m_testInitCount); + + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + + frc::sim::StepTiming(20_ms); + + EXPECT_EQ(1u, robot.m_disabledInitCount); + EXPECT_EQ(0u, robot.m_autonomousInitCount); + EXPECT_EQ(0u, robot.m_teleopInitCount); + EXPECT_EQ(0u, robot.m_testInitCount); + + EXPECT_EQ(0u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + + // Transition to autonomous + frc::sim::DriverStationSim::SetEnabled(true); + frc::sim::DriverStationSim::SetAutonomous(true); + frc::sim::DriverStationSim::SetTest(false); + + frc::sim::StepTiming(20_ms); + + EXPECT_EQ(1u, robot.m_disabledInitCount); + EXPECT_EQ(1u, robot.m_autonomousInitCount); + EXPECT_EQ(0u, robot.m_teleopInitCount); + EXPECT_EQ(0u, robot.m_testInitCount); + + EXPECT_EQ(1u, robot.m_disabledExitCount); + EXPECT_EQ(0u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + + // Transition to teleop + frc::sim::DriverStationSim::SetEnabled(true); + frc::sim::DriverStationSim::SetAutonomous(false); + frc::sim::DriverStationSim::SetTest(false); + + frc::sim::StepTiming(20_ms); + + EXPECT_EQ(1u, robot.m_disabledInitCount); + EXPECT_EQ(1u, robot.m_autonomousInitCount); + EXPECT_EQ(1u, robot.m_teleopInitCount); + EXPECT_EQ(0u, robot.m_testInitCount); + + EXPECT_EQ(1u, robot.m_disabledExitCount); + EXPECT_EQ(1u, robot.m_autonomousExitCount); + EXPECT_EQ(0u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + + // Transition to test + frc::sim::DriverStationSim::SetEnabled(true); + frc::sim::DriverStationSim::SetAutonomous(false); + frc::sim::DriverStationSim::SetTest(true); + + frc::sim::StepTiming(20_ms); + + EXPECT_EQ(1u, robot.m_disabledInitCount); + EXPECT_EQ(1u, robot.m_autonomousInitCount); + EXPECT_EQ(1u, robot.m_teleopInitCount); + EXPECT_EQ(1u, robot.m_testInitCount); + + EXPECT_EQ(1u, robot.m_disabledExitCount); + EXPECT_EQ(1u, robot.m_autonomousExitCount); + EXPECT_EQ(1u, robot.m_teleopExitCount); + EXPECT_EQ(0u, robot.m_testExitCount); + + // Transition to disabled + frc::sim::DriverStationSim::SetEnabled(false); + frc::sim::DriverStationSim::SetAutonomous(false); + frc::sim::DriverStationSim::SetTest(false); + + frc::sim::StepTiming(20_ms); + + EXPECT_EQ(2u, robot.m_disabledInitCount); + EXPECT_EQ(1u, robot.m_autonomousInitCount); + EXPECT_EQ(1u, robot.m_teleopInitCount); + EXPECT_EQ(1u, robot.m_testInitCount); + + EXPECT_EQ(1u, robot.m_disabledExitCount); + EXPECT_EQ(1u, robot.m_autonomousExitCount); + EXPECT_EQ(1u, robot.m_teleopExitCount); + EXPECT_EQ(1u, robot.m_testExitCount); + robot.EndCompetition(); robotThread.join(); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java index ca99053e87..1fd1ddd3ed 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java @@ -23,13 +23,34 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; *

robotInit() -- provide for initialization at robot power-on * *

init() functions -- each of the following functions is called once when the appropriate mode - * is entered: - disabledInit() -- called each and every time disabled is entered from another mode - * - autonomousInit() -- called each and every time autonomous is entered from another mode - - * teleopInit() -- called each and every time teleop is entered from another mode - testInit() -- - * called each and every time test is entered from another mode + * is entered: * - *

periodic() functions -- each of these functions is called on an interval: - robotPeriodic() - - * disabledPeriodic() - autonomousPeriodic() - teleopPeriodic() - testPeriodic() + *

    + *
  • disabledInit() -- called each and every time disabled is entered from another mode + *
  • autonomousInit() -- called each and every time autonomous is entered from another mode + *
  • teleopInit() -- called each and every time teleop is entered from another mode + *
  • testInit() -- called each and every time test is entered from another mode + *
+ * + *

periodic() functions -- each of these functions is called on an interval: + * + *

    + *
  • robotPeriodic() + *
  • disabledPeriodic() + *
  • autonomousPeriodic() + *
  • teleopPeriodic() + *
  • testPeriodic() + *
+ * + *

final() functions -- each of the following functions is called once when the appropriate mode + * is exited: + * + *

    + *
  • disabledExit() -- called each and every time disabled is exited + *
  • autonomousExit() -- called each and every time autonomous is exited + *
  • teleopExit() -- called each and every time teleop is exited + *
  • testExit() -- called each and every time test is exited + *
*/ public abstract class IterativeRobotBase extends RobotBase { private enum Mode { @@ -194,6 +215,39 @@ public abstract class IterativeRobotBase extends RobotBase { } } + /** + * Exit code for disabled mode should go here. + * + *

Users should override this method for code which will be called each time the robot exits + * disabled mode. + */ + public void disabledExit() {} + + /** + * Exit code for autonomous mode should go here. + * + *

Users should override this method for code which will be called each time the robot exits + * autonomous mode. + */ + public void autonomousExit() {} + + /** + * Exit code for teleop mode should go here. + * + *

Users should override this method for code which will be called each time the robot exits + * teleop mode. + */ + public void teleopExit() {} + + /** + * Exit code for test mode should go here. + * + *

Users should override this method for code which will be called each time the robot exits + * test mode. + */ + @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation") + public void testExit() {} + /** * Enables or disables flushing NetworkTables every loop iteration. By default, this is disabled. * @@ -215,60 +269,67 @@ public abstract class IterativeRobotBase extends RobotBase { protected void loopFunc() { m_watchdog.reset(); - // Call the appropriate function depending upon the current robot mode + // Get current mode + Mode mode = Mode.kNone; if (isDisabled()) { - // Call DisabledInit() if we are now just entering disabled mode from either a different mode - // or from power-on. - if (m_lastMode != Mode.kDisabled) { + mode = Mode.kDisabled; + } else if (isAutonomous()) { + mode = Mode.kAutonomous; + } else if (isOperatorControl()) { + mode = Mode.kTeleop; + } else if (isTest()) { + mode = Mode.kTest; + } + + // If mode changed, call mode exit and entry functions + if (m_lastMode != mode) { + // Call last mode's exit function + if (m_lastMode == Mode.kDisabled) { + disabledExit(); + } else if (m_lastMode == Mode.kAutonomous) { + autonomousExit(); + } else if (m_lastMode == Mode.kTeleop) { + teleopExit(); + } else if (m_lastMode == Mode.kTest) { LiveWindow.setEnabled(false); Shuffleboard.disableActuatorWidgets(); + testExit(); + } + + // Call current mode's entry function + if (mode == Mode.kDisabled) { disabledInit(); m_watchdog.addEpoch("disabledInit()"); - m_lastMode = Mode.kDisabled; - } - - HAL.observeUserProgramDisabled(); - disabledPeriodic(); - m_watchdog.addEpoch("disablePeriodic()"); - } else if (isAutonomous()) { - // Call AutonomousInit() if we are now just entering autonomous mode from either a different - // mode or from power-on. - if (m_lastMode != Mode.kAutonomous) { - LiveWindow.setEnabled(false); - Shuffleboard.disableActuatorWidgets(); + } else if (mode == Mode.kAutonomous) { autonomousInit(); m_watchdog.addEpoch("autonomousInit()"); - m_lastMode = Mode.kAutonomous; - } - - HAL.observeUserProgramAutonomous(); - autonomousPeriodic(); - m_watchdog.addEpoch("autonomousPeriodic()"); - } else if (isOperatorControl()) { - // Call TeleopInit() if we are now just entering teleop mode from either a different mode or - // from power-on. - if (m_lastMode != Mode.kTeleop) { - LiveWindow.setEnabled(false); - Shuffleboard.disableActuatorWidgets(); + } else if (mode == Mode.kTeleop) { teleopInit(); m_watchdog.addEpoch("teleopInit()"); - m_lastMode = Mode.kTeleop; - } - - HAL.observeUserProgramTeleop(); - teleopPeriodic(); - m_watchdog.addEpoch("teleopPeriodic()"); - } else { - // Call TestInit() if we are now just entering test mode from either a different mode or from - // power-on. - if (m_lastMode != Mode.kTest) { + } else if (mode == Mode.kTest) { LiveWindow.setEnabled(true); Shuffleboard.enableActuatorWidgets(); testInit(); m_watchdog.addEpoch("testInit()"); - m_lastMode = Mode.kTest; } + m_lastMode = mode; + } + + // Call the appropriate function depending upon the current robot mode + if (mode == Mode.kDisabled) { + HAL.observeUserProgramDisabled(); + disabledPeriodic(); + m_watchdog.addEpoch("disabledPeriodic()"); + } else if (mode == Mode.kAutonomous) { + HAL.observeUserProgramAutonomous(); + autonomousPeriodic(); + m_watchdog.addEpoch("autonomousPeriodic()"); + } else if (mode == Mode.kTeleop) { + HAL.observeUserProgramTeleop(); + teleopPeriodic(); + m_watchdog.addEpoch("teleopPeriodic()"); + } else { HAL.observeUserProgramTest(); testPeriodic(); m_watchdog.addEpoch("testPeriodic()"); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java index cdb088d587..9ca47ad700 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java @@ -30,6 +30,11 @@ class TimedRobotTest { public final AtomicInteger m_teleopPeriodicCount = new AtomicInteger(0); public final AtomicInteger m_testPeriodicCount = new AtomicInteger(0); + public final AtomicInteger m_disabledExitCount = new AtomicInteger(0); + public final AtomicInteger m_autonomousExitCount = new AtomicInteger(0); + public final AtomicInteger m_teleopExitCount = new AtomicInteger(0); + public final AtomicInteger m_testExitCount = new AtomicInteger(0); + @Override public void robotInit() { m_robotInitCount.addAndGet(1); @@ -89,6 +94,26 @@ class TimedRobotTest { public void testPeriodic() { m_testPeriodicCount.addAndGet(1); } + + @Override + public void disabledExit() { + m_disabledExitCount.addAndGet(1); + } + + @Override + public void autonomousExit() { + m_autonomousExitCount.addAndGet(1); + } + + @Override + public void teleopExit() { + m_teleopExitCount.addAndGet(1); + } + + @Override + public void testExit() { + m_testExitCount.addAndGet(1); + } } @BeforeEach @@ -131,6 +156,11 @@ class TimedRobotTest { assertEquals(0, robot.m_teleopPeriodicCount.get()); assertEquals(0, robot.m_testPeriodicCount.get()); + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + SimHooks.stepTiming(0.02); assertEquals(1, robot.m_robotInitCount.get()); @@ -147,6 +177,11 @@ class TimedRobotTest { assertEquals(0, robot.m_teleopPeriodicCount.get()); assertEquals(0, robot.m_testPeriodicCount.get()); + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + SimHooks.stepTiming(0.02); assertEquals(1, robot.m_robotInitCount.get()); @@ -163,6 +198,11 @@ class TimedRobotTest { assertEquals(0, robot.m_teleopPeriodicCount.get()); assertEquals(0, robot.m_testPeriodicCount.get()); + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + robot.endCompetition(); try { robotThread.interrupt(); @@ -205,6 +245,11 @@ class TimedRobotTest { assertEquals(0, robot.m_teleopPeriodicCount.get()); assertEquals(0, robot.m_testPeriodicCount.get()); + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + SimHooks.stepTiming(0.02); assertEquals(1, robot.m_robotInitCount.get()); @@ -221,6 +266,11 @@ class TimedRobotTest { assertEquals(0, robot.m_teleopPeriodicCount.get()); assertEquals(0, robot.m_testPeriodicCount.get()); + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + SimHooks.stepTiming(0.02); assertEquals(1, robot.m_robotInitCount.get()); @@ -237,6 +287,11 @@ class TimedRobotTest { assertEquals(0, robot.m_teleopPeriodicCount.get()); assertEquals(0, robot.m_testPeriodicCount.get()); + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + robot.endCompetition(); try { robotThread.interrupt(); @@ -279,6 +334,11 @@ class TimedRobotTest { assertEquals(0, robot.m_teleopPeriodicCount.get()); assertEquals(0, robot.m_testPeriodicCount.get()); + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + SimHooks.stepTiming(0.02); assertEquals(1, robot.m_robotInitCount.get()); @@ -295,6 +355,11 @@ class TimedRobotTest { assertEquals(1, robot.m_teleopPeriodicCount.get()); assertEquals(0, robot.m_testPeriodicCount.get()); + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + SimHooks.stepTiming(0.02); assertEquals(1, robot.m_robotInitCount.get()); @@ -311,6 +376,11 @@ class TimedRobotTest { assertEquals(2, robot.m_teleopPeriodicCount.get()); assertEquals(0, robot.m_testPeriodicCount.get()); + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + robot.endCompetition(); try { robotThread.interrupt(); @@ -353,6 +423,11 @@ class TimedRobotTest { assertEquals(0, robot.m_teleopPeriodicCount.get()); assertEquals(0, robot.m_testPeriodicCount.get()); + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + SimHooks.stepTiming(0.02); assertEquals(1, robot.m_robotInitCount.get()); @@ -369,6 +444,11 @@ class TimedRobotTest { assertEquals(0, robot.m_teleopPeriodicCount.get()); assertEquals(1, robot.m_testPeriodicCount.get()); + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + SimHooks.stepTiming(0.02); assertEquals(1, robot.m_robotInitCount.get()); @@ -385,6 +465,134 @@ class TimedRobotTest { assertEquals(0, robot.m_teleopPeriodicCount.get()); assertEquals(2, robot.m_testPeriodicCount.get()); + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + + robot.endCompetition(); + try { + robotThread.interrupt(); + robotThread.join(); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + robot.close(); + } + + @Test + @ResourceLock("timing") + void modeChangeTest() { + MockRobot robot = new MockRobot(); + + Thread robotThread = + new Thread( + () -> { + robot.startCompetition(); + }); + robotThread.start(); + + // Start in disabled + DriverStationSim.setEnabled(false); + DriverStationSim.setAutonomous(false); + DriverStationSim.setTest(false); + DriverStationSim.notifyNewData(); + SimHooks.stepTiming(0.0); // Wait for Notifiers + + assertEquals(0, robot.m_disabledInitCount.get()); + assertEquals(0, robot.m_autonomousInitCount.get()); + assertEquals(0, robot.m_teleopInitCount.get()); + assertEquals(0, robot.m_testInitCount.get()); + + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + + SimHooks.stepTiming(0.02); + + assertEquals(1, robot.m_disabledInitCount.get()); + assertEquals(0, robot.m_autonomousInitCount.get()); + assertEquals(0, robot.m_teleopInitCount.get()); + assertEquals(0, robot.m_testInitCount.get()); + + assertEquals(0, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + + // Transition to autonomous + DriverStationSim.setEnabled(true); + DriverStationSim.setAutonomous(true); + DriverStationSim.setTest(false); + DriverStationSim.notifyNewData(); + + SimHooks.stepTiming(0.02); + + assertEquals(1, robot.m_disabledInitCount.get()); + assertEquals(1, robot.m_autonomousInitCount.get()); + assertEquals(0, robot.m_teleopInitCount.get()); + assertEquals(0, robot.m_testInitCount.get()); + + assertEquals(1, robot.m_disabledExitCount.get()); + assertEquals(0, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + + // Transition to teleop + DriverStationSim.setEnabled(true); + DriverStationSim.setAutonomous(false); + DriverStationSim.setTest(false); + DriverStationSim.notifyNewData(); + + SimHooks.stepTiming(0.02); + + assertEquals(1, robot.m_disabledInitCount.get()); + assertEquals(1, robot.m_autonomousInitCount.get()); + assertEquals(1, robot.m_teleopInitCount.get()); + assertEquals(0, robot.m_testInitCount.get()); + + assertEquals(1, robot.m_disabledExitCount.get()); + assertEquals(1, robot.m_autonomousExitCount.get()); + assertEquals(0, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + + // Transition to test + DriverStationSim.setEnabled(true); + DriverStationSim.setAutonomous(false); + DriverStationSim.setTest(true); + DriverStationSim.notifyNewData(); + + SimHooks.stepTiming(0.02); + + assertEquals(1, robot.m_disabledInitCount.get()); + assertEquals(1, robot.m_autonomousInitCount.get()); + assertEquals(1, robot.m_teleopInitCount.get()); + assertEquals(1, robot.m_testInitCount.get()); + + assertEquals(1, robot.m_disabledExitCount.get()); + assertEquals(1, robot.m_autonomousExitCount.get()); + assertEquals(1, robot.m_teleopExitCount.get()); + assertEquals(0, robot.m_testExitCount.get()); + + // Transition to disabled + DriverStationSim.setEnabled(false); + DriverStationSim.setAutonomous(false); + DriverStationSim.setTest(false); + DriverStationSim.notifyNewData(); + + SimHooks.stepTiming(0.02); + + assertEquals(2, robot.m_disabledInitCount.get()); + assertEquals(1, robot.m_autonomousInitCount.get()); + assertEquals(1, robot.m_teleopInitCount.get()); + assertEquals(1, robot.m_testInitCount.get()); + + assertEquals(1, robot.m_disabledExitCount.get()); + assertEquals(1, robot.m_autonomousExitCount.get()); + assertEquals(1, robot.m_teleopExitCount.get()); + assertEquals(1, robot.m_testExitCount.get()); + robot.endCompetition(); try { robotThread.interrupt();