diff --git a/wpilibc/athena/include/IterativeRobot.h b/wpilibc/athena/include/IterativeRobot.h index e10dd2bbea..15d4a76c2d 100644 --- a/wpilibc/athena/include/IterativeRobot.h +++ b/wpilibc/athena/include/IterativeRobot.h @@ -7,67 +7,25 @@ #pragma once -#include "RobotBase.h" +#include "IterativeRobotBase.h" namespace frc { /** - * IterativeRobot implements a specific type of Robot Program framework, - * extending the RobotBase class. + * IterativeRobot implements the IterativeRobotBase robot program framework. * * The IterativeRobot class is intended to be subclassed by a user creating a * robot program. * - * This class is intended to implement the "old style" default code, by - * providing the following functions which are called by the main loop, - * StartCompetition(), at the appropriate times: - * - * 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 only when first disabled - * - 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 each time a - * new packet is received from the driver station: - * - RobotPeriodic() - * - DisabledPeriodic() - * - AutonomousPeriodic() - * - TeleopPeriodic() - * - TestPeriodic() - * + * Periodic() functions from the base class are called each time a new packet is + * received from the driver station. */ -class IterativeRobot : public RobotBase { +class IterativeRobot : public IterativeRobotBase { public: - void StartCompetition() override; - - virtual void RobotInit(); - virtual void DisabledInit(); - virtual void AutonomousInit(); - virtual void TeleopInit(); - virtual void TestInit(); - - virtual void RobotPeriodic(); - virtual void DisabledPeriodic(); - virtual void AutonomousPeriodic(); - virtual void TeleopPeriodic(); - virtual void TestPeriodic(); - - protected: - IterativeRobot() = default; + IterativeRobot(); virtual ~IterativeRobot() = default; - private: - bool m_disabledInitialized = false; - bool m_autonomousInitialized = false; - bool m_teleopInitialized = false; - bool m_testInitialized = false; + void StartCompetition() override; }; } // namespace frc diff --git a/wpilibc/athena/include/IterativeRobotBase.h b/wpilibc/athena/include/IterativeRobotBase.h new file mode 100644 index 0000000000..4d83bb6e67 --- /dev/null +++ b/wpilibc/athena/include/IterativeRobotBase.h @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "RobotBase.h" + +namespace frc { + +/** + * IterativeRobotBase implements a specific type of robot program framework, + * extending the RobotBase class. + * + * The IterativeRobotBase class does not implement StartCompetition(), so it + * should not be used by teams directly. + * + * This class provides the following functions which are called by the main + * loop, StartCompetition(), at the appropriate times: + * + * 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 only when first disabled + * - 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() + */ +class IterativeRobotBase : public RobotBase { + public: + virtual void RobotInit(); + virtual void DisabledInit(); + virtual void AutonomousInit(); + virtual void TeleopInit(); + virtual void TestInit(); + + virtual void RobotPeriodic(); + virtual void DisabledPeriodic(); + virtual void AutonomousPeriodic(); + virtual void TeleopPeriodic(); + virtual void TestPeriodic(); + + protected: + IterativeRobotBase(); + virtual ~IterativeRobotBase() = default; + + void LoopFunc(); + + private: + enum class Mode { kNone, kDisabled, kAutonomous, kTeleop, kTest }; + + Mode m_lastMode = Mode::kNone; +}; + +} // namespace frc diff --git a/wpilibc/athena/include/RobotBase.h b/wpilibc/athena/include/RobotBase.h index fe633073a7..dd613e78a5 100644 --- a/wpilibc/athena/include/RobotBase.h +++ b/wpilibc/athena/include/RobotBase.h @@ -25,8 +25,8 @@ class DriverStation; } \ HAL_Report(HALUsageReporting::kResourceType_Language, \ HALUsageReporting::kLanguage_CPlusPlus); \ - static _ClassName_ robot; \ llvm::outs() << "\n********** Robot program starting **********\n"; \ + static _ClassName_ robot; \ robot.StartCompetition(); \ } diff --git a/wpilibc/athena/include/SampleRobot.h b/wpilibc/athena/include/SampleRobot.h index 372221e72c..ed54fa1430 100644 --- a/wpilibc/athena/include/SampleRobot.h +++ b/wpilibc/athena/include/SampleRobot.h @@ -27,7 +27,7 @@ class SampleRobot : public RobotBase { virtual ~SampleRobot() = default; private: - bool m_robotMainOverridden; + bool m_robotMainOverridden = true; }; } // namespace frc diff --git a/wpilibc/athena/include/TimedRobot.h b/wpilibc/athena/include/TimedRobot.h new file mode 100644 index 0000000000..296f1bd288 --- /dev/null +++ b/wpilibc/athena/include/TimedRobot.h @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "IterativeRobotBase.h" +#include "Notifier.h" + +namespace frc { + +/** + * TimedRobot implements the IterativeRobotBase robot program framework. + * + * The TimedRobot class is intended to be subclassed by a user creating a + * robot program. + * + * Periodic() functions from the base class are called on an interval by a + * Notifier instance. + */ +class TimedRobot : public IterativeRobotBase { + public: + static constexpr double kDefaultPeriod = 0.02; + + void StartCompetition() override; + + void SetPeriod(double seconds); + + protected: + TimedRobot(); + virtual ~TimedRobot(); + + private: + double m_period = kDefaultPeriod; + + // Prevents loop from starting if user calls SetPeriod() in RobotInit() + bool m_startLoop = false; + + std::unique_ptr m_loop; +}; + +} // namespace frc diff --git a/wpilibc/athena/include/WPILib.h b/wpilibc/athena/include/WPILib.h index 5fe5c74de7..a56f238626 100644 --- a/wpilibc/athena/include/WPILib.h +++ b/wpilibc/athena/include/WPILib.h @@ -77,6 +77,7 @@ #include "Talon.h" #include "TalonSRX.h" #include "Threads.h" +#include "TimedRobot.h" #include "Timer.h" #include "Ultrasonic.h" #include "Utility.h" diff --git a/wpilibc/athena/src/IterativeRobot.cpp b/wpilibc/athena/src/IterativeRobot.cpp index c4f216106a..bdb89f52e4 100644 --- a/wpilibc/athena/src/IterativeRobot.cpp +++ b/wpilibc/athena/src/IterativeRobot.cpp @@ -9,12 +9,14 @@ #include "DriverStation.h" #include "HAL/HAL.h" -#include "LiveWindow/LiveWindow.h" -#include "llvm/raw_ostream.h" -#include "networktables/NetworkTable.h" using namespace frc; +IterativeRobot::IterativeRobot() { + HAL_Report(HALUsageReporting::kResourceType_Framework, + HALUsageReporting::kFramework_Iterative); +} + /** * Provide an alternate "main loop" via StartCompetition(). * @@ -22,244 +24,11 @@ using namespace frc; * the DS packets. */ void IterativeRobot::StartCompetition() { - HAL_Report(HALUsageReporting::kResourceType_Framework, - HALUsageReporting::kFramework_Iterative); - - LiveWindow* lw = LiveWindow::GetInstance(); - // first and one-time initialization - NetworkTable::GetTable("LiveWindow") - ->GetSubTable("~STATUS~") - ->PutBoolean("LW Enabled", false); - RobotInit(); - - // Tell the DS that the robot is ready to be enabled - HAL_ObserveUserProgramStarting(); - - // loop forever, calling the appropriate mode-dependent function - lw->SetEnabled(false); + // Loop forever, calling the appropriate mode-dependent function while (true) { // wait for driver station data so the loop doesn't hog the CPU - m_ds.WaitForData(); - // Call the appropriate function depending upon the current robot mode - if (IsDisabled()) { - // call DisabledInit() if we are now just entering disabled mode from - // either a different mode or from power-on - if (!m_disabledInitialized) { - lw->SetEnabled(false); - DisabledInit(); - m_disabledInitialized = true; - // reset the initialization flags for the other modes - m_autonomousInitialized = false; - m_teleopInitialized = false; - m_testInitialized = false; - } - HAL_ObserveUserProgramDisabled(); - 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_autonomousInitialized) { - lw->SetEnabled(false); - AutonomousInit(); - m_autonomousInitialized = true; - // reset the initialization flags for the other modes - m_disabledInitialized = false; - m_teleopInitialized = false; - m_testInitialized = false; - } - HAL_ObserveUserProgramAutonomous(); - AutonomousPeriodic(); - } else if (IsTest()) { - // call TestInit() if we are now just entering test mode from - // either a different mode or from power-on - if (!m_testInitialized) { - lw->SetEnabled(true); - TestInit(); - m_testInitialized = true; - // reset the initialization flags for the other modes - m_disabledInitialized = false; - m_autonomousInitialized = false; - m_teleopInitialized = false; - } - HAL_ObserveUserProgramTest(); - TestPeriodic(); - } else { - // call TeleopInit() if we are now just entering teleop mode from - // either a different mode or from power-on - if (!m_teleopInitialized) { - lw->SetEnabled(false); - TeleopInit(); - m_teleopInitialized = true; - // reset the initialization flags for the other modes - m_disabledInitialized = false; - m_autonomousInitialized = false; - m_testInitialized = false; - Scheduler::GetInstance()->SetEnabled(true); - } - HAL_ObserveUserProgramTeleop(); - TeleopPeriodic(); - } - RobotPeriodic(); - } -} - -/** - * Robot-wide initialization code should go here. - * - * Users should override this method for default Robot-wide initialization which - * will be called when the robot is first powered on. It will be called exactly - * one time. - * - * Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" - * indicators will be off until RobotInit() exits. Code in RobotInit() that - * waits for enable will cause the robot to never indicate that the code is - * ready, causing the robot to be bypassed in a match. - */ -void IterativeRobot::RobotInit() { - llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; -} - -/** - * Initialization code for disabled mode should go here. - * - * Users should override this method for initialization code which will be - * called each time - * the robot enters disabled mode. - */ -void IterativeRobot::DisabledInit() { - llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; -} - -/** - * Initialization code for autonomous mode should go here. - * - * Users should override this method for initialization code which will be - * called each time the robot enters autonomous mode. - */ -void IterativeRobot::AutonomousInit() { - llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; -} - -/** - * Initialization code for teleop mode should go here. - * - * Users should override this method for initialization code which will be - * called each time the robot enters teleop mode. - */ -void IterativeRobot::TeleopInit() { - llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; -} - -/** - * Initialization code for test mode should go here. - * - * Users should override this method for initialization code which will be - * called each time the robot enters test mode. - */ -void IterativeRobot::TestInit() { - llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; -} - -/** - * Periodic code for all modes should go here. - * - * This function is called each time a new packet is received from the driver - * station. - * - * Packets are received approximately every 20ms. Fixed loop timing is not - * guaranteed due to network timing variability and the function may not be - * called at all if the Driver Station is disconnected. For most use cases the - * variable timing will not be an issue. If your code does require guaranteed - * fixed periodic timing, consider using Notifier or PIDController instead. - */ -void IterativeRobot::RobotPeriodic() { - static bool firstRun = true; - if (firstRun) { - llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; - firstRun = false; - } -} - -/** - * Periodic code for disabled mode should go here. - * - * Users should override this method for code which will be called each time a - * new packet is received from the driver station and the robot is in disabled - * mode. - * - * Packets are received approximately every 20ms. Fixed loop timing is not - * guaranteed due to network timing variability and the function may not be - * called at all if the Driver Station is disconnected. For most use cases the - * variable timing will not be an issue. If your code does require guaranteed - * fixed periodic timing, consider using Notifier or PIDController instead. - */ -void IterativeRobot::DisabledPeriodic() { - static bool firstRun = true; - if (firstRun) { - llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; - firstRun = false; - } -} - -/** - * Periodic code for autonomous mode should go here. - * - * Users should override this method for code which will be called each time a - * new packet is received from the driver station and the robot is in autonomous - * mode. - * - * Packets are received approximately every 20ms. Fixed loop timing is not - * guaranteed due to network timing variability and the function may not be - * called at all if the Driver Station is disconnected. For most use cases the - * variable timing will not be an issue. If your code does require guaranteed - * fixed periodic timing, consider using Notifier or PIDController instead. - */ -void IterativeRobot::AutonomousPeriodic() { - static bool firstRun = true; - if (firstRun) { - llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; - firstRun = false; - } -} - -/** - * Periodic code for teleop mode should go here. - * - * Users should override this method for code which will be called each time a - * new packet is received from the driver station and the robot is in teleop - * mode. - * - * Packets are received approximately every 20ms. Fixed loop timing is not - * guaranteed due to network timing variability and the function may not be - * called at all if the Driver Station is disconnected. For most use cases the - * variable timing will not be an issue. If your code does require guaranteed - * fixed periodic timing, consider using Notifier or PIDController instead. - */ -void IterativeRobot::TeleopPeriodic() { - static bool firstRun = true; - if (firstRun) { - llvm::outs() << "Default " << __FUNCTION__ - << "()v method... Overload me!\n"; - firstRun = false; - } -} - -/** - * Periodic code for test mode should go here. - * - * Users should override this method for code which will be called each time a - * new packet is received from the driver station and the robot is in test mode. - * - * Packets are received approximately every 20ms. Fixed loop timing is not - * guaranteed due to network timing variability and the function may not be - * called at all if the Driver Station is disconnected. For most use cases the - * variable timing will not be an issue. If your code does require guaranteed - * fixed periodic timing, consider using Notifier or PIDController instead. - */ -void IterativeRobot::TestPeriodic() { - static bool firstRun = true; - if (firstRun) { - llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; - firstRun = false; + DriverStation::GetInstance().WaitForData(); + + LoopFunc(); } } diff --git a/wpilibc/athena/src/IterativeRobotBase.cpp b/wpilibc/athena/src/IterativeRobotBase.cpp new file mode 100644 index 0000000000..be8ee75364 --- /dev/null +++ b/wpilibc/athena/src/IterativeRobotBase.cpp @@ -0,0 +1,200 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "IterativeRobotBase.h" + +#include + +#include "HAL/HAL.h" +#include "LiveWindow/LiveWindow.h" +#include "llvm/raw_ostream.h" + +using namespace frc; + +/** + * Robot-wide initialization code should go here. + * + * Users should override this method for default Robot-wide initialization which + * will be called when the robot is first powered on. It will be called exactly + * one time. + * + * Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" + * indicators will be off until RobotInit() exits. Code in RobotInit() that + * waits for enable will cause the robot to never indicate that the code is + * ready, causing the robot to be bypassed in a match. + */ +void IterativeRobotBase::RobotInit() { + llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; +} + +/** + * Initialization code for disabled mode should go here. + * + * Users should override this method for initialization code which will be + * called each time + * the robot enters disabled mode. + */ +void IterativeRobotBase::DisabledInit() { + llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; +} + +/** + * Initialization code for autonomous mode should go here. + * + * Users should override this method for initialization code which will be + * called each time the robot enters autonomous mode. + */ +void IterativeRobotBase::AutonomousInit() { + llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; +} + +/** + * Initialization code for teleop mode should go here. + * + * Users should override this method for initialization code which will be + * called each time the robot enters teleop mode. + */ +void IterativeRobotBase::TeleopInit() { + llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; +} + +/** + * Initialization code for test mode should go here. + * + * Users should override this method for initialization code which will be + * called each time the robot enters test mode. + */ +void IterativeRobotBase::TestInit() { + llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; +} + +/** + * Periodic code for all modes should go here. + * + * This function is called each time a new packet is received from the driver + * station. + */ +void IterativeRobotBase::RobotPeriodic() { + static bool firstRun = true; + if (firstRun) { + llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; + firstRun = false; + } +} + +/** + * Periodic code for disabled mode should go here. + * + * Users should override this method for code which will be called each time a + * new packet is received from the driver station and the robot is in disabled + * mode. + */ +void IterativeRobotBase::DisabledPeriodic() { + static bool firstRun = true; + if (firstRun) { + llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; + firstRun = false; + } +} + +/** + * Periodic code for autonomous mode should go here. + * + * Users should override this method for code which will be called each time a + * new packet is received from the driver station and the robot is in autonomous + * mode. + */ +void IterativeRobotBase::AutonomousPeriodic() { + static bool firstRun = true; + if (firstRun) { + llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; + firstRun = false; + } +} + +/** + * Periodic code for teleop mode should go here. + * + * Users should override this method for code which will be called each time a + * new packet is received from the driver station and the robot is in teleop + * mode. + */ +void IterativeRobotBase::TeleopPeriodic() { + static bool firstRun = true; + if (firstRun) { + llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; + firstRun = false; + } +} + +/** + * Periodic code for test mode should go here. + * + * Users should override this method for code which will be called each time a + * new packet is received from the driver station and the robot is in test mode. + */ +void IterativeRobotBase::TestPeriodic() { + static bool firstRun = true; + if (firstRun) { + llvm::outs() << "Default " << __FUNCTION__ << "() method... Overload me!\n"; + firstRun = false; + } +} + +IterativeRobotBase::IterativeRobotBase() { + RobotInit(); + + // Tell the DS that the robot is ready to be enabled + HAL_ObserveUserProgramStarting(); +} + +void IterativeRobotBase::LoopFunc() { + // Call the appropriate function depending upon the current robot mode + 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) { + LiveWindow::GetInstance()->SetEnabled(false); + DisabledInit(); + m_lastMode = Mode::kDisabled; + } + HAL_ObserveUserProgramDisabled(); + 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::GetInstance()->SetEnabled(false); + AutonomousInit(); + m_lastMode = Mode::kAutonomous; + } + HAL_ObserveUserProgramAutonomous(); + 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::GetInstance()->SetEnabled(false); + TeleopInit(); + m_lastMode = Mode::kTeleop; + Scheduler::GetInstance()->SetEnabled(true); + } + HAL_ObserveUserProgramTeleop(); + 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) { + LiveWindow::GetInstance()->SetEnabled(true); + TestInit(); + m_lastMode = Mode::kTest; + } + HAL_ObserveUserProgramTest(); + TestPeriodic(); + } + RobotPeriodic(); +} diff --git a/wpilibc/athena/src/RobotBase.cpp b/wpilibc/athena/src/RobotBase.cpp index e242d03a18..84921e1f7f 100644 --- a/wpilibc/athena/src/RobotBase.cpp +++ b/wpilibc/athena/src/RobotBase.cpp @@ -13,6 +13,7 @@ #include "HAL/HAL.h" #include "HLUsageReporting.h" #include "Internal/HardwareHLReporting.h" +#include "LiveWindow/LiveWindow.h" #include "RobotState.h" #include "SmartDashboard/SmartDashboard.h" #include "Utility.h" @@ -53,6 +54,13 @@ RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) { std::fputs(WPILibVersion, file); std::fclose(file); } + + // First and one-time initialization + NetworkTable::GetTable("LiveWindow") + ->GetSubTable("~STATUS~") + ->PutBoolean("LW Enabled", false); + + LiveWindow::GetInstance()->SetEnabled(false); } /** diff --git a/wpilibc/athena/src/SampleRobot.cpp b/wpilibc/athena/src/SampleRobot.cpp index a7daacf783..90b458c737 100644 --- a/wpilibc/athena/src/SampleRobot.cpp +++ b/wpilibc/athena/src/SampleRobot.cpp @@ -28,13 +28,6 @@ using namespace frc; void SampleRobot::StartCompetition() { LiveWindow* lw = LiveWindow::GetInstance(); - HAL_Report(HALUsageReporting::kResourceType_Framework, - HALUsageReporting::kFramework_Simple); - - NetworkTable::GetTable("LiveWindow") - ->GetSubTable("~STATUS~") - ->PutBoolean("LW Enabled", false); - RobotInit(); // Tell the DS that the robot is ready to be enabled @@ -43,9 +36,6 @@ void SampleRobot::StartCompetition() { RobotMain(); if (!m_robotMainOverridden) { - // first and one-time initialization - lw->SetEnabled(false); - while (true) { if (IsDisabled()) { m_ds.InDisabled(true); @@ -147,4 +137,7 @@ void SampleRobot::Test() { */ void SampleRobot::RobotMain() { m_robotMainOverridden = false; } -SampleRobot::SampleRobot() : m_robotMainOverridden(true) {} +SampleRobot::SampleRobot() { + HAL_Report(HALUsageReporting::kResourceType_Framework, + HALUsageReporting::kFramework_Simple); +} diff --git a/wpilibc/athena/src/TimedRobot.cpp b/wpilibc/athena/src/TimedRobot.cpp new file mode 100644 index 0000000000..96e237d74b --- /dev/null +++ b/wpilibc/athena/src/TimedRobot.cpp @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "TimedRobot.h" + +#include + +#include "HAL/HAL.h" + +using namespace frc; +using namespace std::chrono_literals; + +/** + * Provide an alternate "main loop" via StartCompetition(). + */ +void TimedRobot::StartCompetition() { + // Loop forever, calling the appropriate mode-dependent function + m_startLoop = true; + m_loop->StartPeriodic(m_period); + while (true) { + std::this_thread::sleep_for(24h); + } +} + +/** + * Set time period between calls to Periodic() functions. + * + * A timer event is queued for periodic event notification. Each time the + * interrupt occurs, the event will be immediately requeued for the same time + * interval. + * + * @param period Period in seconds. + */ +void TimedRobot::SetPeriod(double period) { + m_period = period; + + if (m_startLoop) { + m_loop->StartPeriodic(m_period); + } +} + +TimedRobot::TimedRobot() { + m_loop = std::make_unique(&TimedRobot::LoopFunc, this); + + // HAL_Report(HALUsageReporting::kResourceType_Framework, + // HALUsageReporting::kFramework_Periodic); + HAL_Report(HALUsageReporting::kResourceType_Framework, + HALUsageReporting::kFramework_Iterative); +} + +TimedRobot::~TimedRobot() { m_loop->Stop(); } diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/IterativeRobot.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/IterativeRobot.java index ab018043a3..85808cc4b8 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/IterativeRobot.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/IterativeRobot.java @@ -10,285 +10,30 @@ package edu.wpi.first.wpilibj; import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances; import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** - * IterativeRobot implements a specific type of Robot Program framework, extending the RobotBase - * class. + * IterativeRobot implements the IterativeRobotBase robot program framework. * *

