From fb2ee8ec349a770aefac87b561f2c8019b830ebd Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Thu, 5 Aug 2021 19:08:29 -0700 Subject: [PATCH] [wpilib] Add TimedRobot functions for running code on mode exit (#3499) Currently, we have functions like TeleopInit() for running code on mode entry, but no such functions for running code on mode exit, and it's cumbersome to add those in user code without making a custom robot class. This PR adds exit functions to TimedRobot. Some example use cases include DisabledExit() for operations when the robot enables (whether that be into teleop, autonomous, or test) and AutonomousExit() for disabling feedback controllers. --- .../main/native/cpp/IterativeRobotBase.cpp | 93 ++++---- .../native/include/frc/IterativeRobotBase.h | 70 ++++-- .../src/test/native/cpp/TimedRobotTest.cpp | 179 +++++++++++++++ .../wpi/first/wpilibj/IterativeRobotBase.java | 151 +++++++++---- .../edu/wpi/first/wpilibj/TimedRobotTest.java | 208 ++++++++++++++++++ 5 files changed, 603 insertions(+), 98 deletions(-) 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();