Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41: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

147
hal/src/main/native/systemcore/FRCDriverStation.cpp
View File

@@ -18,7 +18,9 @@
#include "HALInitializer.h"
#include "SystemServerInternal.h"
#include "mrc/NtNetComm.h"
#include "wpi/hal/DashboardOpMode.hpp"
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/hal/Errors.h"
#include "wpi/hal/proto/ControlData.h"
#include "wpi/hal/proto/ErrorInfo.h"
@@ -37,6 +39,7 @@
#include "wpi/util/SmallVector.hpp"
#include "wpi/util/condition_variable.hpp"
#include "wpi/util/mutex.hpp"
#include "wpi/util/string.h"
#include "wpi/util/timestamp.h"
static_assert(sizeof(int32_t) >= sizeof(int),
@@ -86,9 +89,7 @@ struct SystemServerDriverStation {
MRC_MAX_NUM_JOYSTICKS>
joystickOutputTopics;
wpi::nt::ProtobufPublisher<std::vector<mrc::OpMode>> teleopOpModes;
wpi::nt::ProtobufPublisher<std::vector<mrc::OpMode>> autoOpModes;
wpi::nt::ProtobufPublisher<std::vector<mrc::OpMode>> testOpModes;
wpi::nt::ProtobufPublisher<std::vector<mrc::OpMode>> opModeOptionsPublisher;
wpi::nt::IntegerPublisher traceOpModePublisher;
NT_Listener controlDataListener;
@@ -151,33 +152,11 @@ struct SystemServerDriverStation {
ROBOT_JOYSTICK_DESCRIPTORS_PATH)
.Subscribe({});
teleopOpModes = ntInst
.GetProtobufTopic<std::vector<mrc::OpMode>>(
ROBOT_TELEOP_OP_MODES_PATH)
.Publish();
autoOpModes = ntInst
.GetProtobufTopic<std::vector<mrc::OpMode>>(
ROBOT_AUTO_OP_MODES_PATH)
.Publish();
testOpModes = ntInst
.GetProtobufTopic<std::vector<mrc::OpMode>>(
ROBOT_TEST_OP_MODES_PATH)
.Publish();
std::vector<mrc::OpMode> staticTeleopOpModes;
staticTeleopOpModes.emplace_back(
mrc::OpMode{"TeleOp", mrc::OpModeHash::MakeTele(2)});
teleopOpModes.Set(staticTeleopOpModes);
std::vector<mrc::OpMode> staticAutoOpModes;
staticAutoOpModes.emplace_back(
mrc::OpMode{"Auto", mrc::OpModeHash::MakeAuto(1)});
autoOpModes.Set(staticAutoOpModes);
std::vector<mrc::OpMode> staticTestOpModes;
staticTestOpModes.emplace_back(
mrc::OpMode{"Test", mrc::OpModeHash::MakeTest(3)});
testOpModes.Set(staticTestOpModes);
opModeOptionsPublisher = ntInst
.GetProtobufTopic<std::vector<mrc::OpMode>>(
ROBOT_OP_MODE_OPTIONS_PATH)
.Publish();
opModeOptionsPublisher.Set({});
controlDataListener = ntInst.AddListener(
controlDataSubscriber, NT_EVENT_VALUE_REMOTE | NT_EVENT_UNPUBLISH,
@@ -243,13 +222,20 @@ void JoystickDataCache::Update(const mrc::ControlData& data) {
allianceInt += 1;
allianceStation = static_cast<HAL_AllianceStationID>(allianceInt);
std::memset(&controlWord, 0, sizeof(controlWord));
controlWord.enabled = data.ControlWord.Enabled;
controlWord.fmsAttached = data.ControlWord.FmsConnected;
controlWord.dsAttached = data.ControlWord.DsConnected;
controlWord.eStop = data.ControlWord.EStop;
controlWord.test = data.ControlWord.Test;
controlWord.autonomous = data.ControlWord.Auto;
if (data.ControlWord.SupportsOpModes) {
controlWord = HAL_MakeControlWord(
data.CurrentOpMode.ToValue(),
static_cast<HAL_RobotMode>(data.ControlWord.RobotMode),
data.ControlWord.Enabled, data.ControlWord.EStop,
data.ControlWord.FmsConnected, data.ControlWord.DsConnected);
} else {
wpi::hal::EnableDashboardOpMode();
auto robotMode = static_cast<HAL_RobotMode>(data.ControlWord.RobotMode);
controlWord = HAL_MakeControlWord(
wpi::hal::GetDashboardSelectedOpMode(robotMode), robotMode,
data.ControlWord.Enabled, data.ControlWord.EStop,
data.ControlWord.FmsConnected, data.ControlWord.DsConnected);
}
auto sticks = data.Joysticks();
@@ -373,7 +359,8 @@ void TcpCache::Update() {
namespace wpi::hal::init {
void InitializeFRCDriverStation() {
std::memset(&newestControlWord, 0, sizeof(newestControlWord));
InitializeDashboardOpMode();
newestControlWord.value = 0;
static FRCDriverStation ds;
driverStation = &ds;
}
@@ -478,6 +465,63 @@ int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
return 0;
}
int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord) {
mrc::ControlData data;
int64_t dataTime{0};
bool dataValid = systemServerDs->GetLastControlData(&data, &dataTime);
if (dataValid && data.ControlWord.DsConnected) {
if (data.ControlWord.SupportsOpModes) {
*controlWord = HAL_MakeControlWord(
data.CurrentOpMode.ToValue(),
static_cast<HAL_RobotMode>(data.ControlWord.RobotMode),
data.ControlWord.Enabled, data.ControlWord.EStop,
data.ControlWord.FmsConnected, data.ControlWord.DsConnected);
} else {
wpi::hal::EnableDashboardOpMode();
auto robotMode = static_cast<HAL_RobotMode>(data.ControlWord.RobotMode);
*controlWord = HAL_MakeControlWord(
wpi::hal::GetDashboardSelectedOpMode(robotMode), robotMode,
data.ControlWord.Enabled, data.ControlWord.EStop,
data.ControlWord.FmsConnected, data.ControlWord.DsConnected);
}
} else {
// DS disconnected. Clear the control word
controlWord->value = 0;
}
return 0;
}
int32_t HAL_SetOpModeOptions(const struct HAL_OpModeOption* options,
int32_t count) {
if (count < 0 || count > 1000 || (count != 0 && !options)) {
return PARAMETER_OUT_OF_RANGE;
}
std::vector<mrc::OpMode> newOptions;
newOptions.reserve(count);
if (count != 0) {
for (auto&& option : std::span{options, options + count}) {
if (option.id == 0) {
continue;
}
newOptions.emplace_back(mrc::OpModeHash::FromValue(option.id),
wpi::util::to_string_view(&option.name),
wpi::util::to_string_view(&option.group),
wpi::util::to_string_view(&option.description),
option.textColor, option.backgroundColor);
}
}
{
std::scoped_lock lock{tcpCacheMutex};
systemServerDs->opModeOptionsPublisher.Set(newOptions);
}
wpi::hal::SetDashboardOpModeOptions({options, options + count});
return 0;
}
int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
@@ -619,24 +663,11 @@ void HAL_ObserveUserProgramStarting(void) {
systemServerDs->hasUserCodeReadyPublisher.Set(true);
}
void HAL_ObserveUserProgramDisabled(void) {
systemServerDs->traceOpModePublisher.Set(
mrc::OpModeHash::MakeTele(1, false).ToValue());
}
void HAL_ObserveUserProgramAutonomous(void) {
auto tVal = mrc::OpModeHash::MakeAuto(2, true).ToValue();
systemServerDs->traceOpModePublisher.Set(tVal);
}
void HAL_ObserveUserProgramTeleop(void) {
auto tVal = mrc::OpModeHash::MakeTele(1, true).ToValue();
systemServerDs->traceOpModePublisher.Set(tVal);
}
void HAL_ObserveUserProgramTest(void) {
systemServerDs->traceOpModePublisher.Set(
mrc::OpModeHash::MakeTest(3, true).ToValue());
void HAL_ObserveUserProgram(HAL_ControlWord word) {
systemServerDs->traceOpModePublisher.Set(word.value &
(HAL_CONTROLWORD_OPMODE_HASH_MASK |
HAL_CONTROLWORD_ROBOT_MODE_MASK |
HAL_CONTROLWORD_ENABLED_MASK));
}
HAL_Bool HAL_RefreshDSData(void) {
@@ -657,8 +688,7 @@ HAL_Bool HAL_RefreshDSData(void) {
updatedData = true;
} else {
// DS disconnected. Clear the control word
std::memset(&cacheToUpdate->controlWord, 0,
sizeof(cacheToUpdate->controlWord));
cacheToUpdate->controlWord.value = 0;
}
{
@@ -694,6 +724,7 @@ HAL_Bool HAL_GetSystemTimeValid(int32_t* status) {
namespace wpi::hal {
void InitializeDriverStation() {
StartDashboardOpMode();
systemServerDs = new ::SystemServerDriverStation{wpi::hal::GetSystemServer()};
}