mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
[hal] Use MrcLib to talk to DS (#8858)
Using MrcLib on the robot is going to be the plan for the future, to make things easier. MrcLib is how sim is supported going forward. The desktop version of mrclib can act as a robot server. This is set up where the mrclib interface is in shared code. On robot, that is the only backend used. On desktop, a default sim backend is used. However, the sim plugin can switch that to the real robot backend, so the robot code will exactly look like a real robot.
This commit is contained in:
Thad House
302
hal/src/main/native/cpp/FIRSTDriverStation.cpp
Normal file
302
hal/src/main/native/cpp/FIRSTDriverStation.cpp
Normal file
@@ -0,0 +1,302 @@
|
||||
// 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 <atomic>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <functional>
|
||||
#include <string>
|
||||
|
||||
#include <fmt/format.h>
|
||||
|
||||
#include "wpi/hal/DashboardOpMode.hpp"
|
||||
#include "wpi/hal/cpp/MrcLibDs.hpp"
|
||||
#include "wpi/hal/monotonic_clock.hpp"
|
||||
#include "wpi/util/Synchronization.hpp"
|
||||
#include "wpi/util/mutex.hpp"
|
||||
|
||||
static wpi::hal::MrcLibDs* mrcLibDs;
|
||||
|
||||
wpi::hal::MrcLibDs::~MrcLibDs() = default;
|
||||
|
||||
static void DefaultPrintErrorImpl(const struct WPI_String* line) {
|
||||
if (line && line->str) {
|
||||
std::fwrite(line->str, line->len, 1, stderr);
|
||||
}
|
||||
}
|
||||
|
||||
static std::atomic<void (*)(const struct WPI_String* line)> gPrintErrorImpl{
|
||||
DefaultPrintErrorImpl};
|
||||
|
||||
namespace wpi::hal {
|
||||
int32_t DefaultSendErrorImpl(bool isError, int32_t errorCode,
|
||||
const struct WPI_String* details,
|
||||
const struct WPI_String* location,
|
||||
const struct WPI_String* callStack, bool printMsg,
|
||||
wpi::hal::BackendPrintFunction& backendWriteFunc) {
|
||||
auto detailsView = wpi::util::to_string_view(details);
|
||||
auto locationView = wpi::util::to_string_view(location);
|
||||
auto callStackView = wpi::util::to_string_view(callStack);
|
||||
// Avoid flooding console by keeping track of previous 5 error
|
||||
// messages and only printing again if they're longer than 1 second old.
|
||||
static wpi::util::mutex msgMutex;
|
||||
static constexpr int KEEP_MSGS = 5;
|
||||
std::scoped_lock lock(msgMutex);
|
||||
static std::string prevMsg[KEEP_MSGS];
|
||||
static std::chrono::time_point<monotonic_clock> prevMsgTime[KEEP_MSGS];
|
||||
static bool initialized = false;
|
||||
if (!initialized) {
|
||||
for (int i = 0; i < KEEP_MSGS; i++) {
|
||||
prevMsgTime[i] = monotonic_clock::now() - std::chrono::seconds(2);
|
||||
}
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
auto curTime = monotonic_clock::now();
|
||||
int i;
|
||||
for (i = 0; i < KEEP_MSGS; ++i) {
|
||||
if (prevMsg[i] == detailsView) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
int retval = 0;
|
||||
if (i == KEEP_MSGS || (curTime - prevMsgTime[i]) >= std::chrono::seconds(1)) {
|
||||
if (backendWriteFunc) {
|
||||
retval = backendWriteFunc(isError, errorCode, details, location,
|
||||
callStack, &printMsg);
|
||||
}
|
||||
|
||||
if (printMsg) {
|
||||
fmt::memory_buffer buf;
|
||||
if (!locationView.empty() && locationView[0] != '\0') {
|
||||
fmt::format_to(fmt::appender{buf},
|
||||
"{} at {}: ", isError ? "Error" : "Warning",
|
||||
locationView);
|
||||
}
|
||||
fmt::format_to(fmt::appender{buf}, "{}\n", detailsView);
|
||||
if (!callStackView.empty() && callStackView[0] != '\0') {
|
||||
fmt::format_to(fmt::appender{buf}, "{}\n", callStackView);
|
||||
}
|
||||
auto printError = gPrintErrorImpl.load();
|
||||
struct WPI_String line = {buf.data(), buf.size()};
|
||||
printError(&line);
|
||||
}
|
||||
if (i == KEEP_MSGS) {
|
||||
// replace the oldest one
|
||||
i = 0;
|
||||
auto first = prevMsgTime[0];
|
||||
for (int j = 1; j < KEEP_MSGS; ++j) {
|
||||
if (prevMsgTime[j] < first) {
|
||||
first = prevMsgTime[j];
|
||||
i = j;
|
||||
}
|
||||
}
|
||||
prevMsg[i] = detailsView;
|
||||
}
|
||||
prevMsgTime[i] = curTime;
|
||||
}
|
||||
return retval;
|
||||
}
|
||||
} // namespace wpi::hal
|
||||
|
||||
extern "C" {
|
||||
|
||||
void HAL_SetPrintErrorImpl(void (*func)(const struct WPI_String* line)) {
|
||||
gPrintErrorImpl.store(func ? func : DefaultPrintErrorImpl);
|
||||
}
|
||||
|
||||
int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode,
|
||||
const struct WPI_String* details,
|
||||
const struct WPI_String* location,
|
||||
const struct WPI_String* callStack, HAL_Bool printMsg) {
|
||||
return mrcLibDs->sendError(isError, errorCode, details, location, callStack,
|
||||
printMsg);
|
||||
}
|
||||
|
||||
int32_t HAL_SendConsoleLine(const struct WPI_String* line) {
|
||||
return mrcLibDs->sendConsoleLine(line);
|
||||
}
|
||||
|
||||
int32_t HAL_SendProgramCrash(const struct WPI_String* details,
|
||||
const struct WPI_String* location,
|
||||
const struct WPI_String* callStack) {
|
||||
return mrcLibDs->sendProgramCrash(details, location, callStack);
|
||||
}
|
||||
|
||||
int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
|
||||
return mrcLibDs->getControlWord(controlWord);
|
||||
}
|
||||
|
||||
int32_t HAL_GetUncachedControlWord(HAL_ControlWord* controlWord) {
|
||||
return mrcLibDs->getUncachedControlWord(controlWord);
|
||||
}
|
||||
|
||||
int32_t HAL_SetOpModeOptions(const struct HAL_OpModeOption* options,
|
||||
int32_t count) {
|
||||
return mrcLibDs->setOpModeOptions(options, count);
|
||||
}
|
||||
|
||||
int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
|
||||
return mrcLibDs->getJoystickAxes(joystickNum, axes);
|
||||
}
|
||||
|
||||
int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
|
||||
return mrcLibDs->getJoystickPOVs(joystickNum, povs);
|
||||
}
|
||||
|
||||
int32_t HAL_GetJoystickButtons(int32_t joystickNum,
|
||||
HAL_JoystickButtons* buttons) {
|
||||
return mrcLibDs->getJoystickButtons(joystickNum, buttons);
|
||||
}
|
||||
|
||||
int32_t HAL_GetJoystickTouchpads(int32_t joystickNum,
|
||||
HAL_JoystickTouchpads* touchpads) {
|
||||
return mrcLibDs->getJoystickTouchpads(joystickNum, touchpads);
|
||||
}
|
||||
|
||||
void HAL_GetAllJoystickData(int32_t joystickNum, HAL_JoystickAxes* axes,
|
||||
HAL_JoystickPOVs* povs,
|
||||
HAL_JoystickButtons* buttons,
|
||||
HAL_JoystickTouchpads* touchpads) {
|
||||
mrcLibDs->getAllJoystickData(joystickNum, axes, povs, buttons, touchpads);
|
||||
}
|
||||
|
||||
int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
|
||||
HAL_JoystickDescriptor* desc) {
|
||||
return mrcLibDs->getJoystickDescriptor(joystickNum, desc);
|
||||
}
|
||||
|
||||
int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) {
|
||||
return mrcLibDs->getMatchInfo(info);
|
||||
}
|
||||
|
||||
HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
|
||||
HAL_AllianceStationID allianceStation;
|
||||
*status = mrcLibDs->getAllianceStation(&allianceStation);
|
||||
return allianceStation;
|
||||
}
|
||||
|
||||
HAL_Bool HAL_GetJoystickIsGamepad(int32_t joystickNum) {
|
||||
HAL_JoystickDescriptor joystickDesc;
|
||||
int32_t status = mrcLibDs->getJoystickDescriptor(joystickNum, &joystickDesc);
|
||||
if (status == 0) {
|
||||
return joystickDesc.isGamepad != 0;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
int32_t HAL_GetJoystickGamepadType(int32_t joystickNum) {
|
||||
HAL_JoystickDescriptor joystickDesc;
|
||||
int32_t status = mrcLibDs->getJoystickDescriptor(joystickNum, &joystickDesc);
|
||||
if (status == 0) {
|
||||
return joystickDesc.gamepadType;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int32_t HAL_GetGameData(HAL_GameData* gameData) {
|
||||
return mrcLibDs->getGameData(gameData);
|
||||
}
|
||||
|
||||
int32_t HAL_GetJoystickSupportedOutputs(int32_t joystickNum) {
|
||||
HAL_JoystickDescriptor joystickDesc;
|
||||
int32_t status = mrcLibDs->getJoystickDescriptor(joystickNum, &joystickDesc);
|
||||
if (status == 0) {
|
||||
return joystickDesc.supportedOutputs;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void HAL_GetJoystickName(struct WPI_String* name, int32_t joystickNum) {
|
||||
HAL_JoystickDescriptor joystickDesc;
|
||||
const char* cName = joystickDesc.name;
|
||||
if (mrcLibDs->getJoystickDescriptor(joystickNum, &joystickDesc) == 0) {
|
||||
cName = joystickDesc.name;
|
||||
} else {
|
||||
cName = "";
|
||||
}
|
||||
auto len = std::strlen(cName);
|
||||
auto write = WPI_AllocateString(name, len);
|
||||
std::memcpy(write, cName, len);
|
||||
}
|
||||
|
||||
int32_t HAL_SetJoystickRumble(int32_t joystickNum, int32_t leftRumble,
|
||||
int32_t rightRumble, int32_t leftTriggerRumble,
|
||||
int32_t rightTriggerRumble) {
|
||||
return mrcLibDs->setJoystickRumble(joystickNum, leftRumble, rightRumble,
|
||||
leftTriggerRumble, rightTriggerRumble);
|
||||
}
|
||||
|
||||
int32_t HAL_SetJoystickLeds(int32_t joystickNum, int32_t leds) {
|
||||
return mrcLibDs->setJoystickLeds(joystickNum, leds);
|
||||
}
|
||||
|
||||
double HAL_GetMatchTime(int32_t* status) {
|
||||
double matchTime;
|
||||
*status = mrcLibDs->getMatchTime(&matchTime);
|
||||
if (*status != 0) {
|
||||
return -1.0;
|
||||
}
|
||||
return matchTime;
|
||||
}
|
||||
|
||||
void HAL_ObserveUserProgramStarting(void) {
|
||||
mrcLibDs->observeUserProgramStarting();
|
||||
}
|
||||
|
||||
void HAL_ObserveUserProgram(HAL_ControlWord word) {
|
||||
mrcLibDs->observeUserProgram(word);
|
||||
}
|
||||
|
||||
HAL_Bool HAL_RefreshDSData(void) {
|
||||
bool wasRefreshed;
|
||||
if (mrcLibDs->refreshDSData(&wasRefreshed) < 0) {
|
||||
return false;
|
||||
}
|
||||
return wasRefreshed;
|
||||
}
|
||||
|
||||
void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) {
|
||||
mrcLibDs->provideNewDataEventHandle(handle);
|
||||
}
|
||||
|
||||
void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle) {
|
||||
mrcLibDs->removeNewDataEventHandle(handle);
|
||||
}
|
||||
|
||||
HAL_Bool HAL_GetOutputsEnabled(void) {
|
||||
bool outputsEnabled;
|
||||
if (mrcLibDs->getOutputsEnabled(&outputsEnabled) < 0) {
|
||||
return false;
|
||||
}
|
||||
return outputsEnabled;
|
||||
}
|
||||
|
||||
HAL_Bool HAL_GetSystemTimeValid(int32_t* status) {
|
||||
bool systemTimeValid;
|
||||
*status = mrcLibDs->getSystemTimeValid(&systemTimeValid);
|
||||
return systemTimeValid;
|
||||
}
|
||||
|
||||
} // extern "C"
|
||||
|
||||
namespace wpi::hal {
|
||||
void InitializeDriverStation() {
|
||||
InitializeDashboardOpMode();
|
||||
mrcLibDs = wpi::hal::GetDefaultDriverStationImpl();
|
||||
}
|
||||
|
||||
void ForceDsInstance(MrcLibDs* ds) {
|
||||
mrcLibDs = ds;
|
||||
}
|
||||
|
||||
void WaitForInitialPacket() {
|
||||
wpi::util::Event waitForInitEvent;
|
||||
mrcLibDs->provideNewDataEventHandle(waitForInitEvent.GetHandle());
|
||||
bool timed_out = false;
|
||||
wpi::util::WaitForObject(waitForInitEvent.GetHandle(), 0.1, &timed_out);
|
||||
// Don't care what the result is, just want to give it a chance.
|
||||
mrcLibDs->removeNewDataEventHandle(waitForInitEvent.GetHandle());
|
||||
}
|
||||
} // namespace wpi::hal
|
||||
@@ -341,20 +341,23 @@ Java_org_wpilib_hardware_hal_DriverStationJNI_getGameData
|
||||
/*
|
||||
* Class: org_wpilib_hardware_hal_DriverStationJNI
|
||||
* Method: sendError
|
||||
* Signature: (ZIZLjava/lang/String;Ljava/lang/String;Ljava/lang/String;Z)I
|
||||
* Signature: (ZILjava/lang/String;Ljava/lang/String;Ljava/lang/String;Z)I
|
||||
*/
|
||||
JNIEXPORT jint JNICALL
|
||||
Java_org_wpilib_hardware_hal_DriverStationJNI_sendError
|
||||
(JNIEnv* env, jclass, jboolean isError, jint errorCode, jboolean isLVCode,
|
||||
jstring details, jstring location, jstring callStack, jboolean printMsg)
|
||||
(JNIEnv* env, jclass, jboolean isError, jint errorCode, jstring details,
|
||||
jstring location, jstring callStack, jboolean printMsg)
|
||||
{
|
||||
JStringRef detailsStr{env, details};
|
||||
JStringRef locationStr{env, location};
|
||||
JStringRef callStackStr{env, callStack};
|
||||
|
||||
jint returnValue =
|
||||
HAL_SendError(isError, errorCode, isLVCode, detailsStr.c_str(),
|
||||
locationStr.c_str(), callStackStr.c_str(), printMsg);
|
||||
WPI_String detailsWpiStr = wpi::util::make_string(detailsStr);
|
||||
WPI_String locationWpiStr = wpi::util::make_string(locationStr);
|
||||
WPI_String callStackWpiStr = wpi::util::make_string(callStackStr);
|
||||
|
||||
jint returnValue = HAL_SendError(isError, errorCode, &detailsWpiStr,
|
||||
&locationWpiStr, &callStackWpiStr, printMsg);
|
||||
return returnValue;
|
||||
}
|
||||
|
||||
@@ -369,7 +372,30 @@ Java_org_wpilib_hardware_hal_DriverStationJNI_sendConsoleLine
|
||||
{
|
||||
JStringRef lineStr{env, line};
|
||||
|
||||
jint returnValue = HAL_SendConsoleLine(lineStr.c_str());
|
||||
WPI_String lineWpiStr = wpi::util::make_string(lineStr);
|
||||
jint returnValue = HAL_SendConsoleLine(&lineWpiStr);
|
||||
return returnValue;
|
||||
}
|
||||
|
||||
/*
|
||||
* Class: org_wpilib_hardware_hal_DriverStationJNI
|
||||
* Method: sendProgramCrash
|
||||
* Signature: (Ljava/lang/String;Ljava/lang/String;Ljava/lang/String;)I
|
||||
*/
|
||||
JNIEXPORT jint JNICALL
|
||||
Java_org_wpilib_hardware_hal_DriverStationJNI_sendProgramCrash
|
||||
(JNIEnv* env, jclass, jstring details, jstring location, jstring callStack)
|
||||
{
|
||||
JStringRef detailsStr{env, details};
|
||||
JStringRef locationStr{env, location};
|
||||
JStringRef callStackStr{env, callStack};
|
||||
|
||||
WPI_String detailsWpiStr = wpi::util::make_string(detailsStr);
|
||||
WPI_String locationWpiStr = wpi::util::make_string(locationStr);
|
||||
WPI_String callStackWpiStr = wpi::util::make_string(callStackStr);
|
||||
|
||||
jint returnValue =
|
||||
HAL_SendProgramCrash(&detailsWpiStr, &locationWpiStr, &callStackWpiStr);
|
||||
return returnValue;
|
||||
}
|
||||
|
||||
|
||||
@@ -113,8 +113,11 @@ void ReportError(JNIEnv* env, int32_t status, bool doThrow) {
|
||||
// Make a copy of message for safety, calling back into the HAL might
|
||||
// invalidate the string.
|
||||
std::string lastMessage{message};
|
||||
HAL_SendError(1, status, 0, lastMessage.c_str(), func.c_str(),
|
||||
stack.c_str(), 1);
|
||||
WPI_String details = wpi::util::make_string(lastMessage);
|
||||
WPI_String location = wpi::util::make_string(func);
|
||||
WPI_String callStack = wpi::util::make_string(stack);
|
||||
|
||||
HAL_SendError(1, status, &details, &location, &callStack, 1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -658,8 +658,9 @@ Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setSendError
|
||||
HALSIM_SetSendError(nullptr);
|
||||
} else {
|
||||
HALSIM_SetSendError([](HAL_Bool isError, int32_t errorCode,
|
||||
HAL_Bool isLVCode, const char* details,
|
||||
const char* location, const char* callStack,
|
||||
const struct WPI_String* details,
|
||||
const struct WPI_String* location,
|
||||
const struct WPI_String* callStack,
|
||||
HAL_Bool printMsg) { return 0; });
|
||||
}
|
||||
}
|
||||
@@ -676,7 +677,7 @@ Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setSendConsoleLine
|
||||
if (shouldSend) {
|
||||
HALSIM_SetSendConsoleLine(nullptr);
|
||||
} else {
|
||||
HALSIM_SetSendConsoleLine([](const char* line) { return 0; });
|
||||
HALSIM_SetSendConsoleLine([](const struct WPI_String* line) { return 0; });
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
670
hal/src/main/native/cpp/mrclib/MrcLibDs.cpp
Normal file
670
hal/src/main/native/cpp/mrclib/MrcLibDs.cpp
Normal file
@@ -0,0 +1,670 @@
|
||||
// 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/hal/cpp/MrcLibDs.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstring>
|
||||
#include <exception>
|
||||
#include <span>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <fmt/format.h>
|
||||
|
||||
#include "mrclib/ApiVersion.h"
|
||||
#include "mrclib/Console.h"
|
||||
#include "mrclib/DsComms.h"
|
||||
#include "mrclib/DsComms.hpp"
|
||||
#include "mrclib/DsCommsControl.h"
|
||||
#include "mrclib/MrcString.hpp"
|
||||
#include "wpi/hal/DashboardOpMode.hpp"
|
||||
#include "wpi/hal/Errors.h"
|
||||
#include "wpi/util/EventVector.hpp"
|
||||
#include "wpi/util/mutex.hpp"
|
||||
|
||||
using namespace wpi::hal;
|
||||
|
||||
namespace {
|
||||
struct JoystickDataCache {
|
||||
JoystickDataCache() { std::memset(this, 0, sizeof(*this)); }
|
||||
void Update(const MRC_ControlData& controlData,
|
||||
const MRC_Joysticks& joystickData);
|
||||
|
||||
HAL_JoystickAxes axes[HAL_MAX_JOYSTICKS];
|
||||
HAL_JoystickPOVs povs[HAL_MAX_JOYSTICKS];
|
||||
HAL_JoystickButtons buttons[HAL_MAX_JOYSTICKS];
|
||||
HAL_JoystickTouchpads touchpads[HAL_MAX_JOYSTICKS];
|
||||
HAL_AllianceStationID allianceStation;
|
||||
float matchTime;
|
||||
HAL_ControlWord controlWord;
|
||||
HAL_GameData gameData;
|
||||
};
|
||||
static_assert(std::is_standard_layout_v<JoystickDataCache>);
|
||||
|
||||
struct TcpCache {
|
||||
TcpCache() { std::memset(this, 0, sizeof(*this)); }
|
||||
void Update();
|
||||
void CloneTo(TcpCache* other) { std::memcpy(other, this, sizeof(*this)); }
|
||||
|
||||
HAL_MatchInfo matchInfo;
|
||||
HAL_JoystickDescriptor descriptors[HAL_MAX_JOYSTICKS];
|
||||
};
|
||||
static_assert(std::is_standard_layout_v<TcpCache>);
|
||||
} // namespace
|
||||
|
||||
void JoystickDataCache::Update(const MRC_ControlData& controlData,
|
||||
const MRC_Joysticks& joystickData) {
|
||||
matchTime = controlData.matchTime;
|
||||
uint32_t allianceInt = mrclib::GetAlliance(controlData.controlFlags);
|
||||
allianceInt += 1;
|
||||
allianceStation = static_cast<HAL_AllianceStationID>(allianceInt);
|
||||
|
||||
auto gameDataSize = controlData.gameDataLength;
|
||||
if (gameDataSize > MRCLIB_MAX_GAMEDATA_LENGTH) {
|
||||
gameDataSize = MRCLIB_MAX_GAMEDATA_LENGTH;
|
||||
}
|
||||
std::memcpy(this->gameData.gameData, controlData.gameData, gameDataSize);
|
||||
this->gameData.gameData[gameDataSize] = '\0';
|
||||
|
||||
if (mrclib::GetSupportsOpModes(controlData.controlFlags)) {
|
||||
controlWord =
|
||||
HAL_MakeControlWord(controlData.currentOpMode,
|
||||
static_cast<HAL_RobotMode>(
|
||||
mrclib::GetRobotMode(controlData.controlFlags)),
|
||||
mrclib::GetEnabled(controlData.controlFlags),
|
||||
mrclib::GetEStop(controlData.controlFlags),
|
||||
mrclib::GetFmsConnected(controlData.controlFlags),
|
||||
mrclib::GetDsConnected(controlData.controlFlags));
|
||||
} else {
|
||||
wpi::hal::EnableDashboardOpMode();
|
||||
auto robotMode = static_cast<HAL_RobotMode>(
|
||||
mrclib::GetRobotMode(controlData.controlFlags));
|
||||
controlWord =
|
||||
HAL_MakeControlWord(wpi::hal::GetDashboardSelectedOpMode(robotMode),
|
||||
static_cast<HAL_RobotMode>(
|
||||
mrclib::GetRobotMode(controlData.controlFlags)),
|
||||
mrclib::GetEnabled(controlData.controlFlags),
|
||||
mrclib::GetEStop(controlData.controlFlags),
|
||||
mrclib::GetFmsConnected(controlData.controlFlags),
|
||||
mrclib::GetDsConnected(controlData.controlFlags));
|
||||
}
|
||||
|
||||
for (size_t count = 0; count < joystickData.count; count++) {
|
||||
auto& newStick = joystickData.joysticks[count];
|
||||
|
||||
axes[count].available = newStick.availableAxes;
|
||||
|
||||
for (size_t i = 0; i < MRCLIB_MAX_AXES; i++) {
|
||||
auto raw = newStick.axes[i];
|
||||
axes[count].raw[i] = raw;
|
||||
int16_t axisValue = raw;
|
||||
if (axisValue < 0) {
|
||||
axes[count].axes[i] = axisValue / 32768.0f;
|
||||
} else {
|
||||
axes[count].axes[i] = axisValue / 32767.0f;
|
||||
}
|
||||
}
|
||||
|
||||
povs[count].available = newStick.availablePovs;
|
||||
for (size_t i = 0; i < MRCLIB_MAX_POVS; i++) {
|
||||
povs[count].povs[i] = static_cast<HAL_JoystickPOV>(newStick.povs[i]);
|
||||
}
|
||||
|
||||
buttons[count].available = newStick.availableButtons;
|
||||
buttons[count].buttons = newStick.buttons;
|
||||
|
||||
touchpads[count].count = newStick.touchpadCount;
|
||||
for (size_t i = 0; i < MRCLIB_MAX_TOUCHPADS; i++) {
|
||||
touchpads[count].touchpads[i].count = newStick.touchpads[i].count;
|
||||
for (size_t j = 0; j < MRCLIB_MAX_TOUCHPAD_FINGERS; j++) {
|
||||
auto& finger = newStick.touchpads[i].fingers[j];
|
||||
touchpads[count].touchpads[i].fingers[j].down = finger.down ? 1 : 0;
|
||||
touchpads[count].touchpads[i].fingers[j].x = finger.x / 65535.0f;
|
||||
touchpads[count].touchpads[i].fingers[j].y = finger.y / 65535.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Mark remaining sticks as unavailable
|
||||
for (size_t i = joystickData.count; i < HAL_MAX_JOYSTICKS; i++) {
|
||||
axes[i].available = 0;
|
||||
povs[i].available = 0;
|
||||
buttons[i].available = 0;
|
||||
touchpads[i].count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void TcpCache::Update() {
|
||||
MRC_MatchInfo newMatchInfo;
|
||||
std::memset(&newMatchInfo, 0, sizeof(newMatchInfo));
|
||||
MRC_DsComms_GetMatchInfo(&newMatchInfo);
|
||||
|
||||
matchInfo.matchNumber = newMatchInfo.matchNumber;
|
||||
matchInfo.matchType = static_cast<HAL_MatchType>(newMatchInfo.matchType);
|
||||
matchInfo.replayNumber = newMatchInfo.replayNumber;
|
||||
|
||||
auto eventNameSize = newMatchInfo.eventNameLength;
|
||||
if (eventNameSize > MRCLIB_MAX_EVENTNAME_LENGTH) {
|
||||
eventNameSize = MRCLIB_MAX_EVENTNAME_LENGTH;
|
||||
}
|
||||
std::memcpy(matchInfo.eventName, newMatchInfo.eventName, eventNameSize);
|
||||
matchInfo.eventName[eventNameSize] = '\0';
|
||||
|
||||
MRC_JoystickDescriptors descriptorsMsg;
|
||||
std::memset(&descriptorsMsg, 0, sizeof(descriptorsMsg));
|
||||
MRC_DsComms_GetJoystickDescriptors(&descriptorsMsg);
|
||||
|
||||
size_t descriptorCount = descriptorsMsg.count;
|
||||
|
||||
for (size_t count = 0; count < descriptorCount; count++) {
|
||||
const auto& newDesc = descriptorsMsg.descriptors[count];
|
||||
|
||||
auto& desc = descriptors[count];
|
||||
|
||||
desc.isGamepad = newDesc.isGamepad ? 1 : 0;
|
||||
desc.supportedOutputs = newDesc.supportedOutputs;
|
||||
desc.gamepadType = newDesc.gamepadType;
|
||||
|
||||
auto joystickNameLen = newDesc.nameLength;
|
||||
std::memcpy(desc.name, newDesc.name, joystickNameLen);
|
||||
desc.name[joystickNameLen] = '\0';
|
||||
}
|
||||
for (size_t i = descriptorCount; i < HAL_MAX_JOYSTICKS; i++) {
|
||||
std::memset(&descriptors[i], 0, sizeof(descriptors[i]));
|
||||
}
|
||||
}
|
||||
|
||||
class MrcLibDsImpl : public MrcLibDs {
|
||||
public:
|
||||
MrcLibDsImpl();
|
||||
int32_t sendError(bool isError, int32_t errorCode,
|
||||
const struct WPI_String* details,
|
||||
const struct WPI_String* location,
|
||||
const struct WPI_String* callStack, bool printMsg) override;
|
||||
|
||||
int32_t sendConsoleLine(const struct WPI_String* line) override;
|
||||
|
||||
int32_t sendProgramCrash(const struct WPI_String* details,
|
||||
const struct WPI_String* location,
|
||||
const struct WPI_String* callStack) override;
|
||||
|
||||
int32_t getControlWord(HAL_ControlWord* controlWord) override;
|
||||
|
||||
int32_t getUncachedControlWord(HAL_ControlWord* controlWord) override;
|
||||
|
||||
int32_t setOpModeOptions(const struct HAL_OpModeOption* options,
|
||||
int32_t count) override;
|
||||
|
||||
int32_t getAllianceStation(HAL_AllianceStationID* allianceStation) override;
|
||||
|
||||
int32_t getJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) override;
|
||||
|
||||
int32_t getJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) override;
|
||||
|
||||
int32_t getJoystickButtons(int32_t joystickNum,
|
||||
HAL_JoystickButtons* buttons) override;
|
||||
|
||||
int32_t getJoystickTouchpads(int32_t joystickNum,
|
||||
HAL_JoystickTouchpads* touchpads) override;
|
||||
|
||||
int32_t getAllJoystickData(int32_t joystickNum, HAL_JoystickAxes* axes,
|
||||
HAL_JoystickPOVs* povs,
|
||||
HAL_JoystickButtons* buttons,
|
||||
HAL_JoystickTouchpads* touchpads) override;
|
||||
|
||||
int32_t getJoystickDescriptor(int32_t joystickNum,
|
||||
HAL_JoystickDescriptor* desc) override;
|
||||
|
||||
int32_t getGameData(HAL_GameData* gameData) override;
|
||||
|
||||
int32_t setJoystickRumble(int32_t joystickNum, int32_t leftRumble,
|
||||
int32_t rightRumble, int32_t leftTriggerRumble,
|
||||
int32_t rightTriggerRumble) override;
|
||||
|
||||
int32_t setJoystickLeds(int32_t joystickNum, int32_t leds) override;
|
||||
|
||||
int32_t getMatchTime(double* matchTime) override;
|
||||
|
||||
int32_t getMatchInfo(HAL_MatchInfo* info) override;
|
||||
|
||||
int32_t getOutputsEnabled(bool* outputsEnabled) override;
|
||||
|
||||
int32_t refreshDSData(bool* wasRefreshed) override;
|
||||
|
||||
void provideNewDataEventHandle(WPI_EventHandle handle) override;
|
||||
|
||||
void removeNewDataEventHandle(WPI_EventHandle handle) override;
|
||||
|
||||
int32_t observeUserProgramStarting() override;
|
||||
|
||||
int32_t observeUserProgram(HAL_ControlWord controlWord) override;
|
||||
|
||||
int32_t getSystemTimeValid(bool* systemTimeValid) override;
|
||||
|
||||
wpi::util::EventVector newDataEvents;
|
||||
|
||||
private:
|
||||
int32_t BackendPrintFunctionImpl(bool isError, int32_t errorCode,
|
||||
const struct WPI_String* details,
|
||||
const struct WPI_String* location,
|
||||
const struct WPI_String* callStack,
|
||||
bool* forcePrintMsg);
|
||||
|
||||
HAL_ControlWord newestControlWord{};
|
||||
JoystickDataCache caches[2];
|
||||
JoystickDataCache* currentRead = &caches[0];
|
||||
JoystickDataCache* cacheToUpdate = &caches[1];
|
||||
|
||||
BackendPrintFunction backendPrintFunc;
|
||||
|
||||
wpi::util::mutex cacheMutex;
|
||||
|
||||
TcpCache tcpCaches[2];
|
||||
TcpCache* tcpCurrentRead = &tcpCaches[0];
|
||||
TcpCache* tcpCacheToUpdate = &tcpCaches[1];
|
||||
|
||||
wpi::util::mutex tcpCacheMutex;
|
||||
|
||||
wpi::util::mutex joystickOutputMutexes[MRCLIB_MAX_JOYSTICKS];
|
||||
MRC_JoystickOutputs joystickOutputs[MRCLIB_MAX_JOYSTICKS];
|
||||
};
|
||||
|
||||
static MrcLibDsImpl* staticImpl;
|
||||
|
||||
static void MRC_CALLCONV newDataCallback() {
|
||||
if (staticImpl) {
|
||||
staticImpl->newDataEvents.Wakeup();
|
||||
}
|
||||
}
|
||||
|
||||
namespace wpi::hal {
|
||||
MrcLibDs* GetMrcLibDs() {
|
||||
static MrcLibDsImpl impl;
|
||||
return &impl;
|
||||
}
|
||||
} // namespace wpi::hal
|
||||
|
||||
static_assert(sizeof(int32_t) >= sizeof(int),
|
||||
"WPILIB_NetworkComm status variable is larger than 32 bits");
|
||||
|
||||
static_assert(MRCLIB_MAX_AXES == HAL_MAX_JOYSTICK_AXES);
|
||||
static_assert(MRCLIB_MAX_POVS == HAL_MAX_JOYSTICK_POVS);
|
||||
static_assert(MRCLIB_MAX_JOYSTICKS == HAL_MAX_JOYSTICKS);
|
||||
static_assert(MRCLIB_MAX_TOUCHPADS == HAL_MAX_JOYSTICK_TOUCHPADS);
|
||||
static_assert(MRCLIB_MAX_TOUCHPAD_FINGERS == HAL_MAX_JOYSTICK_TOUCHPAD_FINGERS);
|
||||
static_assert(MRCLIB_MAX_GAMEDATA_LENGTH == sizeof(HAL_GameData::gameData) - 1);
|
||||
static_assert(MRCLIB_MAX_EVENTNAME_LENGTH ==
|
||||
sizeof(HAL_MatchInfo::eventName) - 1);
|
||||
static_assert(MRCLIB_MAX_JOYSTICK_NAME_LENGTH ==
|
||||
sizeof(HAL_JoystickDescriptor::name) - 1);
|
||||
|
||||
MrcLibDsImpl::MrcLibDsImpl() {
|
||||
// Initialize joystick outputs to 0
|
||||
for (size_t i = 0; i < MRCLIB_MAX_JOYSTICKS; i++) {
|
||||
std::memset(&joystickOutputs[i], 0, sizeof(joystickOutputs[i]));
|
||||
}
|
||||
|
||||
if (!MRC_CHECK_API_VERSION()) {
|
||||
fmt::print(
|
||||
stderr,
|
||||
"Error: MRC API version mismatch. Restarting app and retrying...");
|
||||
|
||||
std::terminate();
|
||||
}
|
||||
|
||||
MRC_DsComms_Initialize();
|
||||
MRC_DsCommsControl_Initialize();
|
||||
MRC_Console_Initialize();
|
||||
|
||||
// Wait for 10 seconds for the system server to be ready.
|
||||
if (!MRC_DsComms_WaitForSystemServer(10000)) {
|
||||
fmt::print(stderr,
|
||||
"Error: Waiting for server ready failed. Restarting app and "
|
||||
"retrying...");
|
||||
|
||||
std::terminate();
|
||||
}
|
||||
|
||||
backendPrintFunc =
|
||||
[this](bool isError, int32_t errorCode, const struct WPI_String* details,
|
||||
const struct WPI_String* location,
|
||||
const struct WPI_String* callStack, bool* forcePrintMsg) {
|
||||
return BackendPrintFunctionImpl(isError, errorCode, details, location,
|
||||
callStack, forcePrintMsg);
|
||||
};
|
||||
|
||||
newestControlWord.value = 0;
|
||||
staticImpl = this;
|
||||
MRC_DsComms_SetNewDataCallback(newDataCallback);
|
||||
MRC_DsCommsControl_SetHasUserCode(true);
|
||||
}
|
||||
|
||||
static MRC_String WPIStringToMRCString(const struct WPI_String* wpiStr) {
|
||||
MRC_String mrcStr;
|
||||
if (wpiStr) {
|
||||
mrcStr.str = wpiStr->str;
|
||||
mrcStr.len = wpiStr->len;
|
||||
} else {
|
||||
mrcStr.str = nullptr;
|
||||
mrcStr.len = 0;
|
||||
}
|
||||
return mrcStr;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::BackendPrintFunctionImpl(
|
||||
bool isError, int32_t errorCode, const struct WPI_String* details,
|
||||
const struct WPI_String* location, const struct WPI_String* callStack,
|
||||
bool* forcePrintMsg) {
|
||||
// Don't touch forcePrintMsg, it's a sim thing.
|
||||
MRC_String mrcDetails = WPIStringToMRCString(details);
|
||||
MRC_String mrcLocation = WPIStringToMRCString(location);
|
||||
MRC_String mrcCallStack = WPIStringToMRCString(callStack);
|
||||
return MRC_Console_WriteError(isError, errorCode, &mrcDetails, &mrcLocation,
|
||||
&mrcCallStack);
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::sendError(bool isError, int32_t errorCode,
|
||||
const struct WPI_String* details,
|
||||
const struct WPI_String* location,
|
||||
const struct WPI_String* callStack,
|
||||
bool printMsg) {
|
||||
return DefaultSendErrorImpl(isError, errorCode, details, location, callStack,
|
||||
printMsg, backendPrintFunc);
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::sendConsoleLine(const struct WPI_String* line) {
|
||||
MRC_String mrcLine = WPIStringToMRCString(line);
|
||||
return MRC_Console_WriteLine(&mrcLine);
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::sendProgramCrash(const struct WPI_String* details,
|
||||
const struct WPI_String* location,
|
||||
const struct WPI_String* callStack) {
|
||||
MRC_String mrcDetails = WPIStringToMRCString(details);
|
||||
MRC_String mrcLocation = WPIStringToMRCString(location);
|
||||
MRC_String mrcCallStack = WPIStringToMRCString(callStack);
|
||||
return MRC_Console_WriteProgramCrash(&mrcDetails, &mrcLocation,
|
||||
&mrcCallStack);
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getControlWord(HAL_ControlWord* controlWord) {
|
||||
std::scoped_lock lock{cacheMutex};
|
||||
*controlWord = newestControlWord;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getUncachedControlWord(HAL_ControlWord* controlWord) {
|
||||
MRC_ControlData data;
|
||||
int32_t status = MRC_DsComms_GetControlData(&data);
|
||||
bool dataValid = (status == 0);
|
||||
if (dataValid && mrclib::GetDsConnected(data.controlFlags)) {
|
||||
if (mrclib::GetSupportsOpModes(data.controlFlags)) {
|
||||
*controlWord = HAL_MakeControlWord(
|
||||
data.currentOpMode,
|
||||
static_cast<HAL_RobotMode>(mrclib::GetRobotMode(data.controlFlags)),
|
||||
mrclib::GetEnabled(data.controlFlags),
|
||||
mrclib::GetEStop(data.controlFlags),
|
||||
mrclib::GetFmsConnected(data.controlFlags),
|
||||
mrclib::GetDsConnected(data.controlFlags));
|
||||
} else {
|
||||
wpi::hal::EnableDashboardOpMode();
|
||||
auto robotMode =
|
||||
static_cast<HAL_RobotMode>(mrclib::GetRobotMode(data.controlFlags));
|
||||
*controlWord = HAL_MakeControlWord(
|
||||
wpi::hal::GetDashboardSelectedOpMode(robotMode),
|
||||
static_cast<HAL_RobotMode>(mrclib::GetRobotMode(data.controlFlags)),
|
||||
mrclib::GetEnabled(data.controlFlags),
|
||||
mrclib::GetEStop(data.controlFlags),
|
||||
mrclib::GetFmsConnected(data.controlFlags),
|
||||
mrclib::GetDsConnected(data.controlFlags));
|
||||
}
|
||||
} else {
|
||||
// DS disconnected. Clear the control word
|
||||
controlWord->value = 0;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::setOpModeOptions(const struct HAL_OpModeOption* options,
|
||||
int32_t count) {
|
||||
if (count < 0 || count > 1000 || (count != 0 && !options)) {
|
||||
return HAL_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;
|
||||
}
|
||||
auto& newValue = newOptions.emplace_back();
|
||||
newValue.hash = option.id;
|
||||
auto nameLen =
|
||||
std::min(static_cast<size_t>(option.name.len),
|
||||
static_cast<size_t>(MRCLIB_MAX_OPMODE_NAME_LENGTH));
|
||||
std::memcpy(newValue.name, option.name.str, nameLen);
|
||||
newValue.nameLength = static_cast<uint8_t>(nameLen);
|
||||
|
||||
auto groupLen =
|
||||
std::min(static_cast<size_t>(option.group.len),
|
||||
static_cast<size_t>(MRCLIB_MAX_OPMODE_GROUP_LENGTH));
|
||||
std::memcpy(newValue.group, option.group.str, groupLen);
|
||||
newValue.groupLength = static_cast<uint8_t>(groupLen);
|
||||
|
||||
auto descLen =
|
||||
std::min(static_cast<size_t>(option.description.len),
|
||||
static_cast<size_t>(MRCLIB_MAX_OPMODE_DESCRIPTION_LENGTH));
|
||||
std::memcpy(newValue.description, option.description.str, descLen);
|
||||
newValue.descriptionLength = static_cast<uint8_t>(descLen);
|
||||
|
||||
newValue.textColor = option.textColor;
|
||||
newValue.backgroundColor = option.backgroundColor;
|
||||
}
|
||||
}
|
||||
|
||||
int32_t status =
|
||||
MRC_DsCommsControl_SetOpModeOptions(newOptions.data(), newOptions.size());
|
||||
|
||||
if (count == 0) {
|
||||
wpi::hal::SetDashboardOpModeOptions({});
|
||||
} else {
|
||||
wpi::hal::SetDashboardOpModeOptions({options, options + count});
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getAllianceStation(
|
||||
HAL_AllianceStationID* allianceStation) {
|
||||
MRC_ControlData data;
|
||||
int32_t status = MRC_DsComms_GetControlData(&data);
|
||||
if (status == 0) {
|
||||
*allianceStation = static_cast<HAL_AllianceStationID>(
|
||||
mrclib::GetAlliance(data.controlFlags) + 1);
|
||||
} else {
|
||||
*allianceStation = HAL_ALLIANCE_STATION_UNKNOWN;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
#define CHECK_JOYSTICK_NUMBER(stickNum) \
|
||||
if ((stickNum) < 0 || (stickNum) >= HAL_MAX_JOYSTICKS) \
|
||||
return HAL_PARAMETER_OUT_OF_RANGE
|
||||
|
||||
int32_t MrcLibDsImpl::getJoystickAxes(int32_t joystickNum,
|
||||
HAL_JoystickAxes* axes) {
|
||||
CHECK_JOYSTICK_NUMBER(joystickNum);
|
||||
std::scoped_lock lock{cacheMutex};
|
||||
*axes = currentRead->axes[joystickNum];
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getJoystickPOVs(int32_t joystickNum,
|
||||
HAL_JoystickPOVs* povs) {
|
||||
CHECK_JOYSTICK_NUMBER(joystickNum);
|
||||
std::scoped_lock lock{cacheMutex};
|
||||
*povs = currentRead->povs[joystickNum];
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getJoystickButtons(int32_t joystickNum,
|
||||
HAL_JoystickButtons* buttons) {
|
||||
CHECK_JOYSTICK_NUMBER(joystickNum);
|
||||
std::scoped_lock lock{cacheMutex};
|
||||
*buttons = currentRead->buttons[joystickNum];
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getJoystickTouchpads(int32_t joystickNum,
|
||||
HAL_JoystickTouchpads* touchpads) {
|
||||
CHECK_JOYSTICK_NUMBER(joystickNum);
|
||||
std::scoped_lock lock{cacheMutex};
|
||||
*touchpads = currentRead->touchpads[joystickNum];
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getAllJoystickData(int32_t joystickNum,
|
||||
HAL_JoystickAxes* axes,
|
||||
HAL_JoystickPOVs* povs,
|
||||
HAL_JoystickButtons* buttons,
|
||||
HAL_JoystickTouchpads* touchpads) {
|
||||
CHECK_JOYSTICK_NUMBER(joystickNum);
|
||||
std::scoped_lock lock{cacheMutex};
|
||||
*axes = currentRead->axes[joystickNum];
|
||||
*povs = currentRead->povs[joystickNum];
|
||||
*buttons = currentRead->buttons[joystickNum];
|
||||
*touchpads = currentRead->touchpads[joystickNum];
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getJoystickDescriptor(int32_t joystickNum,
|
||||
HAL_JoystickDescriptor* desc) {
|
||||
CHECK_JOYSTICK_NUMBER(joystickNum);
|
||||
std::scoped_lock lock{tcpCacheMutex};
|
||||
*desc = tcpCurrentRead->descriptors[joystickNum];
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getGameData(HAL_GameData* gameData) {
|
||||
std::scoped_lock lock{cacheMutex};
|
||||
*gameData = currentRead->gameData;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::setJoystickRumble(int32_t joystickNum, int32_t leftRumble,
|
||||
int32_t rightRumble,
|
||||
int32_t leftTriggerRumble,
|
||||
int32_t rightTriggerRumble) {
|
||||
CHECK_JOYSTICK_NUMBER(joystickNum);
|
||||
std::scoped_lock lock{joystickOutputMutexes[joystickNum]};
|
||||
joystickOutputs[joystickNum].leftRumble =
|
||||
std::clamp<int32_t>(leftRumble, 0, UINT16_MAX);
|
||||
joystickOutputs[joystickNum].rightRumble =
|
||||
std::clamp<int32_t>(rightRumble, 0, UINT16_MAX);
|
||||
joystickOutputs[joystickNum].leftTriggerRumble =
|
||||
std::clamp<int32_t>(leftTriggerRumble, 0, UINT16_MAX);
|
||||
joystickOutputs[joystickNum].rightTriggerRumble =
|
||||
std::clamp<int32_t>(rightTriggerRumble, 0, UINT16_MAX);
|
||||
|
||||
return MRC_DsCommsControl_SetJoystickOutputs(joystickNum,
|
||||
&joystickOutputs[joystickNum]);
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::setJoystickLeds(int32_t joystickNum, int32_t leds) {
|
||||
CHECK_JOYSTICK_NUMBER(joystickNum);
|
||||
std::scoped_lock lock{joystickOutputMutexes[joystickNum]};
|
||||
joystickOutputs[joystickNum].r = (leds >> 16) & 0xFF;
|
||||
joystickOutputs[joystickNum].g = (leds >> 8) & 0xFF;
|
||||
joystickOutputs[joystickNum].b = leds & 0xFF;
|
||||
return MRC_DsCommsControl_SetJoystickOutputs(joystickNum,
|
||||
&joystickOutputs[joystickNum]);
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getMatchTime(double* matchTime) {
|
||||
std::scoped_lock lock{cacheMutex};
|
||||
*matchTime = currentRead->matchTime;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getMatchInfo(HAL_MatchInfo* info) {
|
||||
std::scoped_lock lock{tcpCacheMutex};
|
||||
*info = tcpCurrentRead->matchInfo;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::observeUserProgramStarting() {
|
||||
return MRC_DsCommsControl_SetHasUserCodeReady(true);
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::observeUserProgram(HAL_ControlWord controlWord) {
|
||||
return MRC_DsCommsControl_SetOpModeTrace(controlWord.value &
|
||||
(HAL_CONTROLWORD_OPMODE_HASH_MASK |
|
||||
HAL_CONTROLWORD_ROBOT_MODE_MASK |
|
||||
HAL_CONTROLWORD_ENABLED_MASK));
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::refreshDSData(bool* wasRefreshed) {
|
||||
MRC_Joysticks joystickData;
|
||||
MRC_ControlData controlData;
|
||||
int32_t status =
|
||||
MRC_DsComms_GetControlDataWithJoysticks(&controlData, &joystickData);
|
||||
|
||||
bool updatedData = false;
|
||||
|
||||
if (status == 0 && mrclib::GetDsConnected(controlData.controlFlags)) {
|
||||
// Update the cache
|
||||
cacheToUpdate->Update(controlData, joystickData);
|
||||
updatedData = true;
|
||||
} else {
|
||||
// DS disconnected. Clear the control word
|
||||
cacheToUpdate->controlWord.value = 0;
|
||||
}
|
||||
|
||||
{
|
||||
std::scoped_lock lock{cacheMutex};
|
||||
std::swap(cacheToUpdate, currentRead);
|
||||
newestControlWord = currentRead->controlWord;
|
||||
}
|
||||
|
||||
tcpCacheToUpdate->Update();
|
||||
{
|
||||
std::scoped_lock tcpLock(tcpCacheMutex);
|
||||
std::swap(tcpCacheToUpdate, tcpCurrentRead);
|
||||
}
|
||||
|
||||
*wasRefreshed = updatedData;
|
||||
return status;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getOutputsEnabled(bool* outputsEnabled) {
|
||||
MRC_ControlData data;
|
||||
int32_t status = MRC_DsComms_GetControlData(&data);
|
||||
if (status == 0) {
|
||||
*outputsEnabled =
|
||||
mrclib::GetWatchdogActive(data.controlFlags) ? true : false;
|
||||
} else {
|
||||
*outputsEnabled = false;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
int32_t MrcLibDsImpl::getSystemTimeValid(bool* systemTimeValid) {
|
||||
MRC_Bool valid;
|
||||
int32_t status = MRC_DsComms_GetSystemTimeValid(&valid);
|
||||
if (status == 0) {
|
||||
*systemTimeValid = valid ? true : false;
|
||||
} else {
|
||||
*systemTimeValid = false;
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
void MrcLibDsImpl::provideNewDataEventHandle(WPI_EventHandle handle) {
|
||||
newDataEvents.Add(handle);
|
||||
}
|
||||
|
||||
void MrcLibDsImpl::removeNewDataEventHandle(WPI_EventHandle handle) {
|
||||
newDataEvents.Remove(handle);
|
||||
}
|
||||
Reference in New Issue
Block a user