mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-30 02:31:44 +00:00
Move robot base classes from opmode to framework (#8344)
Having these in opmode will be confusing to users when opmodes are added.
This commit is contained in:
Peter Johnson
@@ -0,0 +1,240 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "wpi/framework/RobotBase.hpp"
|
||||
#include "wpi/system/Watchdog.hpp"
|
||||
#include "wpi/units/time.hpp"
|
||||
|
||||
namespace wpi {
|
||||
|
||||
/**
|
||||
* 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:
|
||||
*
|
||||
* DriverStationConnected() -- provide for initialization the first time the DS
|
||||
* is connected
|
||||
*
|
||||
* Init() functions -- each of the following functions is called once when the
|
||||
* 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:
|
||||
*
|
||||
* \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:
|
||||
/**
|
||||
* Code that needs to know the DS state should go here.
|
||||
*
|
||||
* Users should override this method for initialization that needs to occur
|
||||
* after the DS is connected, such as needing the alliance information.
|
||||
*/
|
||||
virtual void DriverStationConnected();
|
||||
|
||||
/**
|
||||
* Robot-wide simulation initialization code should go here.
|
||||
*
|
||||
* Users should override this method for default Robot-wide simulation
|
||||
* related initialization which will be called when the robot is first
|
||||
* started. It will be called exactly one time after the robot class
|
||||
* constructor is called only when the robot is in simulation.
|
||||
*/
|
||||
virtual void SimulationInit();
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
virtual void DisabledInit();
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
virtual void AutonomousInit();
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
virtual void TeleopInit();
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
virtual void TestInit();
|
||||
|
||||
/**
|
||||
* Periodic code for all modes should go here.
|
||||
*
|
||||
* This function is called each time a new packet is received from the driver
|
||||
* station.
|
||||
*/
|
||||
virtual void RobotPeriodic();
|
||||
|
||||
/**
|
||||
* Periodic simulation code should go here.
|
||||
*
|
||||
* This function is called in a simulated robot after user code executes.
|
||||
*/
|
||||
virtual void SimulationPeriodic();
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
virtual void DisabledPeriodic();
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
virtual void AutonomousPeriodic();
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
virtual void TeleopPeriodic();
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
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 enabled.
|
||||
*
|
||||
* @param enabled True to enable, false to disable
|
||||
* @deprecated Deprecated without replacement.
|
||||
*/
|
||||
[[deprecated("Deprecated without replacement.")]]
|
||||
void SetNetworkTablesFlushEnabled(bool enabled);
|
||||
|
||||
/**
|
||||
* Gets time period between calls to Periodic() functions.
|
||||
*/
|
||||
wpi::units::second_t GetPeriod() const;
|
||||
|
||||
/**
|
||||
* Prints list of epochs added so far and their times.
|
||||
*/
|
||||
void PrintWatchdogEpochs();
|
||||
|
||||
/**
|
||||
* Constructor for IterativeRobotBase.
|
||||
*
|
||||
* @param period Period.
|
||||
*/
|
||||
explicit IterativeRobotBase(wpi::units::second_t period);
|
||||
|
||||
~IterativeRobotBase() override = default;
|
||||
|
||||
protected:
|
||||
IterativeRobotBase(IterativeRobotBase&&) = default;
|
||||
IterativeRobotBase& operator=(IterativeRobotBase&&) = default;
|
||||
|
||||
/**
|
||||
* Loop function.
|
||||
*/
|
||||
void LoopFunc();
|
||||
|
||||
private:
|
||||
enum class Mode { kNone, kDisabled, kAutonomous, kTeleop, kTest };
|
||||
|
||||
Mode m_lastMode = Mode::kNone;
|
||||
wpi::units::second_t m_period;
|
||||
Watchdog m_watchdog;
|
||||
bool m_ntFlushEnabled = true;
|
||||
bool m_calledDsConnected = false;
|
||||
|
||||
void PrintLoopOverrunMessage();
|
||||
};
|
||||
|
||||
} // namespace wpi
|
||||
280
wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp
Normal file
280
wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp
Normal file
@@ -0,0 +1,280 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "wpi/hal/DriverStation.h"
|
||||
#include "wpi/hal/HALBase.h"
|
||||
#include "wpi/hal/Main.h"
|
||||
#include "wpi/nt/NetworkTable.hpp"
|
||||
#include "wpi/system/Errors.hpp"
|
||||
#include "wpi/system/RuntimeType.hpp"
|
||||
#include "wpi/util/RuntimeCheck.h"
|
||||
#include "wpi/util/condition_variable.hpp"
|
||||
#include "wpi/util/mutex.hpp"
|
||||
#include "wpi/util/string.h"
|
||||
|
||||
namespace wpi {
|
||||
|
||||
int RunHALInitialization();
|
||||
|
||||
namespace impl {
|
||||
#ifndef __FRC_SYSTEMCORE__
|
||||
void ResetMotorSafety();
|
||||
#endif
|
||||
|
||||
template <class Robot>
|
||||
void RunRobot(wpi::util::mutex& m, Robot** robot) {
|
||||
try {
|
||||
static Robot theRobot;
|
||||
{
|
||||
std::scoped_lock lock{m};
|
||||
*robot = &theRobot;
|
||||
}
|
||||
theRobot.StartCompetition();
|
||||
} catch (const wpi::RuntimeError& e) {
|
||||
e.Report();
|
||||
WPILIB_ReportError(
|
||||
err::Error,
|
||||
"The robot program quit unexpectedly."
|
||||
" This is usually due to a code error.\n"
|
||||
" The above stacktrace can help determine where the error occurred.\n"
|
||||
" See https://wpilib.org/stacktrace for more information.\n");
|
||||
throw;
|
||||
} catch (const std::exception& e) {
|
||||
HAL_SendError(1, err::Error, 0, e.what(), "", "", 1);
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace impl
|
||||
|
||||
template <class Robot>
|
||||
int StartRobot() {
|
||||
uint32_t foundMajor;
|
||||
uint32_t foundMinor;
|
||||
uint32_t expectedMajor;
|
||||
uint32_t expectedMinor;
|
||||
WPI_String runtimePath;
|
||||
if (!WPI_IsRuntimeValid(&foundMajor, &foundMinor, &expectedMajor,
|
||||
&expectedMinor, &runtimePath)) {
|
||||
// We could make this error better, however unlike Java, there is only a
|
||||
// single scenario that could be occuring. The entirety of VS is too out
|
||||
// of date. In most cases the linker should detect this, but not always.
|
||||
fmt::println(
|
||||
"Your copy of Visual Studio is out of date. Please update it.\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
int halInit = RunHALInitialization();
|
||||
if (halInit != 0) {
|
||||
return halInit;
|
||||
}
|
||||
|
||||
static wpi::util::mutex m;
|
||||
static wpi::util::condition_variable cv;
|
||||
static Robot* robot = nullptr;
|
||||
static bool exited = false;
|
||||
|
||||
if (HAL_HasMain()) {
|
||||
std::thread thr([] {
|
||||
try {
|
||||
impl::RunRobot<Robot>(m, &robot);
|
||||
} catch (...) {
|
||||
HAL_ExitMain();
|
||||
{
|
||||
std::scoped_lock lock{m};
|
||||
robot = nullptr;
|
||||
exited = true;
|
||||
}
|
||||
cv.notify_all();
|
||||
throw;
|
||||
}
|
||||
|
||||
HAL_ExitMain();
|
||||
{
|
||||
std::scoped_lock lock{m};
|
||||
robot = nullptr;
|
||||
exited = true;
|
||||
}
|
||||
cv.notify_all();
|
||||
});
|
||||
|
||||
HAL_RunMain();
|
||||
|
||||
// signal loop to exit
|
||||
if (robot) {
|
||||
robot->EndCompetition();
|
||||
}
|
||||
|
||||
// prefer to join, but detach to exit if it doesn't exit in a timely manner
|
||||
using namespace std::chrono_literals;
|
||||
std::unique_lock lock{m};
|
||||
if (cv.wait_for(lock, 1s, [] { return exited; })) {
|
||||
thr.join();
|
||||
} else {
|
||||
thr.detach();
|
||||
}
|
||||
} else {
|
||||
impl::RunRobot<Robot>(m, &robot);
|
||||
}
|
||||
|
||||
#ifndef __FRC_SYSTEMCORE__
|
||||
wpi::impl::ResetMotorSafety();
|
||||
#endif
|
||||
HAL_Shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Implement a Robot Program framework. The RobotBase class is intended to be
|
||||
* subclassed to create a robot program. The user must implement
|
||||
* StartCompetition() which will be called once and is not expected to exit. The
|
||||
* user must also implement EndCompetition(), which signals to the code in
|
||||
* StartCompetition() that it should exit.
|
||||
*
|
||||
* It is not recommended to subclass this class directly - instead subclass
|
||||
* IterativeRobotBase or TimedRobot.
|
||||
*/
|
||||
class RobotBase {
|
||||
public:
|
||||
/**
|
||||
* Determine if the Robot is currently enabled.
|
||||
*
|
||||
* @return True if the Robot is currently enabled by the Driver Station.
|
||||
*/
|
||||
bool IsEnabled() const;
|
||||
|
||||
/**
|
||||
* Determine if the Robot is currently disabled.
|
||||
*
|
||||
* @return True if the Robot is currently disabled by the Driver Station.
|
||||
*/
|
||||
bool IsDisabled() const;
|
||||
|
||||
/**
|
||||
* Determine if the robot is currently in Autonomous mode.
|
||||
*
|
||||
* @return True if the robot is currently operating Autonomously as determined
|
||||
* by the Driver Station.
|
||||
*/
|
||||
bool IsAutonomous() const;
|
||||
|
||||
/**
|
||||
* Determine if the robot is currently in Autonomous mode and enabled.
|
||||
*
|
||||
* @return True if the robot us currently operating Autonomously while enabled
|
||||
* as determined by the Driver Station.
|
||||
*/
|
||||
bool IsAutonomousEnabled() const;
|
||||
|
||||
/**
|
||||
* Determine if the robot is currently in Operator Control mode.
|
||||
*
|
||||
* @return True if the robot is currently operating in Tele-Op mode as
|
||||
* determined by the Driver Station.
|
||||
*/
|
||||
bool IsTeleop() const;
|
||||
|
||||
/**
|
||||
* Determine if the robot is current in Operator Control mode and enabled.
|
||||
*
|
||||
* @return True if the robot is currently operating in Tele-Op mode while
|
||||
* enabled as determined by the Driver Station.
|
||||
*/
|
||||
bool IsTeleopEnabled() const;
|
||||
|
||||
/**
|
||||
* Determine if the robot is currently in Test mode.
|
||||
*
|
||||
* @return True if the robot is currently running in Test mode as determined
|
||||
* by the Driver Station.
|
||||
*/
|
||||
bool IsTest() const;
|
||||
|
||||
/**
|
||||
* Determine if the robot is current in Test mode and enabled.
|
||||
*
|
||||
* @return True if the robot is currently operating in Test mode while
|
||||
* enabled as determined by the Driver Station.
|
||||
*/
|
||||
bool IsTestEnabled() const;
|
||||
|
||||
/**
|
||||
* Returns the main thread ID.
|
||||
*
|
||||
* @return The main thread ID.
|
||||
*/
|
||||
static std::thread::id GetThreadId();
|
||||
|
||||
/**
|
||||
* Start the main robot code. This function will be called once and should not
|
||||
* exit until signalled by EndCompetition()
|
||||
*/
|
||||
virtual void StartCompetition() = 0;
|
||||
|
||||
/** Ends the main loop in StartCompetition(). */
|
||||
virtual void EndCompetition() = 0;
|
||||
|
||||
/**
|
||||
* Get the current runtime type.
|
||||
*
|
||||
* @return Current runtime type.
|
||||
*/
|
||||
static RuntimeType GetRuntimeType();
|
||||
|
||||
/**
|
||||
* Get if the robot is real.
|
||||
*
|
||||
* @return If the robot is running in the real world.
|
||||
*/
|
||||
static constexpr bool IsReal() {
|
||||
#ifdef __FRC_SYSTEMCORE__
|
||||
return true;
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* Get if the robot is a simulation.
|
||||
*
|
||||
* @return If the robot is running in simulation.
|
||||
*/
|
||||
static constexpr bool IsSimulation() {
|
||||
#ifdef __FRC_SYSTEMCORE__
|
||||
return false;
|
||||
#else
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor for a generic robot program.
|
||||
*
|
||||
* User code can be placed in the constructor that runs before the
|
||||
* Autonomous or Operator Control period starts. The constructor will run to
|
||||
* completion before Autonomous is entered.
|
||||
*
|
||||
* This must be used to ensure that the communications code starts. In the
|
||||
* future it would be nice to put this code into it's own task that loads on
|
||||
* boot so ensure that it runs.
|
||||
*/
|
||||
RobotBase();
|
||||
|
||||
virtual ~RobotBase() = default;
|
||||
|
||||
protected:
|
||||
RobotBase(RobotBase&&) = default;
|
||||
RobotBase& operator=(RobotBase&&) = default;
|
||||
|
||||
static std::thread::id m_threadId;
|
||||
NT_Listener connListenerHandle;
|
||||
};
|
||||
|
||||
} // namespace wpi
|
||||
59
wpilibc/src/main/native/include/wpi/framework/RobotState.hpp
Normal file
59
wpilibc/src/main/native/include/wpi/framework/RobotState.hpp
Normal file
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace wpi {
|
||||
|
||||
/**
|
||||
* Robot state utility functions.
|
||||
*/
|
||||
class RobotState {
|
||||
public:
|
||||
RobotState() = delete;
|
||||
|
||||
/**
|
||||
* Returns true if the robot is disabled.
|
||||
*
|
||||
* @return True if the robot is disabled.
|
||||
*/
|
||||
static bool IsDisabled();
|
||||
|
||||
/**
|
||||
* Returns true if the robot is enabled.
|
||||
*
|
||||
* @return True if the robot is enabled.
|
||||
*/
|
||||
static bool IsEnabled();
|
||||
|
||||
/**
|
||||
* Returns true if the robot is E-stopped.
|
||||
*
|
||||
* @return True if the robot is E-stopped.
|
||||
*/
|
||||
static bool IsEStopped();
|
||||
|
||||
/**
|
||||
* Returns true if the robot is in teleop mode.
|
||||
*
|
||||
* @return True if the robot is in teleop mode.
|
||||
*/
|
||||
static bool IsTeleop();
|
||||
|
||||
/**
|
||||
* Returns true if the robot is in autonomous mode.
|
||||
*
|
||||
* @return True if the robot is in autonomous mode.
|
||||
*/
|
||||
static bool IsAutonomous();
|
||||
|
||||
/**
|
||||
* Returns true if the robot is in test mode.
|
||||
*
|
||||
* @return True if the robot is in test mode.
|
||||
*/
|
||||
static bool IsTest();
|
||||
};
|
||||
|
||||
} // namespace wpi
|
||||
131
wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp
Normal file
131
wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp
Normal file
@@ -0,0 +1,131 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "wpi/framework/IterativeRobotBase.hpp"
|
||||
#include "wpi/hal/Notifier.h"
|
||||
#include "wpi/hal/Types.h"
|
||||
#include "wpi/system/RobotController.hpp"
|
||||
#include "wpi/units/frequency.hpp"
|
||||
#include "wpi/units/math.hpp"
|
||||
#include "wpi/units/time.hpp"
|
||||
#include "wpi/util/priority_queue.hpp"
|
||||
|
||||
namespace wpi {
|
||||
|
||||
/**
|
||||
* 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:
|
||||
/// Default loop period.
|
||||
static constexpr auto kDefaultPeriod = 20_ms;
|
||||
|
||||
/**
|
||||
* Provide an alternate "main loop" via StartCompetition().
|
||||
*/
|
||||
void StartCompetition() override;
|
||||
|
||||
/**
|
||||
* Ends the main loop in StartCompetition().
|
||||
*/
|
||||
void EndCompetition() override;
|
||||
|
||||
/**
|
||||
* Constructor for TimedRobot.
|
||||
*
|
||||
* @param period The period of the robot loop function.
|
||||
*/
|
||||
explicit TimedRobot(wpi::units::second_t period = kDefaultPeriod);
|
||||
|
||||
/**
|
||||
* Constructor for TimedRobot.
|
||||
*
|
||||
* @param frequency The frequency of the robot loop function.
|
||||
*/
|
||||
explicit TimedRobot(wpi::units::hertz_t frequency);
|
||||
|
||||
TimedRobot(TimedRobot&&) = default;
|
||||
TimedRobot& operator=(TimedRobot&&) = default;
|
||||
|
||||
~TimedRobot() override;
|
||||
|
||||
/**
|
||||
* Return the system clock time in micrseconds for the start of the current
|
||||
* periodic loop. This is in the same time base as Timer.GetFPGATimestamp(),
|
||||
* but is stable through a loop. It is updated at the beginning of every
|
||||
* periodic callback (including the normal periodic loop).
|
||||
*
|
||||
* @return Robot running time in microseconds, as of the start of the current
|
||||
* periodic function.
|
||||
*/
|
||||
uint64_t GetLoopStartTime();
|
||||
|
||||
/**
|
||||
* Add a callback to run at a specific period with a starting time offset.
|
||||
*
|
||||
* This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback
|
||||
* run synchronously. Interactions between them are thread-safe.
|
||||
*
|
||||
* @param callback The callback to run.
|
||||
* @param period The period at which to run the callback.
|
||||
* @param offset The offset from the common starting time. This is useful
|
||||
* for scheduling a callback in a different timeslot relative
|
||||
* to TimedRobot.
|
||||
*/
|
||||
void AddPeriodic(std::function<void()> callback, wpi::units::second_t period,
|
||||
wpi::units::second_t offset = 0_s);
|
||||
|
||||
private:
|
||||
class Callback {
|
||||
public:
|
||||
std::function<void()> func;
|
||||
std::chrono::microseconds period;
|
||||
std::chrono::microseconds expirationTime;
|
||||
|
||||
/**
|
||||
* Construct a callback container.
|
||||
*
|
||||
* @param func The callback to run.
|
||||
* @param startTime The common starting point for all callback scheduling.
|
||||
* @param period The period at which to run the callback.
|
||||
* @param offset The offset from the common starting time.
|
||||
*/
|
||||
Callback(std::function<void()> func, std::chrono::microseconds startTime,
|
||||
std::chrono::microseconds period, std::chrono::microseconds offset)
|
||||
: func{std::move(func)},
|
||||
period{period},
|
||||
expirationTime(
|
||||
startTime + offset + period +
|
||||
(std::chrono::microseconds{wpi::RobotController::GetFPGATime()} -
|
||||
startTime) /
|
||||
period * period) {}
|
||||
|
||||
bool operator>(const Callback& rhs) const {
|
||||
return expirationTime > rhs.expirationTime;
|
||||
}
|
||||
};
|
||||
|
||||
wpi::hal::Handle<HAL_NotifierHandle, HAL_CleanNotifier> m_notifier;
|
||||
std::chrono::microseconds m_startTime;
|
||||
uint64_t m_loopStartTimeUs = 0;
|
||||
|
||||
wpi::util::priority_queue<Callback, std::vector<Callback>,
|
||||
std::greater<Callback>>
|
||||
m_callbacks;
|
||||
};
|
||||
|
||||
} // namespace wpi
|
||||
119
wpilibc/src/main/native/include/wpi/framework/TimesliceRobot.hpp
Normal file
119
wpilibc/src/main/native/include/wpi/framework/TimesliceRobot.hpp
Normal file
@@ -0,0 +1,119 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
|
||||
#include "wpi/framework/TimedRobot.hpp"
|
||||
#include "wpi/units/time.hpp"
|
||||
|
||||
namespace wpi {
|
||||
|
||||
/**
|
||||
* TimesliceRobot extends the TimedRobot robot program framework to provide
|
||||
* timeslice scheduling of periodic functions.
|
||||
*
|
||||
* The TimesliceRobot class is intended to be subclassed by a user creating a
|
||||
* robot program.
|
||||
*
|
||||
* This class schedules robot operations serially in a timeslice format.
|
||||
* TimedRobot's periodic functions are the first in the timeslice table with 0
|
||||
* ms offset and 20 ms period. You can schedule additional controller periodic
|
||||
* functions at a shorter period (5 ms by default). You give each one a
|
||||
* timeslice duration, then they're run sequentially. The main benefit of this
|
||||
* approach is consistent starting times for each controller periodic, which can
|
||||
* make odometry and estimators more accurate and controller outputs change more
|
||||
* consistently.
|
||||
*
|
||||
* Here's an example of measured subsystem durations and their timeslice
|
||||
* allocations:
|
||||
*
|
||||
* <table>
|
||||
* <tr>
|
||||
* <td><b>Subsystem</b></td>
|
||||
* <td><b>Duration (ms)</b></td>
|
||||
* <td><b>Allocation (ms)</b></td>
|
||||
* </tr>
|
||||
* <tr>
|
||||
* <td><b>Total</b></td>
|
||||
* <td>5.0</td>
|
||||
* <td>5.0</td>
|
||||
* </tr>
|
||||
* <tr>
|
||||
* <td>TimedRobot</td>
|
||||
* <td>?</td>
|
||||
* <td>2.0</td>
|
||||
* </tr>
|
||||
* <tr>
|
||||
* <td>Drivetrain</td>
|
||||
* <td>1.32</td>
|
||||
* <td>1.5</td>
|
||||
* </tr>
|
||||
* <tr>
|
||||
* <td>Flywheel</td>
|
||||
* <td>0.6</td>
|
||||
* <td>0.7</td>
|
||||
* </tr>
|
||||
* <tr>
|
||||
* <td>Turret</td>
|
||||
* <td>0.6</td>
|
||||
* <td>0.8</td>
|
||||
* </tr>
|
||||
* <tr>
|
||||
* <td><b>Free</b></td>
|
||||
* <td>0.0</td>
|
||||
* <td>N/A</td>
|
||||
* </tr>
|
||||
* </table>
|
||||
*
|
||||
* Since TimedRobot periodic functions only run every 20ms, that leaves a 2 ms
|
||||
* empty spot in the allocation table for three of the four 5 ms cycles
|
||||
* comprising 20 ms. That's OK because the OS needs time to do other things.
|
||||
*
|
||||
* If the robot periodic functions and the controller periodic functions have a
|
||||
* lot of scheduling jitter that cause them to occasionally overlap with later
|
||||
* timeslices, consider giving the main robot thread a real-time priority using
|
||||
* wpi::SetCurrentThreadPriority(). An RT priority of 15 is a reasonable choice.
|
||||
*
|
||||
* If you do enable RT though, <i>make sure your periodic functions do not
|
||||
* block</i>. If they do, the operating system will lock up, and you'll have to
|
||||
* boot the roboRIO into safe mode and delete the robot program to recover.
|
||||
*/
|
||||
class TimesliceRobot : public TimedRobot {
|
||||
public:
|
||||
/**
|
||||
* Constructor for TimesliceRobot.
|
||||
*
|
||||
* @param robotPeriodicAllocation The allocation to give the TimesliceRobot
|
||||
* periodic functions.
|
||||
* @param controllerPeriod The controller period. The sum of all scheduler
|
||||
* allocations should be less than or equal to this
|
||||
* value.
|
||||
*/
|
||||
explicit TimesliceRobot(wpi::units::second_t robotPeriodicAllocation,
|
||||
wpi::units::second_t controllerPeriod);
|
||||
|
||||
/**
|
||||
* Schedule a periodic function with the constructor's controller period and
|
||||
* the given allocation. The function's runtime allocation will be placed
|
||||
* after the end of the previous one's.
|
||||
*
|
||||
* If a call to this function makes the allocations exceed the controller
|
||||
* period, an exception will be thrown since that means the TimesliceRobot
|
||||
* periodic functions and the given function will have conflicting
|
||||
* timeslices.
|
||||
*
|
||||
* @param func Function to schedule.
|
||||
* @param allocation The function's runtime allocation out of the controller
|
||||
* period.
|
||||
*/
|
||||
void Schedule(std::function<void()> func, wpi::units::second_t allocation);
|
||||
|
||||
private:
|
||||
wpi::units::second_t m_nextOffset;
|
||||
wpi::units::second_t m_controllerPeriod;
|
||||
};
|
||||
|
||||
} // namespace wpi
|
||||
Reference in New Issue
Block a user