Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal, wpilib] Add OpMode support (#7744)

Browse Source 
User code:
- OpModeRobot used as the robot base class
- LinearOpMode and PeriodicOpMode are provided opmode base classes
- In Java, annotations can be used to automatically register opmode classes

Additional user code functionality:
- OpMode (string) is available in addition to the overall
auto/teleop/test robot mode
- OpMode does not indicate enable (enable/disable is still separate)
- The HAL API uses integer UIDs; these are exposed at the user API level
as well for faster checks
- User code creates opmodes on startup (these have name, category,
description, etc).

DS:
- DS will present opmode selection lists for auto and teleop for
match/practice. During a match, the DS will automatically activate the
selected opmode in the corresponding match period.
- For testing, an overall mode is selected (e.g. teleop/auto/test) and a
single opmode is selected

Future work:
- Command framework support/integration
- Python annotation support
- Unit tests (needs race-free DS sim updates)
- Porting of examples

Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
This commit is contained in:
Peter Johnson
2025-12-12 21:25:57 -07:00
committed by GitHub
parent 2a41b80e00
commit dacded37e5
163 changed files with 7454 additions and 2175 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
wpilibc/src/main/native/cpp/driverstation/DSControlWord.cpp
View File

@@ -1,55 +0,0 @@
// 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.
#include "wpi/driverstation/DSControlWord.hpp"
#include "wpi/hal/DriverStation.h"
using namespace wpi;
DSControlWord::DSControlWord() {
HAL_GetControlWord(&m_controlWord);
}
bool DSControlWord::IsEnabled() const {
return m_controlWord.enabled && m_controlWord.dsAttached;
}
bool DSControlWord::IsDisabled() const {
return !(m_controlWord.enabled && m_controlWord.dsAttached);
}
bool DSControlWord::IsEStopped() const {
return m_controlWord.eStop;
}
bool DSControlWord::IsAutonomous() const {
return m_controlWord.autonomous;
}
bool DSControlWord::IsAutonomousEnabled() const {
return m_controlWord.autonomous && m_controlWord.enabled &&
m_controlWord.dsAttached;
}
bool DSControlWord::IsTeleop() const {
return !(m_controlWord.autonomous || m_controlWord.test);
}
bool DSControlWord::IsTeleopEnabled() const {
return !m_controlWord.autonomous && !m_controlWord.test &&
m_controlWord.enabled && m_controlWord.dsAttached;
}
bool DSControlWord::IsTest() const {
return m_controlWord.test;
}
bool DSControlWord::IsDSAttached() const {
return m_controlWord.dsAttached;
}
bool DSControlWord::IsFMSAttached() const {
return m_controlWord.fmsAttached;
}

242
wpilibc/src/main/native/cpp/driverstation/DriverStation.cpp
View File

@@ -8,10 +8,12 @@
#include <array>
#include <atomic>
#include <functional>
#include <memory>
#include <span>
#include <string>
#include <string_view>
#include <vector>
#include <fmt/format.h>
@@ -25,12 +27,18 @@
#include "wpi/nt/NetworkTable.hpp"
#include "wpi/nt/NetworkTableInstance.hpp"
#include "wpi/nt/StringTopic.hpp"
#include "wpi/nt/StructTopic.hpp"
#include "wpi/system/Errors.hpp"
#include "wpi/system/Timer.hpp"
#include "wpi/util/Color.hpp"
#include "wpi/util/DenseMap.hpp"
#include "wpi/util/EventVector.hpp"
#include "wpi/util/StringExtras.hpp"
#include "wpi/util/condition_variable.hpp"
#include "wpi/util/json.hpp"
#include "wpi/util/mutex.hpp"
#include "wpi/util/string.h"
#include "wpi/util/struct/Struct.hpp"
#include "wpi/util/timestamp.h"
using namespace wpi;
@@ -70,6 +78,14 @@ class MatchDataSenderEntry {
static constexpr std::string_view kSmartDashboardType = "FMSInfo";
struct MatchDataSender {
MatchDataSender()
: controlWord{table->GetStructTopic<wpi::hal::ControlWord>("ControlWord")
.Publish()},
opMode{table->GetStringTopic("OpMode").Publish()} {
controlWord.Set(prevControlWord);
opMode.Set("");
}
std::shared_ptr<wpi::nt::NetworkTable> table =
wpi::nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo");
MatchDataSenderEntry<wpi::nt::StringTopic> typeMetaData{
@@ -89,8 +105,9 @@ struct MatchDataSender {
true};
MatchDataSenderEntry<wpi::nt::IntegerTopic> station{table, "StationNumber",
1};
MatchDataSenderEntry<wpi::nt::IntegerTopic> controlWord{table,
"FMSControlData", 0};
wpi::nt::StructPublisher<wpi::hal::ControlWord> controlWord;
wpi::nt::StringPublisher opMode;
wpi::hal::ControlWord prevControlWord;
};
class JoystickLogSender {
@@ -119,11 +136,9 @@ class DataLogSender {
private:
std::atomic_bool m_initialized{false};
HAL_ControlWord m_prevControlWord;
wpi::log::BooleanLogEntry m_logEnabled;
wpi::log::BooleanLogEntry m_logAutonomous;
wpi::log::BooleanLogEntry m_logTest;
wpi::log::BooleanLogEntry m_logEstop;
hal::ControlWord m_prevControlWord;
wpi::log::StructLogEntry<hal::ControlWord> m_logControlWord;
wpi::log::StringLogEntry m_logOpMode;
bool m_logJoysticks;
std::array<JoystickLogSender, DriverStation::kJoystickPorts> m_joysticks;
@@ -146,13 +161,23 @@ struct Instance {
bool silenceJoystickWarning = false;
// Robot state status variables
bool userInDisabled = false;
bool userInAutonomous = false;
bool userInTeleop = false;
bool userInTest = false;
// Op mode lookup
wpi::util::mutex opModeMutex;
wpi::util::DenseMap<int64_t, HAL_OpModeOption> opModes;
wpi::units::second_t nextMessageTime = 0_s;
std::string OpModeToString(int64_t id) {
std::scoped_lock lock{opModeMutex};
if (id == 0) {
return "";
}
auto it = opModes.find(id);
if (it != opModes.end()) {
return std::string{wpi::util::to_string_view(&it->second.name)};
}
return fmt::format("<{}>", id);
}
};
} // namespace
@@ -577,70 +602,103 @@ bool DriverStation::IsJoystickConnected(int stick) {
GetStickPOVsAvailable(stick) != 0;
}
bool DriverStation::IsEnabled() {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
return controlWord.enabled && controlWord.dsAttached;
static int64_t DoAddOpMode(RobotMode mode, std::string_view name,
std::string_view group, std::string_view description,
int32_t textColor, int32_t backgroundColor) {
if (wpi::util::trim(name).empty()) {
return 0;
}
WPI_String nameWpi = wpi::util::make_string(name);
WPI_String groupWpi = wpi::util::make_string(group);
WPI_String descriptionWpi = wpi::util::make_string(description);
auto& inst = ::GetInstance();
std::scoped_lock lock{inst.opModeMutex};
std::string nameCopy{name};
for (;;) {
int64_t id = HAL_MakeOpModeId(static_cast<HAL_RobotMode>(mode),
std::hash<std::string_view>{}(nameCopy));
auto [it, isNew] = inst.opModes.try_emplace(
id, HAL_OpModeOption{id, nameWpi, groupWpi, descriptionWpi, textColor,
backgroundColor});
if (isNew) {
return id;
}
if (HAL_OpMode_GetRobotMode(it->second.id) ==
static_cast<HAL_RobotMode>(mode) &&
wpi::util::to_string_view(&it->second.name) == name) {
return 0; // can't insert duplicate name
}
// collision, try again with space appended
nameCopy += ' ';
}
}
bool DriverStation::IsDisabled() {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
return !(controlWord.enabled && controlWord.dsAttached);
static int32_t ConvertColorToInt(const wpi::util::Color& color) {
return ((static_cast<int32_t>(color.red * 255) & 0xff) << 16) |
((static_cast<int32_t>(color.green * 255) & 0xff) << 8) |
(static_cast<int32_t>(color.blue * 255) & 0xff);
}
bool DriverStation::IsEStopped() {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
return controlWord.eStop;
int64_t DriverStation::AddOpMode(RobotMode mode, std::string_view name,
std::string_view group,
std::string_view description,
const wpi::util::Color& textColor,
const wpi::util::Color& backgroundColor) {
return DoAddOpMode(mode, name, group, description,
ConvertColorToInt(textColor),
ConvertColorToInt(backgroundColor));
}
bool DriverStation::IsAutonomous() {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
return controlWord.autonomous;
int64_t DriverStation::AddOpMode(RobotMode mode, std::string_view name,
std::string_view group,
std::string_view description) {
return DoAddOpMode(mode, name, group, description, -1, -1);
}
bool DriverStation::IsAutonomousEnabled() {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
return controlWord.autonomous && controlWord.enabled;
int64_t DriverStation::RemoveOpMode(RobotMode mode, std::string_view name) {
if (wpi::util::trim(name).empty()) {
return 0;
}
auto& inst = ::GetInstance();
std::scoped_lock lock{inst.opModeMutex};
// we have to loop over all entries to find the one with the correct name
// because the of the unique ID generation scheme
for (auto it = inst.opModes.begin(), end = inst.opModes.end(); it != end;
++it) {
if (HAL_OpMode_GetRobotMode(it->second.id) ==
static_cast<HAL_RobotMode>(mode) &&
wpi::util::to_string_view(&it->second.name) == name) {
int64_t id = it->second.id;
inst.opModes.erase(it);
return id;
}
}
return 0;
}
bool DriverStation::IsTeleop() {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
return !(controlWord.autonomous || controlWord.test);
void DriverStation::PublishOpModes() {
auto& inst = ::GetInstance();
std::scoped_lock lock{inst.opModeMutex};
std::vector<HAL_OpModeOption> options;
options.reserve(inst.opModes.size());
for (auto&& [id, option] : inst.opModes) {
options.emplace_back(option);
}
HAL_SetOpModeOptions(options.data(), options.size());
}
bool DriverStation::IsTeleopEnabled() {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
return !controlWord.autonomous && !controlWord.test && controlWord.enabled;
void DriverStation::ClearOpModes() {
auto& inst = ::GetInstance();
std::scoped_lock lock{inst.opModeMutex};
inst.opModes.clear();
HAL_SetOpModeOptions(nullptr, 0);
}
bool DriverStation::IsTest() {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
return controlWord.test;
}
bool DriverStation::IsTestEnabled() {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
return controlWord.test && controlWord.enabled;
}
bool DriverStation::IsDSAttached() {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
return controlWord.dsAttached;
}
bool DriverStation::IsFMSAttached() {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
return controlWord.fmsAttached;
std::string DriverStation::GetOpMode() {
return GetInstance().OpModeToString(GetOpModeId());
}
std::string DriverStation::GetGameSpecificMessage() {
@@ -853,11 +911,16 @@ void SendMatchData() {
inst.matchDataSender.replayNumber.Set(tmpDataStore.replayNumber);
inst.matchDataSender.matchType.Set(static_cast<int>(tmpDataStore.matchType));
HAL_ControlWord ctlWord;
HAL_GetControlWord(&ctlWord);
int32_t wordInt = 0;
std::memcpy(&wordInt, &ctlWord, sizeof(wordInt));
inst.matchDataSender.controlWord.Set(wordInt);
hal::ControlWord ctlWord = hal::GetControlWord();
if (ctlWord != inst.matchDataSender.prevControlWord) {
int64_t opModeId = ctlWord.GetOpModeId();
if (opModeId != inst.matchDataSender.prevControlWord.GetOpModeId()) {
inst.matchDataSender.opMode.Set(inst.OpModeToString(opModeId));
}
inst.matchDataSender.prevControlWord = ctlWord;
inst.matchDataSender.controlWord.Set(ctlWord);
}
}
void JoystickLogSender::Init(wpi::log::DataLog& log, unsigned int stick,
@@ -939,17 +1002,17 @@ void JoystickLogSender::AppendPOVs(const HAL_JoystickPOVs& povs,
void DataLogSender::Init(wpi::log::DataLog& log, bool logJoysticks,
int64_t timestamp) {
m_logEnabled = wpi::log::BooleanLogEntry{log, "DS:enabled", timestamp};
m_logAutonomous = wpi::log::BooleanLogEntry{log, "DS:autonomous", timestamp};
m_logTest = wpi::log::BooleanLogEntry{log, "DS:test", timestamp};
m_logEstop = wpi::log::BooleanLogEntry{log, "DS:estop", timestamp};
m_logControlWord = wpi::log::StructLogEntry<hal::ControlWord>{
log, "DS:controlWord", timestamp};
m_logOpMode = wpi::log::StringLogEntry{log, "DS:opMode", timestamp};
// append initial control word values
HAL_GetControlWord(&m_prevControlWord);
m_logEnabled.Append(m_prevControlWord.enabled, timestamp);
m_logAutonomous.Append(m_prevControlWord.autonomous, timestamp);
m_logTest.Append(m_prevControlWord.test, timestamp);
m_logEstop.Append(m_prevControlWord.eStop, timestamp);
// append initial control word value
m_prevControlWord = hal::GetControlWord();
m_logControlWord.Append(m_prevControlWord);
// append initial opmode value
auto& inst = GetInstance();
m_logOpMode.Append(inst.OpModeToString(m_prevControlWord.GetOpModeId()));
m_logJoysticks = logJoysticks;
if (logJoysticks) {
@@ -968,21 +1031,18 @@ void DataLogSender::Send(uint64_t timestamp) {
}
// append control word value changes
HAL_ControlWord ctlWord;
HAL_GetControlWord(&ctlWord);
if (ctlWord.enabled != m_prevControlWord.enabled) {
m_logEnabled.Append(ctlWord.enabled, timestamp);
hal::ControlWord ctlWord = hal::GetControlWord();
if (ctlWord != m_prevControlWord) {
// append opmode value changes
int64_t opModeId = ctlWord.GetOpModeId();
if (opModeId != m_prevControlWord.GetOpModeId()) {
auto& inst = GetInstance();
m_logOpMode.Append(inst.OpModeToString(opModeId));
}
m_prevControlWord = ctlWord;
m_logControlWord.Append(ctlWord);
}
if (ctlWord.autonomous != m_prevControlWord.autonomous) {
m_logAutonomous.Append(ctlWord.autonomous, timestamp);
}
if (ctlWord.test != m_prevControlWord.test) {
m_logTest.Append(ctlWord.test, timestamp);
}
if (ctlWord.eStop != m_prevControlWord.eStop) {
m_logEstop.Append(ctlWord.eStop, timestamp);
}
m_prevControlWord = ctlWord;
if (m_logJoysticks) {
// append joystick value changes

51
wpilibc/src/main/native/cpp/framework/IterativeRobotBase.cpp
View File

@@ -4,9 +4,9 @@
#include "wpi/framework/IterativeRobotBase.hpp"
#include "wpi/driverstation/DSControlWord.hpp"
#include "wpi/driverstation/DriverStation.hpp"
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/nt/NetworkTableInstance.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
#include "wpi/system/Errors.hpp"
@@ -98,18 +98,10 @@ void IterativeRobotBase::LoopFunc() {
DriverStation::RefreshData();
m_watchdog.Reset();
// Get current mode
DSControlWord word;
Mode mode = Mode::kNone;
if (word.IsDisabled()) {
mode = Mode::kDisabled;
} else if (word.IsAutonomous()) {
mode = Mode::kAutonomous;
} else if (word.IsTeleop()) {
mode = Mode::kTeleop;
} else if (word.IsTest()) {
mode = Mode::kTest;
}
// Get current mode; treat disabled as unknown
wpi::hal::ControlWord word = wpi::hal::GetControlWord();
bool enabled = word.IsEnabled();
RobotMode mode = enabled ? word.GetRobotMode() : RobotMode::UNKNOWN;
if (!m_calledDsConnected && word.IsDSAttached()) {
m_calledDsConnected = true;
@@ -117,51 +109,48 @@ void IterativeRobotBase::LoopFunc() {
}
// If mode changed, call mode exit and entry functions
if (m_lastMode != mode) {
if (m_lastMode != static_cast<int>(mode)) {
// Call last mode's exit function
if (m_lastMode == Mode::kDisabled) {
if (m_lastMode == static_cast<int>(RobotMode::UNKNOWN)) {
DisabledExit();
} else if (m_lastMode == Mode::kAutonomous) {
} else if (m_lastMode == static_cast<int>(RobotMode::AUTONOMOUS)) {
AutonomousExit();
} else if (m_lastMode == Mode::kTeleop) {
} else if (m_lastMode == static_cast<int>(RobotMode::TELEOPERATED)) {
TeleopExit();
} else if (m_lastMode == Mode::kTest) {
} else if (m_lastMode == static_cast<int>(RobotMode::TEST)) {
TestExit();
}
// Call current mode's entry function
if (mode == Mode::kDisabled) {
if (mode == RobotMode::UNKNOWN) {
DisabledInit();
m_watchdog.AddEpoch("DisabledInit()");
} else if (mode == Mode::kAutonomous) {
} else if (mode == RobotMode::AUTONOMOUS) {
AutonomousInit();
m_watchdog.AddEpoch("AutonomousInit()");
} else if (mode == Mode::kTeleop) {
} else if (mode == RobotMode::TELEOPERATED) {
TeleopInit();
m_watchdog.AddEpoch("TeleopInit()");
} else if (mode == Mode::kTest) {
} else if (mode == RobotMode::TEST) {
TestInit();
m_watchdog.AddEpoch("TestInit()");
}
m_lastMode = mode;
m_lastMode = static_cast<int>(mode);
}
// Call the appropriate function depending upon the current robot mode
if (mode == Mode::kDisabled) {
HAL_ObserveUserProgramDisabled();
HAL_ObserveUserProgram(word.GetValue());
if (mode == RobotMode::UNKNOWN) {
DisabledPeriodic();
m_watchdog.AddEpoch("DisabledPeriodic()");
} else if (mode == Mode::kAutonomous) {
HAL_ObserveUserProgramAutonomous();
} else if (mode == RobotMode::AUTONOMOUS) {
AutonomousPeriodic();
m_watchdog.AddEpoch("AutonomousPeriodic()");
} else if (mode == Mode::kTeleop) {
HAL_ObserveUserProgramTeleop();
} else if (mode == RobotMode::TELEOPERATED) {
TeleopPeriodic();
m_watchdog.AddEpoch("TeleopPeriodic()");
} else if (mode == Mode::kTest) {
HAL_ObserveUserProgramTest();
} else if (mode == RobotMode::TEST) {
TestPeriodic();
m_watchdog.AddEpoch("TestPeriodic()");
}

248
wpilibc/src/main/native/cpp/framework/OpModeRobot.cpp Normal file
View File

@@ -0,0 +1,248 @@
// 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.
#include "wpi/framework/OpModeRobot.hpp"
#include <cstdint>
#include <cstdlib>
#include <memory>
#include <string>
#include <utility>
#include <fmt/format.h>
#include "wpi/driverstation/DriverStation.hpp"
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/HALBase.h"
#include "wpi/hal/Notifier.h"
#include "wpi/opmode/OpMode.hpp"
#include "wpi/util/SafeThread.hpp"
#include "wpi/util/Synchronization.h"
using namespace wpi;
namespace {
class MonitorThread : public wpi::util::SafeThreadEvent {
public:
MonitorThread(int64_t modeId, wpi::util::Event& dsEvent,
HAL_NotifierHandle notifier, std::weak_ptr<OpMode> activeOpMode)
: m_modeId{modeId},
m_dsEvent{dsEvent.GetHandle()},
m_notifier{notifier},
m_activeOpMode{std::move(activeOpMode)} {}
private:
void Main() override {
// Wait for DS to disable or change modes
WPI_EventHandle events[] = {m_dsEvent, m_stopEvent.GetHandle()};
WPI_Handle signaledBuf[2];
for (;;) {
auto signaled = wpi::util::WaitForObjects(events, signaledBuf);
if (signaled.empty()) {
return; // handles destroyed
}
for (auto signal : signaled) {
if ((signal & 0x80000000) != 0 || signal == m_stopEvent.GetHandle()) {
return; // handle destroyed or transitioned
}
}
// did the opmode or enable state change?
HAL_ControlWord word;
HAL_GetUncachedControlWord(&word);
if (!HAL_ControlWord_IsEnabled(word) ||
HAL_ControlWord_GetOpModeId(word) != m_modeId) {
break;
}
}
// call opmode stop
auto opMode = m_activeOpMode.lock();
if (opMode) {
opMode->OpModeStop();
}
events[0] = m_notifier;
int32_t status = 0;
HAL_SetNotifierAlarm(m_notifier, 1000000, 0, false, true, &status); // 1s
auto signaled = wpi::util::WaitForObjects(events, signaledBuf);
if (signaled.empty()) {
return; // handles destroyed
}
for (auto signal : signaled) {
if ((signal & 0x80000000) != 0 || signal == m_stopEvent.GetHandle()) {
return; // handle destroyed or transitioned
}
}
// if it hasn't transitioned after 1 second, terminate the program
WPILIB_ReportError(err::Error, "OpMode did not exit, terminating program");
HAL_Shutdown();
std::exit(0);
}
int64_t m_modeId;
WPI_EventHandle m_dsEvent;
HAL_NotifierHandle m_notifier;
std::weak_ptr<OpMode> m_activeOpMode;
};
} // namespace
void OpModeRobotBase::StartCompetition() {
fmt::print("********** Robot program startup complete **********\n");
HAL_ObserveUserProgramStarting();
wpi::util::Event event;
struct DSListener {
wpi::util::Event& event;
explicit DSListener(wpi::util::Event& event) : event{event} {
HAL_ProvideNewDataEventHandle(event.GetHandle());
}
~DSListener() { HAL_RemoveNewDataEventHandle(event.GetHandle()); }
} listener{event};
int32_t status = 0;
m_notifier = HAL_CreateNotifier(&status);
HAL_SetNotifierName(m_notifier, "OpModeRobot", &status);
int64_t lastModeId = -1;
bool calledDriverStationConnected = false;
std::shared_ptr<OpMode> opMode;
WPI_EventHandle events[] = {event.GetHandle(),
static_cast<WPI_EventHandle>(m_notifier)};
WPI_Handle signaledBuf[2];
for (;;) {
// Wait for new data from the driver station, with 50 ms timeout
HAL_SetNotifierAlarm(m_notifier, 50000, 0, false, true, &status);
auto signaled = wpi::util::WaitForObjects(events, signaledBuf);
if (signaled.empty()) {
return; // handles destroyed
}
for (auto signal : signaled) {
if ((signal & 0x80000000) != 0) {
return; // handle destroyed
}
}
// Get the latest control word and opmode
DriverStation::RefreshData();
hal::ControlWord ctlWord = DriverStation::GetControlWord();
if (!calledDriverStationConnected && ctlWord.IsDSAttached()) {
calledDriverStationConnected = true;
DriverStationConnected();
}
int64_t modeId;
if (!ctlWord.IsDSAttached()) {
modeId = 0;
} else {
modeId = ctlWord.GetOpModeId();
}
if (!opMode || modeId != lastModeId) {
if (opMode) {
// no or different opmode selected
opMode.reset();
}
if (modeId == 0) {
// no opmode selected
NonePeriodic();
HAL_ObserveUserProgram(ctlWord.GetValue());
continue;
}
auto data = m_opModes.lookup(modeId);
if (!data.factory) {
WPILIB_ReportError(err::Error, "No OpMode found for mode {}", modeId);
ctlWord.SetOpModeId(0);
HAL_ObserveUserProgram(ctlWord.GetValue());
continue;
}
// Instantiate the opmode
fmt::print("********** Starting OpMode {} **********\n", data.name);
opMode = data.factory();
if (!opMode) {
// could not construct
ctlWord.SetOpModeId(0);
HAL_ObserveUserProgram(ctlWord.GetValue());
continue;
}
{
std::scoped_lock lock(m_opModeMutex);
m_activeOpMode = opMode;
}
lastModeId = modeId;
// Ensure disabledPeriodic is called at least once
opMode->DisabledPeriodic();
}
HAL_ObserveUserProgram(ctlWord.GetValue());
if (ctlWord.IsEnabled()) {
// When enabled, call the opmode run function, then close and clear
wpi::util::SafeThreadOwner<MonitorThread> monitor;
monitor.Start(modeId, event, static_cast<HAL_NotifierHandle>(m_notifier),
opMode);
opMode->OpModeRun(modeId);
opMode.reset();
} else {
// When disabled, call the DisabledPeriodic function
opMode->DisabledPeriodic();
}
}
}
void OpModeRobotBase::EndCompetition() {
m_notifier = {};
std::shared_ptr<OpMode> opMode;
{
std::scoped_lock lock(m_opModeMutex);
opMode = m_activeOpMode.lock();
}
if (opMode) {
opMode->OpModeStop();
}
}
void OpModeRobotBase::AddOpModeFactory(
OpModeFactory factory, RobotMode mode, std::string_view name,
std::string_view group, std::string_view description,
const wpi::util::Color& textColor,
const wpi::util::Color& backgroundColor) {
int64_t id = DriverStation::AddOpMode(mode, name, group, description,
textColor, backgroundColor);
if (id != 0) {
m_opModes[id] = OpModeData{std::string{name}, std::move(factory)};
}
}
void OpModeRobotBase::AddOpModeFactory(OpModeFactory factory, RobotMode mode,
std::string_view name,
std::string_view group,
std::string_view description) {
int64_t id = DriverStation::AddOpMode(mode, name, group, description);
if (id != 0) {
m_opModes[id] = OpModeData{std::string{name}, std::move(factory)};
}
}
void OpModeRobotBase::RemoveOpMode(RobotMode mode, std::string_view name) {
int64_t id = DriverStation::RemoveOpMode(mode, name);
if (id != 0) {
m_opModes.erase(id);
}
}
void OpModeRobotBase::PublishOpModes() {
DriverStation::PublishOpModes();
}
void OpModeRobotBase::ClearOpModes() {
DriverStation::ClearOpModes();
m_opModes.clear();
}

33
wpilibc/src/main/native/cpp/framework/RobotState.cpp
View File

@@ -1,33 +0,0 @@
// 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.
#include "wpi/framework/RobotState.hpp"
#include "wpi/driverstation/DriverStation.hpp"
using namespace wpi;
bool RobotState::IsDisabled() {
return DriverStation::IsDisabled();
}
bool RobotState::IsEnabled() {
return DriverStation::IsEnabled();
}
bool RobotState::IsEStopped() {
return DriverStation::IsEStopped();
}
bool RobotState::IsTeleop() {
return DriverStation::IsTeleop();
}
bool RobotState::IsAutonomous() {
return DriverStation::IsAutonomous();
}
bool RobotState::IsTest() {
return DriverStation::IsTest();
}

6
wpilibc/src/main/native/cpp/hardware/motor/MotorSafety.cpp
View File

@@ -4,11 +4,11 @@
#include "wpi/hardware/motor/MotorSafety.hpp"
#include <algorithm>
#include <utility>
#include "wpi/driverstation/DriverStation.hpp"
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/system/Errors.hpp"
#include "wpi/util/SafeThread.hpp"
#include "wpi/util/SmallPtrSet.hpp"
@@ -32,9 +32,9 @@ void Thread::Main() {
bool signaled = wpi::util::WaitForObject(event.GetHandle(), 0.1, &timedOut);
if (signaled) {
HAL_ControlWord controlWord;
std::memset(&controlWord, 0, sizeof(controlWord));
HAL_GetControlWord(&controlWord);
if (!(controlWord.enabled && controlWord.dsAttached)) {
if (!HAL_ControlWord_IsEnabled(controlWord) ||
!HAL_ControlWord_IsDSAttached(controlWord)) {
safetyCounter = 0;
}
if (++safetyCounter >= 4) {

49
wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp
View File

@@ -10,55 +10,30 @@
using namespace wpi::internal;
DriverStationModeThread::DriverStationModeThread() {
DriverStationModeThread::DriverStationModeThread(wpi::hal::ControlWord word)
: m_userControlWord{word.GetValue().value} {
HAL_ProvideNewDataEventHandle(m_event.GetHandle());
m_keepAlive = true;
m_thread = std::thread{[&] { Run(); }};
m_thread = std::thread{[this] { Run(); }};
}
DriverStationModeThread::~DriverStationModeThread() {
HAL_RemoveNewDataEventHandle(m_event.GetHandle());
m_keepAlive = false;
m_event.Set();
if (m_thread.joinable()) {
m_thread.join();
}
}
void DriverStationModeThread::InDisabled(bool entering) {
m_userInDisabled = entering;
}
void DriverStationModeThread::InAutonomous(bool entering) {
m_userInAutonomous = entering;
}
void DriverStationModeThread::InTeleop(bool entering) {
m_userInTeleop = entering;
}
void DriverStationModeThread::InTest(bool entering) {
m_userInTest = entering;
}
void DriverStationModeThread::Run() {
wpi::util::Event event{false, false};
HAL_ProvideNewDataEventHandle(event.GetHandle());
while (m_keepAlive.load()) {
for (;;) {
bool timedOut = false;
wpi::util::WaitForObject(event.GetHandle(), 0.1, &timedOut);
wpi::util::WaitForObject(m_event.GetHandle(), 0.1, &timedOut);
if (!m_keepAlive) {
break;
}
wpi::DriverStation::RefreshData();
if (m_userInDisabled) {
HAL_ObserveUserProgramDisabled();
}
if (m_userInAutonomous) {
HAL_ObserveUserProgramAutonomous();
}
if (m_userInTeleop) {
HAL_ObserveUserProgramTeleop();
}
if (m_userInTest) {
HAL_ObserveUserProgramTest();
}
HAL_ObserveUserProgram({.value = m_userControlWord});
}
HAL_RemoveNewDataEventHandle(event.GetHandle());
}

21
wpilibc/src/main/native/cpp/opmode/LinearOpMode.cpp Normal file
View File

@@ -0,0 +1,21 @@
// 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.
#include "wpi/opmode/LinearOpMode.hpp"
#include "wpi/hal/DriverStation.h"
#include "wpi/internal/DriverStationModeThread.hpp"
using namespace wpi;
void LinearOpMode::OpModeRun(int64_t opModeId) {
auto word = wpi::hal::GetControlWord();
word.SetOpModeId(opModeId);
internal::DriverStationModeThread bgThread{word};
Run();
}
void LinearOpMode::OpModeStop() {
m_running = false;
}

158
wpilibc/src/main/native/cpp/opmode/PeriodicOpMode.cpp Normal file
View File

@@ -0,0 +1,158 @@
// 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.
#include "wpi/opmode/PeriodicOpMode.hpp"
#include <utility>
#include "wpi/driverstation/DriverStation.hpp"
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/UsageReporting.h"
#include "wpi/nt/NetworkTableInstance.hpp"
#include "wpi/smartdashboard/SmartDashboard.hpp"
#include "wpi/system/Errors.hpp"
#include "wpi/system/RobotController.hpp"
using namespace wpi;
PeriodicOpMode::Callback::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) {}
PeriodicOpMode::~PeriodicOpMode() {
if (m_notifier != HAL_kInvalidHandle) {
HAL_DestroyNotifier(m_notifier);
}
}
PeriodicOpMode::PeriodicOpMode(wpi::units::second_t period)
: m_period{period},
m_watchdog(period, [this] { PrintLoopOverrunMessage(); }) {
m_startTime = std::chrono::microseconds{RobotController::GetFPGATime()};
AddPeriodic([=, this] { LoopFunc(); }, period);
int32_t status = 0;
m_notifier = HAL_CreateNotifier(&status);
WPILIB_CheckErrorStatus(status, "CreateNotifier");
HAL_SetNotifierName(m_notifier, "PeriodicOpMode", &status);
HAL_ReportUsage("OpMode", "PeriodicOpMode");
}
void PeriodicOpMode::AddPeriodic(std::function<void()> callback,
wpi::units::second_t period,
wpi::units::second_t offset) {
m_callbacks.emplace(
callback, m_startTime,
std::chrono::microseconds{static_cast<int64_t>(period.value() * 1e6)},
std::chrono::microseconds{static_cast<int64_t>(offset.value() * 1e6)});
}
void PeriodicOpMode::LoopFunc() {
DriverStation::RefreshData();
HAL_ControlWord word;
HAL_GetControlWord(&word);
HAL_ControlWord_SetOpModeId(&word, m_opModeId);
HAL_ObserveUserProgram(word);
if (!DriverStation::IsEnabled() ||
DriverStation::GetOpModeId() != m_opModeId) {
m_running = false;
return;
}
m_watchdog.Reset();
Periodic();
m_watchdog.AddEpoch("Periodic()");
SmartDashboard::UpdateValues();
m_watchdog.AddEpoch("SmartDashboard::UpdateValues()");
// if constexpr (IsSimulation()) {
// HAL_SimPeriodicBefore();
// SimulationPeriodic();
// HAL_SimPeriodicAfter();
// m_watchdog.AddEpoch("SimulationPeriodic()");
// }
m_watchdog.Disable();
// Flush NetworkTables
nt::NetworkTableInstance::GetDefault().FlushLocal();
// Warn on loop time overruns
if (m_watchdog.IsExpired()) {
m_watchdog.PrintEpochs();
}
}
void PeriodicOpMode::OpModeRun(int64_t opModeId) {
m_opModeId = opModeId;
Start();
while (m_running) {
// We don't have to check there's an element in the queue first because
// there's always at least one (the constructor adds one). It's reenqueued
// at the end of the loop.
auto callback = m_callbacks.pop();
int32_t status = 0;
HAL_SetNotifierAlarm(m_notifier, callback.expirationTime.count(), 0, true,
true, &status);
WPILIB_CheckErrorStatus(status, "SetNotifierAlarm");
if (WPI_WaitForObject(m_notifier) == 0) {
break;
}
m_loopStartTimeUs = RobotController::GetFPGATime();
std::chrono::microseconds currentTime{m_loopStartTimeUs};
callback.func();
// Increment the expiration time by the number of full periods it's behind
// plus one to avoid rapid repeat fires from a large loop overrun. We assume
// currentTime ≥ expirationTime rather than checking for it since the
// callback wouldn't be running otherwise.
callback.expirationTime +=
callback.period + (currentTime - callback.expirationTime) /
callback.period * callback.period;
m_callbacks.push(std::move(callback));
// Process all other callbacks that are ready to run
while (m_callbacks.top().expirationTime <= currentTime) {
callback = m_callbacks.pop();
callback.func();
callback.expirationTime +=
callback.period + (currentTime - callback.expirationTime) /
callback.period * callback.period;
m_callbacks.push(std::move(callback));
}
}
End();
}
void PeriodicOpMode::OpModeStop() {
HAL_DestroyNotifier(m_notifier);
m_notifier = HAL_kInvalidHandle;
}
void PeriodicOpMode::PrintLoopOverrunMessage() {
WPILIB_ReportWarning("Loop time of {:.6f}s overrun", m_period.value());
}
void PeriodicOpMode::PrintWatchdogEpochs() {
m_watchdog.PrintEpochs();
}

13
wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp
View File

@@ -21,6 +21,13 @@ void wpi::sim::ConstBufferCallbackStoreThunk(const char* name, void* param,
count);
}
void wpi::sim::OpModeOptionsCallbackStoreThunk(const char* name, void* param,
const HAL_OpModeOption* opmodes,
int32_t count) {
reinterpret_cast<CallbackStore*>(param)->opModeOptionsCallback(
name, {opmodes, opmodes + count});
}
CallbackStore::CallbackStore(int32_t i, NotifyCallback cb,
CancelCallbackNoIndexFunc ccf)
: index(i), callback(std::move(cb)), cancelType(NoIndex) {
@@ -66,6 +73,12 @@ CallbackStore::CallbackStore(int32_t i, int32_t c, int32_t u,
this->cccf = ccf;
}
CallbackStore::CallbackStore(int32_t u, OpModeOptionsCallback cb,
CancelCallbackNoIndexFunc ccf)
: uid{u}, opModeOptionsCallback{std::move(cb)}, cancelType{NoIndex} {
this->ccnif = ccf;
}
CallbackStore::~CallbackStore() {
switch (cancelType) {
case Normal:

63
wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp
View File

@@ -31,38 +31,21 @@ void DriverStationSim::SetEnabled(bool enabled) {
HALSIM_SetDriverStationEnabled(enabled);
}
std::unique_ptr<CallbackStore> DriverStationSim::RegisterAutonomousCallback(
std::unique_ptr<CallbackStore> DriverStationSim::RegisterRobotModeCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
-1, callback, &HALSIM_CancelDriverStationAutonomousCallback);
store->SetUid(HALSIM_RegisterDriverStationAutonomousCallback(
-1, callback, &HALSIM_CancelDriverStationRobotModeCallback);
store->SetUid(HALSIM_RegisterDriverStationRobotModeCallback(
&CallbackStoreThunk, store.get(), initialNotify));
return store;
}
bool DriverStationSim::GetAutonomous() {
return HALSIM_GetDriverStationAutonomous();
HAL_RobotMode DriverStationSim::GetRobotMode() {
return HALSIM_GetDriverStationRobotMode();
}
void DriverStationSim::SetAutonomous(bool autonomous) {
HALSIM_SetDriverStationAutonomous(autonomous);
}
std::unique_ptr<CallbackStore> DriverStationSim::RegisterTestCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
-1, callback, &HALSIM_CancelDriverStationTestCallback);
store->SetUid(HALSIM_RegisterDriverStationTestCallback(
&CallbackStoreThunk, store.get(), initialNotify));
return store;
}
bool DriverStationSim::GetTest() {
return HALSIM_GetDriverStationTest();
}
void DriverStationSim::SetTest(bool test) {
HALSIM_SetDriverStationTest(test);
void DriverStationSim::SetRobotMode(HAL_RobotMode robotMode) {
HALSIM_SetDriverStationRobotMode(robotMode);
}
std::unique_ptr<CallbackStore> DriverStationSim::RegisterEStopCallback(
@@ -152,6 +135,38 @@ void DriverStationSim::SetMatchTime(double matchTime) {
HALSIM_SetDriverStationMatchTime(matchTime);
}
std::unique_ptr<CallbackStore> DriverStationSim::RegisterOpModeCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
-1, callback, &HALSIM_CancelDriverStationOpModeCallback);
store->SetUid(HALSIM_RegisterDriverStationOpModeCallback(
&CallbackStoreThunk, store.get(), initialNotify));
return store;
}
int64_t DriverStationSim::GetOpMode() {
return HALSIM_GetDriverStationOpMode();
}
void DriverStationSim::SetOpMode(int64_t opmode) {
HALSIM_SetDriverStationOpMode(opmode);
}
std::unique_ptr<CallbackStore> DriverStationSim::RegisterOpModeOptionsCallback(
OpModeOptionsCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
-1, callback, &HALSIM_CancelOpModeOptionsCallback);
store->SetUid(HALSIM_RegisterOpModeOptionsCallback(
&OpModeOptionsCallbackStoreThunk, store.get(), initialNotify));
return store;
}
OpModeOptions DriverStationSim::GetOpModeOptions() {
int32_t len;
auto options = HALSIM_GetOpModeOptions(&len);
return {options, len};
}
void DriverStationSim::NotifyNewData() {
wpi::util::Event waitEvent{true};
HAL_ProvideNewDataEventHandle(waitEvent.GetHandle());

8
wpilibc/src/main/native/cpp/simulation/SimHooks.cpp
View File

@@ -24,6 +24,14 @@ bool GetProgramStarted() {
return HALSIM_GetProgramStarted();
}
void SetProgramState(wpi::hal::ControlWord controlWord) {
wpi::hal::sim::SetProgramState(controlWord);
}
wpi::hal::ControlWord GetProgramState() {
return wpi::hal::sim::GetProgramState();
}
void RestartTiming() {
HALSIM_RestartTiming();
}

26
wpilibc/src/main/native/cppcs/RobotBase.cpp
View File

@@ -4,6 +4,8 @@
#include "wpi/framework/RobotBase.hpp"
#include <stdint.h>
#ifdef __FRC_SYSTEMCORE__
#include <dlfcn.h>
#endif
@@ -140,38 +142,46 @@ static void SetupMathShared() {
std::make_unique<WPILibMathShared>());
}
bool RobotBase::IsEnabled() const {
bool RobotBase::IsEnabled() {
return DriverStation::IsEnabled();
}
bool RobotBase::IsDisabled() const {
bool RobotBase::IsDisabled() {
return DriverStation::IsDisabled();
}
bool RobotBase::IsAutonomous() const {
bool RobotBase::IsAutonomous() {
return DriverStation::IsAutonomous();
}
bool RobotBase::IsAutonomousEnabled() const {
bool RobotBase::IsAutonomousEnabled() {
return DriverStation::IsAutonomousEnabled();
}
bool RobotBase::IsTeleop() const {
bool RobotBase::IsTeleop() {
return DriverStation::IsTeleop();
}
bool RobotBase::IsTeleopEnabled() const {
bool RobotBase::IsTeleopEnabled() {
return DriverStation::IsTeleopEnabled();
}
bool RobotBase::IsTest() const {
bool RobotBase::IsTest() {
return DriverStation::IsTest();
}
bool RobotBase::IsTestEnabled() const {
bool RobotBase::IsTestEnabled() {
return DriverStation::IsTestEnabled();
}
int64_t RobotBase::GetOpModeId() {
return DriverStation::GetOpModeId();
}
std::string RobotBase::GetOpMode() {
return DriverStation::GetOpMode();
}
std::thread::id RobotBase::GetThreadId() {
return m_threadId;
}

102
wpilibc/src/main/native/include/wpi/driverstation/DSControlWord.hpp
View File

@@ -1,102 +0,0 @@
// 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/hal/DriverStationTypes.h"
namespace wpi {
/**
* A wrapper around Driver Station control word.
*/
class DSControlWord {
public:
/**
* DSControlWord constructor.
*
* Upon construction, the current Driver Station control word is read and
* stored internally.
*/
DSControlWord();
/**
* Check if the DS has enabled the robot.
*
* @return True if the robot is enabled and the DS is connected
*/
bool IsEnabled() const;
/**
* Check if the robot is disabled.
*
* @return True if the robot is explicitly disabled or the DS is not connected
*/
bool IsDisabled() const;
/**
* Check if the robot is e-stopped.
*
* @return True if the robot is e-stopped
*/
bool IsEStopped() const;
/**
* Check if the DS is commanding autonomous mode.
*
* @return True if the robot is being commanded to be in autonomous mode
*/
bool IsAutonomous() const;
/**
* Check if the DS is commanding autonomous mode and if it has enabled the
* robot.
*
* @return True if the robot is being commanded to be in autonomous mode and
* enabled.
*/
bool IsAutonomousEnabled() const;
/**
* Check if the DS is commanding teleop mode.
*
* @return True if the robot is being commanded to be in teleop mode
*/
bool IsTeleop() const;
/**
* Check if the DS is commanding teleop mode and if it has enabled the robot.
*
* @return True if the robot is being commanded to be in teleop mode and
* enabled.
*/
bool IsTeleopEnabled() const;
/**
* Check if the DS is commanding test mode.
*
* @return True if the robot is being commanded to be in test mode
*/
bool IsTest() const;
/**
* Check if the DS is attached.
*
* @return True if the DS is connected to the robot
*/
bool IsDSAttached() const;
/**
* Is the driver station attached to a Field Management System?
*
* @return True if the robot is competing on a field being controlled by a
* Field Management System
*/
bool IsFMSAttached() const;
private:
HAL_ControlWord m_controlWord;
};
} // namespace wpi

150
wpilibc/src/main/native/include/wpi/driverstation/DriverStation.hpp
View File

@@ -6,7 +6,9 @@
#include <optional>
#include <string>
#include <string_view>
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/math/geometry/Rotation2d.hpp"
#include "wpi/units/time.hpp"
@@ -16,8 +18,14 @@ namespace wpi::log {
class DataLog;
} // namespace wpi::log
namespace wpi::util {
class Color;
} // namespace wpi::util
namespace wpi {
using wpi::hal::RobotMode;
/**
* Provide access to the network communication data to / from the Driver
* Station.
@@ -313,28 +321,41 @@ class DriverStation final {
*
* @return True if the robot is enabled and the DS is connected
*/
static bool IsEnabled();
static bool IsEnabled() {
hal::ControlWord controlWord = GetControlWord();
return controlWord.IsEnabled() && controlWord.IsDSAttached();
}
/**
* Check if the robot is disabled.
*
* @return True if the robot is explicitly disabled or the DS is not connected
*/
static bool IsDisabled();
static bool IsDisabled() { return !IsEnabled(); }
/**
* Check if the robot is e-stopped.
*
* @return True if the robot is e-stopped
*/
static bool IsEStopped();
static bool IsEStopped() { return GetControlWord().IsEStopped(); }
/**
* Gets the current robot mode.
*
* <p>Note that this does not indicate whether the robot is enabled or
* disabled.
*
* @return robot mode
*/
static RobotMode GetRobotMode() { return GetControlWord().GetRobotMode(); }
/**
* Check if the DS is commanding autonomous mode.
*
* @return True if the robot is being commanded to be in autonomous mode
*/
static bool IsAutonomous();
static bool IsAutonomous() { return GetControlWord().IsAutonomous(); }
/**
* Check if the DS is commanding autonomous mode and if it has enabled the
@@ -343,14 +364,16 @@ class DriverStation final {
* @return True if the robot is being commanded to be in autonomous mode and
* enabled.
*/
static bool IsAutonomousEnabled();
static bool IsAutonomousEnabled() {
return GetControlWord().IsAutonomousEnabled();
}
/**
* Check if the DS is commanding teleop mode.
*
* @return True if the robot is being commanded to be in teleop mode
*/
static bool IsTeleop();
static bool IsTeleop() { return GetControlWord().IsTeleop(); }
/**
* Check if the DS is commanding teleop mode and if it has enabled the robot.
@@ -358,14 +381,14 @@ class DriverStation final {
* @return True if the robot is being commanded to be in teleop mode and
* enabled.
*/
static bool IsTeleopEnabled();
static bool IsTeleopEnabled() { return GetControlWord().IsTeleopEnabled(); }
/**
* Check if the DS is commanding test mode.
*
* @return True if the robot is being commanded to be in test mode
*/
static bool IsTest();
static bool IsTest() { return GetControlWord().IsTest(); }
/**
* Check if the DS is commanding Test mode and if it has enabled the robot.
@@ -373,14 +396,112 @@ class DriverStation final {
* @return True if the robot is being commanded to be in Test mode and
* enabled.
*/
static bool IsTestEnabled();
static bool IsTestEnabled() { return GetControlWord().IsTestEnabled(); }
/**
* Adds an operating mode option. It's necessary to call PublishOpModes() to
* make the added modes visible to the driver station.
*
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @param description description of the operating mode
* @param textColor text color
* @param backgroundColor background color
* @return unique ID used to later identify the operating mode; if a blank
* name is passed, 0 is returned; identical names for the same robot
* mode result in a 0 return value
*/
static int64_t AddOpMode(RobotMode mode, std::string_view name,
std::string_view group, std::string_view description,
const wpi::util::Color& textColor,
const wpi::util::Color& backgroundColor);
/**
* Adds an operating mode option. It's necessary to call PublishOpModes() to
* make the added modes visible to the driver station.
*
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @param description description of the operating mode
* @return unique ID used to later identify the operating mode; if a blank
* name is passed, 0 is returned; identical names for the same robot
* mode result in a 0 return value
*/
static int64_t AddOpMode(RobotMode mode, std::string_view name,
std::string_view group = {},
std::string_view description = {});
/**
* Removes an operating mode option. It's necessary to call PublishOpModes()
* to make the removed mode no longer visible to the driver station.
*
* @param mode robot mode
* @param name name of the operating mode
* @return unique ID for the opmode, or 0 if not found
*/
static int64_t RemoveOpMode(RobotMode mode, std::string_view name);
/**
* Publishes the operating mode options to the driver station.
*/
static void PublishOpModes();
/**
* Clears all operating mode options and publishes an empty list to the driver
* station.
*/
static void ClearOpModes();
/**
* Gets the operating mode selected on the driver station. Note this does not
* mean the robot is enabled; use IsEnabled() for that. In a match, this will
* indicate the operating mode selected for auto before the match starts
* (i.e., while the robot is disabled in auto mode); after the auto period
* ends, this will change to reflect the operating mode selected for teleop.
*
* @return the unique ID provided by the AddOpMode() function; may return 0 or
* a unique ID not added, so callers should be prepared to handle that case
*/
static int64_t GetOpModeId() { return GetControlWord().GetOpModeId(); }
/**
* Gets the operating mode selected on the driver station. Note this does not
* mean the robot is enabled; use IsEnabled() for that. In a match, this will
* indicate the operating mode selected for auto before the match starts
* (i.e., while the robot is disabled in auto mode); after the auto period
* ends, this will change to reflect the operating mode selected for teleop.
*
* @return Operating mode string; may return a string not in the list of
* options, so callers should be prepared to handle that case
*/
static std::string GetOpMode();
/**
* Check to see if the selected operating mode is a particular value. Note
* this does not mean the robot is enabled; use IsEnabled() for that.
*
* @param id operating mode unique ID
* @return True if that mode is the current mode
*/
static bool IsOpMode(int64_t id) { return GetOpModeId() == id; }
/**
* Check to see if the selected operating mode is a particular value. Note
* this does not mean the robot is enabled; use IsEnabled() for that.
*
* @param mode operating mode
* @return True if that mode is the current mode
*/
static bool IsOpMode(std::string_view mode) { return GetOpMode() == mode; }
/**
* Check if the DS is attached.
*
* @return True if the DS is connected to the robot
*/
static bool IsDSAttached();
static bool IsDSAttached() { return GetControlWord().IsDSAttached(); }
/**
* Is the driver station attached to a Field Management System?
@@ -388,7 +509,7 @@ class DriverStation final {
* @return True if the robot is competing on a field being controlled by a
* Field Management System
*/
static bool IsFMSAttached();
static bool IsFMSAttached() { return GetControlWord().IsFMSAttached(); }
/**
* Returns the game specific message provided by the FMS.
@@ -482,6 +603,13 @@ class DriverStation final {
*/
static double GetBatteryVoltage();
/**
* Get the current control word.
*
* @return control word
*/
static hal::ControlWord GetControlWord() { return hal::GetControlWord(); }
/**
* Copy data from the DS task for the user. If no new data exists, it will
* just be returned, otherwise the data will be copied from the DS polling

4
wpilibc/src/main/native/include/wpi/framework/IterativeRobotBase.hpp
View File

@@ -226,9 +226,7 @@ class IterativeRobotBase : public RobotBase {
void LoopFunc();
private:
enum class Mode { kNone, kDisabled, kAutonomous, kTeleop, kTest };
Mode m_lastMode = Mode::kNone;
int m_lastMode = -1;
wpi::units::second_t m_period;
Watchdog m_watchdog;
bool m_ntFlushEnabled = true;

234
wpilibc/src/main/native/include/wpi/framework/OpModeRobot.hpp Normal file
View File

@@ -0,0 +1,234 @@
// 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 <concepts>
#include <cstdint>
#include <functional>
#include <memory>
#include <string>
#include "wpi/framework/RobotBase.hpp"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/Notifier.h"
#include "wpi/opmode/OpMode.hpp"
#include "wpi/util/DenseMap.hpp"
#include "wpi/util/mutex.hpp"
namespace wpi::util {
class Color;
} // namespace wpi::util
namespace wpi {
using RobotMode = wpi::hal::RobotMode;
namespace detail {
template <typename T>
concept OpModeDerived = std::derived_from<T, OpMode>;
template <typename T>
concept NoArgOpMode = std::constructible_from<T> && OpModeDerived<T>;
template <typename T, typename R>
concept OneArgOpMode = std::constructible_from<T, R&> && OpModeDerived<T>;
} // namespace detail
/**
* Concept indicating a class is derived from OpMode and has either a
* no-argument constructor or a constructorthat accepts R&.
*
* @tparam T opmode class
* @tparam R robot class
*/
template <typename T, typename R>
concept ConstructibleOpMode =
detail::NoArgOpMode<T> || detail::OneArgOpMode<T, R>;
/**
* OpModeRobotBase is the non-templated base class for OpModeRobot. Users should
* generally prefer using OpModeRobot instead of this class.
*
* Opmodes are constructed when selected on the driver station, and destroyed
* when the robot is disabled after being enabled or a different opmode is
* selected. When no opmode is selected, NonePeriodic() is called. The
* DriverStationConnected() function is called the first time the driver station
* connects to the robot.
*/
class OpModeRobotBase : public RobotBase {
public:
using OpModeFactory = std::function<std::unique_ptr<OpMode>()>;
/**
* Provide an alternate "main loop" via StartCompetition().
*/
void StartCompetition() override;
/**
* Ends the main loop in StartCompetition().
*/
void EndCompetition() override;
/**
* Constructor.
*/
OpModeRobotBase() = default;
OpModeRobotBase(OpModeRobotBase&&) = delete;
OpModeRobotBase& operator=(OpModeRobotBase&&) = delete;
/**
* Function called exactly once after the DS is connected.
*
* 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() {}
/**
* Function called periodically anytime when no opmode is selected, including
* when the Driver Station is disconnected.
*/
virtual void NonePeriodic() {}
/**
* Adds an operating mode option using a factory function that creates the
* opmode. It's necessary to call PublishOpModes() to make the added modes
* visible to the driver station.
*
* @param factory factory function
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @param description description of the operating mode
* @param textColor text color
* @param backgroundColor background color
*/
void AddOpModeFactory(OpModeFactory factory, RobotMode mode,
std::string_view name, std::string_view group,
std::string_view description,
const wpi::util::Color& textColor,
const wpi::util::Color& backgroundColor);
/**
* Adds an operating mode option using a factory function that creates the
* opmode. It's necessary to call PublishOpModes() to make the added modes
* visible to the driver station.
*
* @param factory factory function
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @param description description of the operating mode
*/
void AddOpModeFactory(OpModeFactory factory, RobotMode mode,
std::string_view name, std::string_view group = {},
std::string_view description = {});
/**
* Removes an operating mode option. It's necessary to call PublishOpModes()
* to make the removed mode no longer visible to the driver station.
*
* @param mode robot mode
* @param name name of the operating mode
*/
void RemoveOpMode(RobotMode mode, std::string_view name);
/**
* Publishes the operating mode options to the driver station.
*/
void PublishOpModes();
/**
* Clears all operating mode options and publishes an empty list to the driver
* station.
*/
void ClearOpModes();
private:
struct OpModeData {
std::string name;
OpModeFactory factory;
};
wpi::util::DenseMap<int64_t, OpModeData> m_opModes;
wpi::hal::Handle<HAL_NotifierHandle, HAL_DestroyNotifier> m_notifier;
wpi::util::mutex m_opModeMutex;
std::weak_ptr<OpMode> m_activeOpMode;
};
/**
* OpModeRobot implements the opmode-based robot program framework.
*
* The OpModeRobot class is intended to be subclassed by a user creating a robot
* program. Users must provide their derived class as a template parameter to
* this class.
*
* Opmodes are constructed when selected on the driver station, and destroyed
* when the robot is disabled after being enabled or a different opmode is
* selected. When no opmode is selected, NonePeriodic() is called. The
* DriverStationConnected() function is called the first time the driver station
* connects to the robot.
*
* @tparam Derived derived class
*/
template <typename Derived>
class OpModeRobot : public OpModeRobotBase {
public:
/**
* Adds an operating mode option. It's necessary to call PublishOpModes() to
* make the added modes visible to the driver station.
*
* @tparam T opmode class; must be a public, non-abstract subclass of OpMode
* with a public constructor that either takes no arguments or accepts a
* single argument of this class's type (the latter is preferred).
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @param description description of the operating mode
* @param textColor text color
* @param backgroundColor background color
*/
template <ConstructibleOpMode<Derived> T>
void AddOpMode(RobotMode mode, std::string_view name, std::string_view group,
std::string_view description,
const wpi::util::Color& textColor,
const wpi::util::Color& backgroundColor) {
if constexpr (detail::OneArgOpMode<T, Derived>) {
AddOpModeFactory(
[this] { return std::make_unique<T>(*static_cast<Derived*>(this)); },
mode, name, group, description, textColor, backgroundColor);
} else if constexpr (detail::NoArgOpMode<T>) {
AddOpModeFactory([] { return std::make_unique<T>(); }, mode, name, group,
description, textColor, backgroundColor);
}
}
/**
* Adds an operating mode option. It's necessary to call PublishOpModes() to
* make the added modes visible to the driver station.
*
* @tparam T opmode class; must be a public, non-abstract subclass of OpMode
* with a public constructor that either takes no arguments or accepts a
* single argument of this class's type (the latter is preferred).
* @param mode robot mode
* @param name name of the operating mode
* @param group group of the operating mode
* @param description description of the operating mode
*/
template <ConstructibleOpMode<Derived> T>
void AddOpMode(RobotMode mode, std::string_view name,
std::string_view group = {},
std::string_view description = {}) {
if constexpr (detail::OneArgOpMode<T, Derived>) {
AddOpModeFactory(
[this] { return std::make_unique<T>(*static_cast<Derived*>(this)); },
mode, name, group, description);
} else if constexpr (detail::NoArgOpMode<T>) {
AddOpModeFactory([] { return std::make_unique<T>(); }, mode, name, group,
description);
}
}
};
} // namespace wpi

36
wpilibc/src/main/native/include/wpi/framework/RobotBase.hpp
View File

@@ -5,6 +5,7 @@
#pragma once
#include <chrono>
#include <string>
#include <thread>
#include "wpi/hal/DriverStation.h"
@@ -148,14 +149,14 @@ class RobotBase {
*
* @return True if the Robot is currently enabled by the Driver Station.
*/
bool IsEnabled() const;
static bool IsEnabled();
/**
* Determine if the Robot is currently disabled.
*
* @return True if the Robot is currently disabled by the Driver Station.
*/
bool IsDisabled() const;
static bool IsDisabled();
/**
* Determine if the robot is currently in Autonomous mode.
@@ -163,7 +164,7 @@ class RobotBase {
* @return True if the robot is currently operating Autonomously as determined
* by the Driver Station.
*/
bool IsAutonomous() const;
static bool IsAutonomous();
/**
* Determine if the robot is currently in Autonomous mode and enabled.
@@ -171,7 +172,7 @@ class RobotBase {
* @return True if the robot us currently operating Autonomously while enabled
* as determined by the Driver Station.
*/
bool IsAutonomousEnabled() const;
static bool IsAutonomousEnabled();
/**
* Determine if the robot is currently in Operator Control mode.
@@ -179,7 +180,7 @@ class RobotBase {
* @return True if the robot is currently operating in Tele-Op mode as
* determined by the Driver Station.
*/
bool IsTeleop() const;
static bool IsTeleop();
/**
* Determine if the robot is current in Operator Control mode and enabled.
@@ -187,7 +188,7 @@ class RobotBase {
* @return True if the robot is currently operating in Tele-Op mode while
* enabled as determined by the Driver Station.
*/
bool IsTeleopEnabled() const;
static bool IsTeleopEnabled();
/**
* Determine if the robot is currently in Test mode.
@@ -195,7 +196,7 @@ class RobotBase {
* @return True if the robot is currently running in Test mode as determined
* by the Driver Station.
*/
bool IsTest() const;
static bool IsTest();
/**
* Determine if the robot is current in Test mode and enabled.
@@ -203,7 +204,26 @@ class RobotBase {
* @return True if the robot is currently operating in Test mode while
* enabled as determined by the Driver Station.
*/
bool IsTestEnabled() const;
static bool IsTestEnabled();
/**
* Gets the currently selected operating mode of the driver station. Note this
* does not mean the robot is enabled; use IsEnabled() for that.
*
* @return the unique ID provided by the DriverStation::AddOpMode() function;
* may return 0 or a unique ID not added, so callers should be prepared to
* handle that case
*/
static int64_t GetOpModeId();
/**
* Gets the currently selected operating mode of the driver station. Note this
* does not mean the robot is enabled; use IsEnabled() for that.
*
* @return Operating mode string; may return a string not in the list of
* options, so callers should be prepared to handle that case
*/
static std::string GetOpMode();
/**
* Returns the main thread ID.

59
wpilibc/src/main/native/include/wpi/framework/RobotState.hpp
View File

@@ -1,59 +0,0 @@
// 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

46
wpilibc/src/main/native/include/wpi/internal/DriverStationModeThread.hpp
View File

@@ -7,6 +7,9 @@
#include <atomic>
#include <thread>
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/util/Synchronization.h"
namespace wpi::internal {
/**
* For internal use only.
@@ -15,8 +18,10 @@ class DriverStationModeThread {
public:
/**
* For internal use only.
*
* @param word initial control word
*/
DriverStationModeThread();
explicit DriverStationModeThread(wpi::hal::ControlWord word);
~DriverStationModeThread();
@@ -30,44 +35,17 @@ class DriverStationModeThread {
* Only to be used to tell the Driver Station what code you claim to be
* executing for diagnostic purposes only.
*
* @param entering If true, starting disabled code; if false, leaving disabled
* code
* @param word control word
*/
void InDisabled(bool entering);
/**
* Only to be used to tell the Driver Station what code you claim to be
* executing for diagnostic purposes only.
*
* @param entering If true, starting autonomous code; if false, leaving
* autonomous code
*/
void InAutonomous(bool entering);
/**
* Only to be used to tell the Driver Station what code you claim to be
* executing for diagnostic purposes only.
*
* @param entering If true, starting teleop code; if false, leaving teleop
* code
*/
void InTeleop(bool entering);
/**
* Only to be used to tell the Driver Station what code you claim to be
* executing for diagnostic purposes only.
*
* @param entering If true, starting test code; if false, leaving test code
*/
void InTest(bool entering);
void InControl(wpi::hal::ControlWord word) {
m_userControlWord = word.GetValue().value;
}
private:
std::atomic_bool m_keepAlive{false};
wpi::util::Event m_event{false, false};
std::thread m_thread;
void Run();
bool m_userInDisabled{false};
bool m_userInAutonomous{false};
bool m_userInTeleop{false};
bool m_userInTest{false};
std::atomic<int64_t> m_userControlWord;
};
} // namespace wpi::internal

68
wpilibc/src/main/native/include/wpi/opmode/LinearOpMode.hpp Normal file
View File

@@ -0,0 +1,68 @@
// 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 <stdint.h>
#include <atomic>
#include "wpi/opmode/OpMode.hpp"
namespace wpi {
/**
* An opmode structure for "linear" operation. The user is responsible for
* implementing any looping behavior; after Run() returns it will not be called
* again on the same object.
*
* Lifecycle:
*
* - Constructed when opmode selected on driver station
*
* - DisabledPeriodic() called periodically as long as DS is disabled
*
* - When DS transitions from disabled to enabled, Run() is called exactly once
*
* - When DS transitions from enabled to disabled, or a different opmode is
* selected on the driver station, object is destroyed and not reused
*
* The user is responsible for exiting Run() when the opmode is directed to stop
* executing. This is indicated by IsRunning() returning false. All other
* methods should be written to return as quickly as possible when IsRunning()
* returns false.
*/
class LinearOpMode : public OpMode {
public:
/**
* Called periodically while the opmode is selected on the DS and the robot is
* disabled.
*/
void DisabledPeriodic() override {}
/**
* Called once when the robot is enabled. When it returns, it will not be
* called again on the same object.
*/
virtual void Run() = 0;
/**
* Returns true while this opmode is selected (regardless of enable state).
* All other functions should be written to return as quickly as possible when
* this returns false.
*
* @return True if opmode selected, false otherwise
*/
bool IsRunning() const { return m_running; }
// implements OpMode interface
void OpModeRun(int64_t opModeId) final;
void OpModeStop() final;
private:
std::atomic_bool m_running{true};
};
} // namespace wpi

45
wpilibc/src/main/native/include/wpi/opmode/OpMode.hpp Normal file
View File

@@ -0,0 +1,45 @@
// 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 <stdint.h>
namespace wpi {
/**
* Top-level interface for opmode classes. Users should generally extend one of
* the abstract implementations of this interface (e.g. PeriodicOpMode or
* LinearOpMode) rather than directly implementing this interface.
*/
class OpMode {
public:
/**
* The object is destroyed when the opmode is no longer selected on the DS or
* after OpModeRun() returns.
*/
virtual ~OpMode() = default;
/**
* This function is called periodically while the opmode is selected on the DS
* (robot is disabled). Code that should only run once when the opmode is
* selected should go in the opmode constructor.
*/
virtual void DisabledPeriodic() {}
/**
* This function is called when the opmode starts (robot is enabled).
*
* @param opModeId opmode unique ID
*/
virtual void OpModeRun(int64_t opModeId) = 0;
/**
* This function is called asynchronously when the robot is disabled, to
* request the opmode return from OpModeRun().
*/
virtual void OpModeStop() = 0;
};
} // namespace wpi

178
wpilibc/src/main/native/include/wpi/opmode/PeriodicOpMode.hpp Normal file
View File

@@ -0,0 +1,178 @@
// 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 <stdint.h>
#include <chrono>
#include <functional>
#include <vector>
#include "wpi/hal/Notifier.h"
#include "wpi/hal/Types.h"
#include "wpi/opmode/OpMode.hpp"
#include "wpi/system/Watchdog.hpp"
#include "wpi/units/time.hpp"
#include "wpi/util/priority_queue.hpp"
namespace wpi {
/**
* An opmode structure for periodic operation. This base class implements a loop
* that runs one or more functions periodically (on a set time interval aka loop
* period). The primary periodic callback function is the Periodic() function;
* the time interval for this callback is 20 ms by default, but may be changed
* via passing a different time interval to the constructor. Additional periodic
* callbacks with different intervals can be added using the AddPeriodic() set
* of functions.
*
* Lifecycle:
*
* - Constructed when opmode selected on driver station
*
* - DisabledPeriodic() called periodically as long as DS is disabled. Note
* this is not called on a set time interval (it does not use the same time
* interval as Periodic())
*
* - When DS transitions from disabled to enabled, Start() is called once
*
* - While DS is enabled, Periodic() is called periodically on the time interval
* set by the constructor, and additional periodic callbacks added via
* AddPeriodic() are called periodically on their set time intervals
*
* - When DS transitions from enabled to disabled, or a different opmode is
* selected on the driver station when the DS is enabled, End() is called,
* followed by the object being destroyed; the object is not reused
*
* - If a different opmode is selected on the driver station when the DS is
* disabled, the object is destroyed (without End() being called); the object
* is not reused
*/
class PeriodicOpMode : public OpMode {
public:
/** Default loop period. */
static constexpr auto kDefaultPeriod = 20_ms;
protected:
/**
* Constructor. Periodic opmodes may specify the period used for the
* Periodic() function.
*
* @param period period for callbacks to the Periodic() function
*/
explicit PeriodicOpMode(wpi::units::second_t period = kDefaultPeriod);
public:
~PeriodicOpMode() override;
/**
* Called periodically while the opmode is selected on the DS (robot is
* disabled).
*/
void DisabledPeriodic() override {}
/**
* Called a single time when the robot transitions from disabled to enabled.
* This is called prior to Periodic() being called.
*/
virtual void Start() {}
/** Called periodically while the robot is enabled. */
virtual void Periodic() = 0;
/**
* Called a single time when the robot transitions from enabled to disabled,
* or just before the destructor is called if a different opmode is selected
* while the robot is enabled.
*/
virtual void End() {}
/**
* Return the system clock time in microseconds 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.
*/
int64_t GetLoopStartTime() const { return m_loopStartTimeUs; }
/**
* Add a callback to run at a specific period with a starting time offset.
*
* This is scheduled on the same Notifier as Periodic(), so Periodic() 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);
/**
* Gets time period between calls to Periodic() functions.
*/
wpi::units::second_t GetPeriod() const { return m_period; }
/**
* Prints list of epochs added so far and their times.
*/
void PrintWatchdogEpochs();
protected:
/** Loop function. */
void LoopFunc();
public:
// implements OpMode interface
void OpModeRun(int64_t opModeId) final;
void OpModeStop() final;
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);
bool operator>(const Callback& rhs) const {
return expirationTime > rhs.expirationTime;
}
};
int64_t m_opModeId;
bool m_running = true;
wpi::hal::Handle<HAL_NotifierHandle, HAL_DestroyNotifier> m_notifier;
std::chrono::microseconds m_startTime;
int64_t m_loopStartTimeUs = 0;
wpi::units::second_t m_period;
Watchdog m_watchdog;
wpi::util::priority_queue<Callback, std::vector<Callback>,
std::greater<Callback>>
m_callbacks;
void PrintLoopOverrunMessage();
};
} // namespace wpi

15
wpilibc/src/main/native/include/wpi/simulation/CallbackStore.hpp
View File

@@ -5,8 +5,10 @@
#pragma once
#include <functional>
#include <span>
#include <string_view>
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/Value.h"
namespace wpi::sim {
@@ -14,6 +16,8 @@ namespace wpi::sim {
using NotifyCallback = std::function<void(std::string_view, const HAL_Value*)>;
using ConstBufferCallback = std::function<void(
std::string_view, const unsigned char* buffer, unsigned int count)>;
using OpModeOptionsCallback =
std::function<void(std::string_view, std::span<const HAL_OpModeOption>)>;
using CancelCallbackFunc = void (*)(int32_t index, int32_t uid);
using CancelCallbackNoIndexFunc = void (*)(int32_t uid);
using CancelCallbackChannelFunc = void (*)(int32_t index, int32_t channel,
@@ -23,6 +27,9 @@ void CallbackStoreThunk(const char* name, void* param, const HAL_Value* value);
void ConstBufferCallbackStoreThunk(const char* name, void* param,
const unsigned char* buffer,
unsigned int count);
void OpModeOptionsCallbackStoreThunk(const char* name, void* param,
const HAL_OpModeOption* opmodes,
int32_t count);
/**
* Manages simulation callbacks; each object is associated with a callback.
@@ -46,6 +53,9 @@ class CallbackStore {
CallbackStore(int32_t i, int32_t c, int32_t u, ConstBufferCallback cb,
CancelCallbackChannelFunc ccf);
CallbackStore(int32_t u, OpModeOptionsCallback cb,
CancelCallbackNoIndexFunc ccf);
CallbackStore(const CallbackStore&) = delete;
CallbackStore& operator=(const CallbackStore&) = delete;
@@ -60,6 +70,10 @@ class CallbackStore {
const unsigned char* buffer,
unsigned int count);
friend void OpModeOptionsCallbackStoreThunk(const char* name, void* param,
const HAL_OpModeOption* opmodes,
int32_t count);
private:
int32_t index;
int32_t channel;
@@ -67,6 +81,7 @@ class CallbackStore {
NotifyCallback callback;
ConstBufferCallback constBufferCallback;
OpModeOptionsCallback opModeOptionsCallback;
union {
CancelCallbackFunc ccf;
CancelCallbackChannelFunc cccf;

121
wpilibc/src/main/native/include/wpi/simulation/DriverStationSim.hpp
View File

@@ -4,14 +4,46 @@
#pragma once
#include <stdint.h>
#include <memory>
#include "wpi/driverstation/DriverStation.hpp"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/simulation/DriverStationData.h"
#include "wpi/simulation/CallbackStore.hpp"
namespace wpi::sim {
class OpModeOptions : public std::span<HAL_OpModeOption> {
public:
OpModeOptions() = default;
OpModeOptions(HAL_OpModeOption* options, int32_t len)
: span{options, options + len} {}
OpModeOptions(const OpModeOptions&) = delete;
OpModeOptions(OpModeOptions&& oth) : span{oth} {
static_cast<span&>(oth) = {};
}
OpModeOptions& operator=(const OpModeOptions&) = delete;
OpModeOptions& operator=(OpModeOptions&& oth) {
if (data()) {
HALSIM_FreeOpModeOptionsArray(data(), size());
}
static_cast<span&>(*this) = oth;
static_cast<span&>(oth) = {};
return *this;
}
~OpModeOptions() {
if (data()) {
HALSIM_FreeOpModeOptionsArray(data(), size());
}
}
};
/**
* Class to control a simulated driver station.
*/
@@ -44,56 +76,29 @@ class DriverStationSim {
static void SetEnabled(bool enabled);
/**
* Register a callback on whether the DS is in autonomous mode.
* Register a callback on DS robot mode changes.
*
* @param callback the callback that will be called on autonomous mode
* entrance/exit
* @param callback the callback that will be called when robot mode changes
* @param initialNotify if true, the callback will be run on the initial value
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]]
static std::unique_ptr<CallbackStore> RegisterAutonomousCallback(
static std::unique_ptr<CallbackStore> RegisterRobotModeCallback(
NotifyCallback callback, bool initialNotify);
/**
* Check if the DS is in autonomous.
* Get the robot mode set by the DS.
*
* @return true if autonomous
* @return robot mode
*/
static bool GetAutonomous();
static HAL_RobotMode GetRobotMode();
/**
* Change whether the DS is in autonomous.
* Change the robot mode set by the DS.
*
* @param autonomous the new value
* @param robotMode the new value
*/
static void SetAutonomous(bool autonomous);
/**
* Register a callback on whether the DS is in test mode.
*
* @param callback the callback that will be called whenever the test mode
* is entered or left
* @param initialNotify if true, the callback will be run on the initial value
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]]
static std::unique_ptr<CallbackStore> RegisterTestCallback(
NotifyCallback callback, bool initialNotify);
/**
* Check if the DS is in test.
*
* @return true if test
*/
static bool GetTest();
/**
* Change whether the DS is in test.
*
* @param test the new value
*/
static void SetTest(bool test);
static void SetRobotMode(HAL_RobotMode robotMode);
/**
* Register a callback on the eStop state.
@@ -225,6 +230,50 @@ class DriverStationSim {
*/
static void SetMatchTime(double matchTime);
/**
* Register a callback on DS opmode changes.
*
* @param callback the callback that will be called when opmode changes
* @param initialNotify if true, the callback will be run on the initial value
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]]
static std::unique_ptr<CallbackStore> RegisterOpModeCallback(
NotifyCallback callback, bool initialNotify);
/**
* Get the opmode set by the DS.
*
* @return opmode
*/
static int64_t GetOpMode();
/**
* Change the opmode set by the DS.
*
* @param opmode the new value
*/
static void SetOpMode(int64_t opmode);
/**
* Register a callback on opmode options changes.
*
* @param callback the callback that will be called when the list of opmodes
* changes
* @param initialNotify if true, the callback will be run on the initial value
* @return the CallbackStore object associated with this callback.
*/
[[nodiscard]]
static std::unique_ptr<CallbackStore> RegisterOpModeOptionsCallback(
OpModeOptionsCallback callback, bool initialNotify);
/**
* Gets the list of opmode options.
*
* @return opmodes list
*/
static OpModeOptions GetOpModeOptions();
/**
* Updates DriverStation data so that new values are visible to the user
* program.

15
wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp
View File

@@ -6,6 +6,7 @@
#include <stdint.h>
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/HALBase.h"
#include "wpi/units/time.hpp"
@@ -37,6 +38,20 @@ void SetProgramStarted(bool started);
*/
bool GetProgramStarted();
/**
* Sets the user program state (control word).
*
* @param controlWord control word
*/
void SetProgramState(wpi::hal::ControlWord controlWord);
/**
* Gets the user program state (control word).
*
* @return Control word
*/
wpi::hal::ControlWord GetProgramState();
/**
* Restart the simulator time.
*/