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] Update DS API to new format (#7977)

Browse Source
This commit is contained in:
Thad House
2025-05-16 22:15:14 -07:00
committed by GitHub
parent 1596e2fd7a
commit 231ec348fe
35 changed files with 878 additions and 766 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -11,6 +11,7 @@
#include <string>
#include <string_view>
#include <utility>
#include <vector>
#include <fmt/format.h>
#include <networktables/BooleanTopic.h>
@@ -32,8 +33,9 @@
#include "hal/proto/ControlData.h"
#include "hal/proto/ErrorInfo.h"
#include "hal/proto/JoystickDescriptor.h"
#include "hal/proto/JoystickOutputData.h"
#include "hal/proto/JoystickRumbleData.h"
#include "hal/proto/MatchInfo.h"
#include "hal/proto/OpMode.h"
#include "mrc/NtNetComm.h"
static_assert(sizeof(int32_t) >= sizeof(int),
@@ -60,12 +62,8 @@ static_assert(std::is_standard_layout_v<JoystickDataCache>);
struct SystemServerDriverStation {
nt::NetworkTableInstance ntInst;
nt::BooleanPublisher robotProgramPublisher;
nt::BooleanPublisher codeStartedPublisher;
nt::BooleanPublisher userCodeDisabledPublisher;
nt::BooleanPublisher userCodeAutonomousPublisher;
nt::BooleanPublisher userCodeTeleopPublisher;
nt::BooleanPublisher userCodeTestPublisher;
nt::BooleanPublisher hasUserCodePublisher;
nt::BooleanPublisher hasUserCodeReadyPublisher;
nt::ProtobufSubscriber<mrc::ControlData> controlDataSubscriber;
nt::ProtobufSubscriber<mrc::MatchInfo> matchInfoSubscriber;
@@ -79,9 +77,13 @@ struct SystemServerDriverStation {
nt::StringPublisher consoleLinePublisher;
nt::ProtobufPublisher<mrc::ErrorInfo> errorInfoPublisher;
std::array<nt::ProtobufPublisher<mrc::JoystickOutputData>,
std::array<nt::ProtobufPublisher<mrc::JoystickRumbleData>,
MRC_MAX_NUM_JOYSTICKS>
joystickOutputsTopics;
joystickRumbleTopics;
nt::ProtobufPublisher<std::vector<mrc::OpMode>> teleopOpModes;
nt::ProtobufPublisher<std::vector<mrc::OpMode>> autoOpModes;
nt::ProtobufPublisher<std::vector<mrc::OpMode>> testOpModes;
NT_Listener controlDataListener;
@@ -97,28 +99,20 @@ struct SystemServerDriverStation {
options.keepDuplicates = true;
options.periodic = 0.005;
codeStartedPublisher =
ntInst.GetBooleanTopic(ROBOT_CODE_STARTED_PATH).Publish(options);
userCodeDisabledPublisher =
ntInst.GetBooleanTopic(ROBOT_DISABLED_TRACE_PATH).Publish(options);
userCodeAutonomousPublisher =
ntInst.GetBooleanTopic(ROBOT_AUTON_TRACE_PATH).Publish(options);
userCodeTeleopPublisher =
ntInst.GetBooleanTopic(ROBOT_TELEOP_TRACE_PATH).Publish(options);
userCodeTestPublisher =
ntInst.GetBooleanTopic(ROBOT_TEST_TRACE_PATH).Publish(options);
hasUserCodeReadyPublisher =
ntInst.GetBooleanTopic(ROBOT_HAS_USER_CODE_READY_PATH).Publish(options);
for (size_t count = 0; count < joystickOutputsTopics.size(); count++) {
std::string name = ROBOT_JOYSTICK_OUTPUTS_PATH;
for (size_t count = 0; count < joystickRumbleTopics.size(); count++) {
std::string name = ROBOT_JOYSTICK_RUMBLE_PATH;
name += std::to_string(count);
joystickOutputsTopics[count] =
ntInst.GetProtobufTopic<mrc::JoystickOutputData>(name).Publish(
joystickRumbleTopics[count] =
ntInst.GetProtobufTopic<mrc::JoystickRumbleData>(name).Publish(
options);
}
robotProgramPublisher =
ntInst.GetBooleanTopic(ROBOT_NEW_ROBOT_PROGRAM_PATH).Publish();
robotProgramPublisher.Set(true);
hasUserCodePublisher =
ntInst.GetBooleanTopic(ROBOT_HAS_USER_CODE_PATH).Publish();
hasUserCodePublisher.Set(true);
consoleLinePublisher =
ntInst.GetStringTopic(ROBOT_CONSOLE_LINE_PATH).Publish(options);
@@ -147,6 +141,31 @@ struct SystemServerDriverStation {
ntInst.GetProtobufTopic<mrc::JoystickDescriptor>(name).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", 2});
teleopOpModes.Set(staticTeleopOpModes);
std::vector<mrc::OpMode> staticAutoOpModes;
staticAutoOpModes.emplace_back(mrc::OpMode{"Auto", 1});
autoOpModes.Set(staticAutoOpModes);
std::vector<mrc::OpMode> staticTestOpModes;
staticTestOpModes.emplace_back(mrc::OpMode{"Test", 3});
testOpModes.Set(staticTestOpModes);
ntInst.AddListener(
controlDataSubscriber, NT_EVENT_VALUE_REMOTE | NT_EVENT_UNPUBLISH,
[this](const nt::Event& event) { HandleListener(event); });
@@ -211,13 +230,8 @@ void JoystickDataCache::Update(const mrc::ControlData& data) {
controlWord.fmsAttached = data.ControlWord.FmsConnected;
controlWord.dsAttached = data.ControlWord.DsConnected;
controlWord.eStop = data.ControlWord.EStop;
auto mode = data.GetOpMode();
if (mode == "Test") {
controlWord.test = true;
} else if (mode == "Auton") {
controlWord.autonomous = true;
}
controlWord.test = data.ControlWord.Test;
controlWord.autonomous = data.ControlWord.Auto;
auto sticks = data.Joysticks();
@@ -228,7 +242,13 @@ void JoystickDataCache::Update(const mrc::ControlData& data) {
axes[count].count = newAxes.size();
for (size_t i = 0; i < newAxes.size(); i++) {
axes[count].axes[i] = newAxes[i];
axes[count].raw[i] = newAxes[i];
int16_t axisValue = newAxes[i];
if (axisValue < 0) {
axes[count].axes[i] = axisValue / 32768.0f;
} else {
axes[count].axes[i] = axisValue / 32767.0f;
}
}
povs[count].count = newPovs.size();
@@ -301,7 +321,7 @@ void TcpCache::Update() {
auto& desc = descriptors[count];
desc.isXbox = newDesc.IsXbox;
desc.isGamepad = newDesc.IsGamepad;
desc.type = newDesc.Type;
desc.buttonCount = newDesc.GetButtonsCount();
desc.povCount = newDesc.GetPovsCount();
@@ -481,12 +501,12 @@ HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
return currentRead->allianceStation;
}
HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum) {
HAL_Bool HAL_GetJoystickIsGamepad(int32_t joystickNum) {
HAL_JoystickDescriptor joystickDesc;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
return 0;
} else {
return joystickDesc.isXbox;
return joystickDesc.isGamepad;
}
}
@@ -523,15 +543,17 @@ int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
int32_t leftRumble, int32_t rightRumble) {
CHECK_JOYSTICK_NUMBER(joystickNum);
mrc::JoystickOutputData outputData{
.HidOutputs = static_cast<uint32_t>(outputs),
.LeftRumble = std::clamp(leftRumble, 0, UINT16_MAX) /
static_cast<float>(UINT16_MAX),
.RightRumble = std::clamp(rightRumble, 0, UINT16_MAX) /
static_cast<float>(UINT16_MAX),
};
// TODO Update this API
systemServerDs->joystickOutputsTopics[joystickNum].Set(outputData);
// mrc::JoystickOutputData outputData{
// .HidOutputs = static_cast<uint32_t>(outputs),
// .LeftRumble = std::clamp(leftRumble, 0, UINT16_MAX) /
// static_cast<float>(UINT16_MAX),
// .RightRumble = std::clamp(rightRumble, 0, UINT16_MAX) /
// static_cast<float>(UINT16_MAX),
// };
// systemServerDs->joystickRumbleTopics[joystickNum].Set(outputData);
return 0;
}
@@ -542,24 +564,16 @@ double HAL_GetMatchTime(int32_t* status) {
}
void HAL_ObserveUserProgramStarting(void) {
systemServerDs->codeStartedPublisher.Set(true);
systemServerDs->hasUserCodeReadyPublisher.Set(true);
}
void HAL_ObserveUserProgramDisabled(void) {
systemServerDs->userCodeDisabledPublisher.Set(true);
}
void HAL_ObserveUserProgramDisabled(void) {}
void HAL_ObserveUserProgramAutonomous(void) {
systemServerDs->userCodeAutonomousPublisher.Set(true);
}
void HAL_ObserveUserProgramAutonomous(void) {}
void HAL_ObserveUserProgramTeleop(void) {
systemServerDs->userCodeTeleopPublisher.Set(true);
}
void HAL_ObserveUserProgramTeleop(void) {}
void HAL_ObserveUserProgramTest(void) {
systemServerDs->userCodeTestPublisher.Set(true);
}
void HAL_ObserveUserProgramTest(void) {}
HAL_Bool HAL_RefreshDSData(void) {
mrc::ControlData newestData;