The IterativeRobot class is intended to be subclassed by a user creating a robot program. * - *

This class is intended to implement the "old style" default code, by providing the following - * functions which are called by the main loop, startCompetition(), at the appropriate times: - * - *

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 only when first disabled - 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 mode is - * entered from anothermode - * - *

Periodic() functions -- each of these functions is called iteratively at the appropriate - * periodic rate (aka the "slow loop"). The period of the iterative robot is synced to the driver - * station control packets, giving a periodic frequency of about 50Hz (50 times per second). - - * disabledPeriodic() - autonomousPeriodic() - teleopPeriodic() - testPeriodoc() + *

periodic() functions from the base class are called each time a new packet is received from + * the driver station. */ -public class IterativeRobot extends RobotBase { - private boolean m_disabledInitialized; - private boolean m_autonomousInitialized; - private boolean m_teleopInitialized; - private boolean m_testInitialized; - - /** - * Constructor for RobotIterativeBase. - * - *

The constructor initializes the instance variables for the robot to indicate the status of - * initialization for disabled, autonomous, and teleop code. - */ +public class IterativeRobot extends IterativeRobotBase { public IterativeRobot() { - // set status for initialization of disabled, autonomous, and teleop code. - m_disabledInitialized = false; - m_autonomousInitialized = false; - m_teleopInitialized = false; - m_testInitialized = false; + HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative); } /** * Provide an alternate "main loop" via startCompetition(). */ public void startCompetition() { - HAL.report(tResourceType.kResourceType_Framework, - tInstances.kFramework_Iterative); - - robotInit(); - - // Tell the DS that the robot is ready to be enabled - HAL.observeUserProgramStarting(); - // loop forever, calling the appropriate mode-dependent function - LiveWindow.setEnabled(false); while (true) { // Wait for new data to arrive m_ds.waitForData(); - // Call the appropriate function depending upon the current robot mode - if (isDisabled()) { - // call DisabledInit() if we are now just entering disabled mode from - // either a different mode or from power-on - if (!m_disabledInitialized) { - LiveWindow.setEnabled(false); - disabledInit(); - m_disabledInitialized = true; - // reset the initialization flags for the other modes - m_autonomousInitialized = false; - m_teleopInitialized = false; - m_testInitialized = false; - } - HAL.observeUserProgramDisabled(); - disabledPeriodic(); - } else if (isTest()) { - // call TestInit() if we are now just entering test mode from either - // a different mode or from power-on - if (!m_testInitialized) { - LiveWindow.setEnabled(true); - testInit(); - m_testInitialized = true; - m_autonomousInitialized = false; - m_teleopInitialized = false; - m_disabledInitialized = false; - } - HAL.observeUserProgramTest(); - testPeriodic(); - } else if (isAutonomous()) { - // call Autonomous_Init() if this is the first time - // we've entered autonomous_mode - if (!m_autonomousInitialized) { - LiveWindow.setEnabled(false); - // KBS NOTE: old code reset all PWMs and relays to "safe values" - // whenever entering autonomous mode, before calling - // "Autonomous_Init()" - autonomousInit(); - m_autonomousInitialized = true; - m_testInitialized = false; - m_teleopInitialized = false; - m_disabledInitialized = false; - } - HAL.observeUserProgramAutonomous(); - autonomousPeriodic(); - } else { - // call Teleop_Init() if this is the first time - // we've entered teleop_mode - if (!m_teleopInitialized) { - LiveWindow.setEnabled(false); - teleopInit(); - m_teleopInitialized = true; - m_testInitialized = false; - m_autonomousInitialized = false; - m_disabledInitialized = false; - } - HAL.observeUserProgramTeleop(); - teleopPeriodic(); - } - robotPeriodic(); - } - } - /* ----------- Overridable initialization code ----------------- */ - - /** - * Robot-wide initialization code should go here. - * - *

Users should override this method for default Robot-wide initialization which will be called - * when the robot is first powered on. It will be called exactly one time. - * - *

Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" indicators will be off - * until RobotInit() exits. Code in RobotInit() that waits for enable will cause the robot to - * never indicate that the code is ready, causing the robot to be bypassed in a match. - */ - public void robotInit() { - System.out.println("Default IterativeRobot.robotInit() method... Overload me!"); - } - - /** - * Initialization code for disabled mode should go here. - * - *

Users should override this method for initialization code which will be called each time the - * robot enters disabled mode. - */ - public void disabledInit() { - System.out.println("Default IterativeRobot.disabledInit() method... Overload me!"); - } - - /** - * Initialization code for autonomous mode should go here. - * - *

Users should override this method for initialization code which will be called each time the - * robot enters autonomous mode. - */ - public void autonomousInit() { - System.out.println("Default IterativeRobot.autonomousInit() method... Overload me!"); - } - - /** - * Initialization code for teleop mode should go here. - * - *

Users should override this method for initialization code which will be called each time the - * robot enters teleop mode. - */ - public void teleopInit() { - System.out.println("Default IterativeRobot.teleopInit() method... Overload me!"); - } - - /** - * Initialization code for test mode should go here. - * - *

Users should override this method for initialization code which will be called each time the - * robot enters test mode. - */ - @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation") - public void testInit() { - System.out.println("Default IterativeRobot.testInit() method... Overload me!"); - } - - /* ----------- Overridable periodic code ----------------- */ - - private boolean m_rpFirstRun = true; - - /** - * Periodic code for all robot modes should go here. - * - *

This function is called each time a new packet is received from the driver station. - * - *

Packets are received approximately every 20ms. Fixed loop timing is not guaranteed due to - * network timing variability and the function may not be called at all if the Driver Station is - * disconnected. For most use cases the variable timing will not be an issue. If your code does - * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead. - */ - public void robotPeriodic() { - if (m_rpFirstRun) { - System.out.println("Default IterativeRobot.robotPeriodic() method... Overload me!"); - m_rpFirstRun = false; - } - } - - private boolean m_dpFirstRun = true; - - /** - * Periodic code for disabled mode should go here. - * - *

Users should override this method for code which will be called each time a new packet is - * received from the driver station and the robot is in disabled mode. - * - *

Packets are received approximately every 20ms. Fixed loop timing is not guaranteed due to - * network timing variability and the function may not be called at all if the Driver Station is - * disconnected. For most use cases the variable timing will not be an issue. If your code does - * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead. - */ - public void disabledPeriodic() { - if (m_dpFirstRun) { - System.out.println("Default IterativeRobot.disabledPeriodic() method... Overload me!"); - m_dpFirstRun = false; - } - } - - private boolean m_apFirstRun = true; - - /** - * Periodic code for autonomous mode should go here. - * - *

Users should override this method for code which will be called each time a new packet is - * received from the driver station and the robot is in autonomous mode. - * - *

Packets are received approximately every 20ms. Fixed loop timing is not guaranteed due to - * network timing variability and the function may not be called at all if the Driver Station is - * disconnected. For most use cases the variable timing will not be an issue. If your code does - * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead. - */ - public void autonomousPeriodic() { - if (m_apFirstRun) { - System.out.println("Default IterativeRobot.autonomousPeriodic() method... Overload me!"); - m_apFirstRun = false; - } - } - - private boolean m_tpFirstRun = true; - - /** - * Periodic code for teleop mode should go here. - * - *

Users should override this method for code which will be called each time a new packet is - * received from the driver station and the robot is in teleop mode. - * - *

Packets are received approximately every 20ms. Fixed loop timing is not guaranteed due to - * network timing variability and the function may not be called at all if the Driver Station is - * disconnected. For most use cases the variable timing will not be an issue. If your code does - * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead. - */ - public void teleopPeriodic() { - if (m_tpFirstRun) { - System.out.println("Default IterativeRobot.teleopPeriodic() method... Overload me!"); - m_tpFirstRun = false; - } - } - - private boolean m_tmpFirstRun = true; - - /** - * Periodic code for test mode should go here. - * - *

Users should override this method for code which will be called each time a new packet is - * received from the driver station and the robot is in test mode. - * - *

Packets are received approximately every 20ms. Fixed loop timing is not guaranteed due to - * network timing variability and the function may not be called at all if the Driver Station is - * disconnected. For most use cases the variable timing will not be an issue. If your code does - * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead. - */ - @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation") - public void testPeriodic() { - if (m_tmpFirstRun) { - System.out.println("Default IterativeRobot.testPeriodic() method... Overload me!"); - m_tmpFirstRun = false; + loopFunc(); } } } diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/IterativeRobotBase.java new file mode 100644 index 0000000000..0fd094285c --- /dev/null +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/IterativeRobotBase.java @@ -0,0 +1,236 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.wpilibj.hal.HAL; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; + +/** + * IterativeRobotBase implements a specific type of robot program framework, extending the RobotBase + * class. + * + *

The IterativeRobotBase class does not implement startCompetition(), so it should not be used + * by teams directly. + * + *

This class provides the following functions which are called by the main loop, + * startCompetition(), at the appropriate times: + * + *

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 only when first disabled + * - 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() + */ +public abstract class IterativeRobotBase extends RobotBase { + private enum Mode { + kNone, + kDisabled, + kAutonomous, + kTeleop, + kTest + } + + private Mode m_lastMode = Mode.kNone; + + /** + * IterativeRobotBase constructor. + */ + public IterativeRobotBase() { + robotInit(); + + // Tell the DS that the robot is ready to be enabled + HAL.observeUserProgramStarting(); + } + + /** + * Provide an alternate "main loop" via startCompetition(). + */ + public abstract void startCompetition(); + + /* ----------- Overridable initialization code ----------------- */ + + /** + * Robot-wide initialization code should go here. + * + *

Users should override this method for default Robot-wide initialization which will be called + * when the robot is first powered on. It will be called exactly one time. + * + *

Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" indicators will be off + * until RobotInit() exits. Code in RobotInit() that waits for enable will cause the robot to + * never indicate that the code is ready, causing the robot to be bypassed in a match. + */ + public void robotInit() { + System.out.println("Default robotInit() method... Overload me!"); + } + + /** + * Initialization code for disabled mode should go here. + * + *

Users should override this method for initialization code which will be called each time the + * robot enters disabled mode. + */ + public void disabledInit() { + System.out.println("Default disabledInit() method... Overload me!"); + } + + /** + * Initialization code for autonomous mode should go here. + * + *

Users should override this method for initialization code which will be called each time the + * robot enters autonomous mode. + */ + public void autonomousInit() { + System.out.println("Default autonomousInit() method... Overload me!"); + } + + /** + * Initialization code for teleop mode should go here. + * + *

Users should override this method for initialization code which will be called each time the + * robot enters teleop mode. + */ + public void teleopInit() { + System.out.println("Default teleopInit() method... Overload me!"); + } + + /** + * Initialization code for test mode should go here. + * + *

Users should override this method for initialization code which will be called each time the + * robot enters test mode. + */ + @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation") + public void testInit() { + System.out.println("Default testInit() method... Overload me!"); + } + + /* ----------- Overridable periodic code ----------------- */ + + private boolean m_rpFirstRun = true; + + /** + * Periodic code for all robot modes should go here. + */ + public void robotPeriodic() { + if (m_rpFirstRun) { + System.out.println("Default robotPeriodic() method... Overload me!"); + m_rpFirstRun = false; + } + } + + private boolean m_dpFirstRun = true; + + /** + * Periodic code for disabled mode should go here. + */ + public void disabledPeriodic() { + if (m_dpFirstRun) { + System.out.println("Default disabledPeriodic() method... Overload me!"); + m_dpFirstRun = false; + } + } + + private boolean m_apFirstRun = true; + + /** + * Periodic code for autonomous mode should go here. + */ + public void autonomousPeriodic() { + if (m_apFirstRun) { + System.out.println("Default autonomousPeriodic() method... Overload me!"); + m_apFirstRun = false; + } + } + + private boolean m_tpFirstRun = true; + + /** + * Periodic code for teleop mode should go here. + */ + public void teleopPeriodic() { + if (m_tpFirstRun) { + System.out.println("Default teleopPeriodic() method... Overload me!"); + m_tpFirstRun = false; + } + } + + private boolean m_tmpFirstRun = true; + + /** + * Periodic code for test mode should go here. + */ + @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation") + public void testPeriodic() { + if (m_tmpFirstRun) { + System.out.println("Default testPeriodic() method... Overload me!"); + m_tmpFirstRun = false; + } + } + + protected void loopFunc() { + // Call the appropriate function depending upon the current robot mode + 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) { + LiveWindow.setEnabled(false); + disabledInit(); + m_lastMode = Mode.kDisabled; + } + HAL.observeUserProgramDisabled(); + disabledPeriodic(); + } else if (isAutonomous()) { + // call Autonomous_Init() if this is the first time + // we've entered autonomous_mode + if (m_lastMode != Mode.kAutonomous) { + LiveWindow.setEnabled(false); + // KBS NOTE: old code reset all PWMs and relays to "safe values" + // whenever entering autonomous mode, before calling + // "Autonomous_Init()" + autonomousInit(); + m_lastMode = Mode.kAutonomous; + } + HAL.observeUserProgramAutonomous(); + autonomousPeriodic(); + } else if (isOperatorControl()) { + // call Teleop_Init() if this is the first time + // we've entered teleop_mode + if (m_lastMode != Mode.kTeleop) { + LiveWindow.setEnabled(false); + teleopInit(); + m_lastMode = Mode.kTeleop; + } + HAL.observeUserProgramTeleop(); + 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) { + LiveWindow.setEnabled(true); + testInit(); + m_lastMode = Mode.kTest; + } + HAL.observeUserProgramTest(); + testPeriodic(); + } + robotPeriodic(); + } +} diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotBase.java index ea315b24bb..3c43354940 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotBase.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/RobotBase.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; import edu.wpi.first.wpilibj.hal.HAL; import edu.wpi.first.wpilibj.internal.HardwareHLUsageReporting; import edu.wpi.first.wpilibj.internal.HardwareTimer; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.networktables.NetworkTable; import edu.wpi.first.wpilibj.util.WPILibVersion; @@ -55,16 +56,14 @@ public abstract class RobotBase { * to put this code into it's own task that loads on boot so ensure that it runs. */ protected RobotBase() { - // TODO: StartCAPI(); - // TODO: See if the next line is necessary - // Resource.RestartProgram(); - NetworkTable.setNetworkIdentity("Robot"); NetworkTable.setPersistentFilename("/home/lvuser/networktables.ini"); NetworkTable.setServerMode(); // must be before b m_ds = DriverStation.getInstance(); NetworkTable.getTable(""); // forces network tables to initialize NetworkTable.getTable("LiveWindow").getSubTable("~STATUS~").putBoolean("LW Enabled", false); + + LiveWindow.setEnabled(false); } /** @@ -212,6 +211,8 @@ public abstract class RobotBase { } } + System.out.println("********** Robot program starting **********"); + RobotBase robot; try { robot = (RobotBase) Class.forName(robotName).newInstance(); @@ -244,7 +245,6 @@ public abstract class RobotBase { boolean errorOnExit = false; try { - System.out.println("********** Robot program starting **********"); robot.startCompetition(); } catch (Throwable throwable) { DriverStation.reportError( diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SampleRobot.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SampleRobot.java index 407d972048..e7343ec3fa 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SampleRobot.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/SampleRobot.java @@ -25,14 +25,18 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow; */ public class SampleRobot extends RobotBase { - private boolean m_robotMainOverridden; + private boolean m_robotMainOverridden = true; /** * Create a new SampleRobot. */ public SampleRobot() { - super(); - m_robotMainOverridden = true; + robotInit(); + + // Tell the DS that the robot is ready to be enabled + HAL.observeUserProgramStarting(); + + HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Simple); } /** @@ -111,19 +115,8 @@ public class SampleRobot extends RobotBase { * for the robot to be enabled again. */ public void startCompetition() { - HAL.report(tResourceType.kResourceType_Framework, - tInstances.kFramework_Simple); - - robotInit(); - - // Tell the DS that the robot is ready to be enabled - HAL.observeUserProgramStarting(); - robotMain(); if (!m_robotMainOverridden) { - // first and one-time initialization - LiveWindow.setEnabled(false); - while (true) { if (isDisabled()) { m_ds.InDisabled(true); diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/TimedRobot.java new file mode 100644 index 0000000000..1eebb8c921 --- /dev/null +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -0,0 +1,66 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2017. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj; + +import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances; +import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType; +import edu.wpi.first.wpilibj.hal.HAL; + +/** + * TimedRobot implements the IterativeRobotBase robot program framework. + * + *

The TimedRobot class is intended to be subclassed by a user creating a robot program. + * + *

periodic() functions from the base class are called on an interval by a Notifier instance. + */ +public class TimedRobot extends IterativeRobotBase { + public static final double DEFAULT_PERIOD = 0.02; + + private double m_period = DEFAULT_PERIOD; + + // Prevents loop from starting if user calls setPeriod() in robotInit() + private boolean m_startLoop = false; + + private Notifier m_loop = new Notifier(() -> { + loopFunc(); + }); + + public TimedRobot() { + // HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Periodic); + HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative); + } + + /** + * Provide an alternate "main loop" via startCompetition(). + */ + public void startCompetition() { + // loop forever, calling the appropriate mode-dependent function + m_startLoop = true; + m_loop.startPeriodic(m_period); + while (true) { + try { + Thread.sleep(1000 * 60 * 60 * 24); + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + } + } + + /** + * Set time period between calls to Periodic() functions. + * + * @param period Period in seconds. + */ + public void setPeriod(double period) { + m_period = period; + + if (m_startLoop) { + m_loop.startPeriodic(m_period); + } + } +}