mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
Moved C++ comments from source files to headers (#1111)
Also sorted functions in C++ sources to match order in related headers.
This commit is contained in:
Tyler Veness
committed by
Peter Johnson
Peter Johnson
parent
d9971a705a
commit
8c680a26f8
@@ -81,43 +81,23 @@ DriverStation::~DriverStation() {
|
||||
m_dsThread.join();
|
||||
}
|
||||
|
||||
/**
|
||||
* Return a pointer to the singleton DriverStation.
|
||||
*
|
||||
* @return Pointer to the DS instance
|
||||
*/
|
||||
DriverStation& DriverStation::GetInstance() {
|
||||
static DriverStation instance;
|
||||
return instance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Report an error to the DriverStation messages window.
|
||||
*
|
||||
* The error is also printed to the program console.
|
||||
*/
|
||||
void DriverStation::ReportError(const wpi::Twine& error) {
|
||||
wpi::SmallString<128> temp;
|
||||
HAL_SendError(1, 1, 0, error.toNullTerminatedStringRef(temp).data(), "", "",
|
||||
1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Report a warning to the DriverStation messages window.
|
||||
*
|
||||
* The warning is also printed to the program console.
|
||||
*/
|
||||
void DriverStation::ReportWarning(const wpi::Twine& error) {
|
||||
wpi::SmallString<128> temp;
|
||||
HAL_SendError(0, 1, 0, error.toNullTerminatedStringRef(temp).data(), "", "",
|
||||
1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Report an error to the DriverStation messages window.
|
||||
*
|
||||
* The error is also printed to the program console.
|
||||
*/
|
||||
void DriverStation::ReportError(bool isError, int32_t code,
|
||||
const wpi::Twine& error,
|
||||
const wpi::Twine& location,
|
||||
@@ -131,13 +111,6 @@ void DriverStation::ReportError(bool isError, int32_t code,
|
||||
stack.toNullTerminatedStringRef(stackTemp).data(), 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The state of one joystick button. Button indexes begin at 1.
|
||||
*
|
||||
* @param stick The joystick to read.
|
||||
* @param button The button index, beginning at 1.
|
||||
* @return The state of the joystick button.
|
||||
*/
|
||||
bool DriverStation::GetStickButton(int stick, int button) {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -161,14 +134,6 @@ bool DriverStation::GetStickButton(int stick, int button) {
|
||||
return m_joystickButtons[stick].buttons & 1 << (button - 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Whether one joystick button was pressed since the last check. Button indexes
|
||||
* begin at 1.
|
||||
*
|
||||
* @param stick The joystick to read.
|
||||
* @param button The button index, beginning at 1.
|
||||
* @return Whether the joystick button was pressed since the last check.
|
||||
*/
|
||||
bool DriverStation::GetStickButtonPressed(int stick, int button) {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -198,14 +163,6 @@ bool DriverStation::GetStickButtonPressed(int stick, int button) {
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Whether one joystick button was released since the last check. Button indexes
|
||||
* begin at 1.
|
||||
*
|
||||
* @param stick The joystick to read.
|
||||
* @param button The button index, beginning at 1.
|
||||
* @return Whether the joystick button was released since the last check.
|
||||
*/
|
||||
bool DriverStation::GetStickButtonReleased(int stick, int button) {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -235,15 +192,6 @@ bool DriverStation::GetStickButtonReleased(int stick, int button) {
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the value of the axis on a joystick.
|
||||
*
|
||||
* This depends on the mapping of the joystick connected to the specified port.
|
||||
*
|
||||
* @param stick The joystick to read.
|
||||
* @param axis The analog axis value to read from the joystick.
|
||||
* @return The value of the axis on the joystick.
|
||||
*/
|
||||
double DriverStation::GetStickAxis(int stick, int axis) {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -266,11 +214,6 @@ double DriverStation::GetStickAxis(int stick, int axis) {
|
||||
return m_joystickAxes[stick].axes[axis];
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the state of a POV on the joystick.
|
||||
*
|
||||
* @return the angle of the POV in degrees, or -1 if the POV is not pressed.
|
||||
*/
|
||||
int DriverStation::GetStickPOV(int stick, int pov) {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -293,12 +236,6 @@ int DriverStation::GetStickPOV(int stick, int pov) {
|
||||
return m_joystickPOVs[stick].povs[pov];
|
||||
}
|
||||
|
||||
/**
|
||||
* The state of the buttons on the joystick.
|
||||
*
|
||||
* @param stick The joystick to read.
|
||||
* @return The state of the buttons on the joystick.
|
||||
*/
|
||||
int DriverStation::GetStickButtons(int stick) const {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -309,12 +246,6 @@ int DriverStation::GetStickButtons(int stick) const {
|
||||
return m_joystickButtons[stick].buttons;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the number of axes on a given joystick port.
|
||||
*
|
||||
* @param stick The joystick port number
|
||||
* @return The number of axes on the indicated joystick
|
||||
*/
|
||||
int DriverStation::GetStickAxisCount(int stick) const {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -325,12 +256,6 @@ int DriverStation::GetStickAxisCount(int stick) const {
|
||||
return m_joystickAxes[stick].count;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the number of POVs on a given joystick port.
|
||||
*
|
||||
* @param stick The joystick port number
|
||||
* @return The number of POVs on the indicated joystick
|
||||
*/
|
||||
int DriverStation::GetStickPOVCount(int stick) const {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -341,12 +266,6 @@ int DriverStation::GetStickPOVCount(int stick) const {
|
||||
return m_joystickPOVs[stick].count;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the number of buttons on a given joystick port.
|
||||
*
|
||||
* @param stick The joystick port number
|
||||
* @return The number of buttons on the indicated joystick
|
||||
*/
|
||||
int DriverStation::GetStickButtonCount(int stick) const {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -357,12 +276,6 @@ int DriverStation::GetStickButtonCount(int stick) const {
|
||||
return m_joystickButtons[stick].count;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a boolean indicating if the controller is an xbox controller.
|
||||
*
|
||||
* @param stick The joystick port number
|
||||
* @return A boolean that is true if the controller is an xbox controller.
|
||||
*/
|
||||
bool DriverStation::GetJoystickIsXbox(int stick) const {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -373,12 +286,6 @@ bool DriverStation::GetJoystickIsXbox(int stick) const {
|
||||
return static_cast<bool>(m_joystickDescriptor[stick].isXbox);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the type of joystick at a given port.
|
||||
*
|
||||
* @param stick The joystick port number
|
||||
* @return The HID type of joystick at the given port
|
||||
*/
|
||||
int DriverStation::GetJoystickType(int stick) const {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -389,12 +296,6 @@ int DriverStation::GetJoystickType(int stick) const {
|
||||
return static_cast<int>(m_joystickDescriptor[stick].type);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the name of the joystick at the given port.
|
||||
*
|
||||
* @param stick The joystick port number
|
||||
* @return The name of the joystick at the given port
|
||||
*/
|
||||
std::string DriverStation::GetJoystickName(int stick) const {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -404,12 +305,6 @@ std::string DriverStation::GetJoystickName(int stick) const {
|
||||
return m_joystickDescriptor[stick].name;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the types of Axes on a given joystick port.
|
||||
*
|
||||
* @param stick The joystick port number and the target axis
|
||||
* @return What type of axis the axis is reporting to be
|
||||
*/
|
||||
int DriverStation::GetJoystickAxisType(int stick, int axis) const {
|
||||
if (stick < 0 || stick >= kJoystickPorts) {
|
||||
wpi_setWPIError(BadJoystickIndex);
|
||||
@@ -420,104 +315,50 @@ int DriverStation::GetJoystickAxisType(int stick, int axis) const {
|
||||
return m_joystickDescriptor[stick].axisTypes[axis];
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the DS has enabled the robot.
|
||||
*
|
||||
* @return True if the robot is enabled and the DS is connected
|
||||
*/
|
||||
bool DriverStation::IsEnabled() const {
|
||||
HAL_ControlWord controlWord;
|
||||
UpdateControlWord(false, controlWord);
|
||||
return controlWord.enabled && controlWord.dsAttached;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the robot is disabled.
|
||||
*
|
||||
* @return True if the robot is explicitly disabled or the DS is not connected
|
||||
*/
|
||||
bool DriverStation::IsDisabled() const {
|
||||
HAL_ControlWord controlWord;
|
||||
UpdateControlWord(false, controlWord);
|
||||
return !(controlWord.enabled && controlWord.dsAttached);
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the DS is commanding autonomous mode.
|
||||
*
|
||||
* @return True if the robot is being commanded to be in autonomous mode
|
||||
*/
|
||||
bool DriverStation::IsAutonomous() const {
|
||||
HAL_ControlWord controlWord;
|
||||
UpdateControlWord(false, controlWord);
|
||||
return controlWord.autonomous;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the DS is commanding teleop mode.
|
||||
*
|
||||
* @return True if the robot is being commanded to be in teleop mode
|
||||
*/
|
||||
bool DriverStation::IsOperatorControl() const {
|
||||
HAL_ControlWord controlWord;
|
||||
UpdateControlWord(false, controlWord);
|
||||
return !(controlWord.autonomous || controlWord.test);
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the DS is commanding test mode.
|
||||
*
|
||||
* @return True if the robot is being commanded to be in test mode
|
||||
*/
|
||||
bool DriverStation::IsTest() const {
|
||||
HAL_ControlWord controlWord;
|
||||
UpdateControlWord(false, controlWord);
|
||||
return controlWord.test;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the DS is attached.
|
||||
*
|
||||
* @return True if the DS is connected to the robot
|
||||
*/
|
||||
bool DriverStation::IsDSAttached() const {
|
||||
HAL_ControlWord controlWord;
|
||||
UpdateControlWord(false, controlWord);
|
||||
return controlWord.dsAttached;
|
||||
}
|
||||
|
||||
/**
|
||||
* Has a new control packet from the driver station arrived since the last time
|
||||
* this function was called?
|
||||
*
|
||||
* Warning: If you call this function from more than one place at the same time,
|
||||
* you will not get the intended behavior.
|
||||
*
|
||||
* @return True if the control data has been updated since the last call.
|
||||
*/
|
||||
bool DriverStation::IsNewControlData() const { return HAL_IsNewControlData(); }
|
||||
|
||||
/**
|
||||
* 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 DriverStation::IsFMSAttached() const {
|
||||
HAL_ControlWord controlWord;
|
||||
UpdateControlWord(false, controlWord);
|
||||
return controlWord.fmsAttached;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the FPGA outputs are enabled.
|
||||
*
|
||||
* The outputs may be disabled if the robot is disabled or e-stopped, the
|
||||
* watchdog has expired, or if the roboRIO browns out.
|
||||
*
|
||||
* @return True if the FPGA outputs are enabled.
|
||||
* @deprecated Use RobotController static class method
|
||||
*/
|
||||
bool DriverStation::IsSysActive() const {
|
||||
int32_t status = 0;
|
||||
bool retVal = HAL_GetSystemActive(&status);
|
||||
@@ -525,12 +366,6 @@ bool DriverStation::IsSysActive() const {
|
||||
return retVal;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the system is browned out.
|
||||
*
|
||||
* @return True if the system is browned out
|
||||
* @deprecated Use RobotController static class method
|
||||
*/
|
||||
bool DriverStation::IsBrownedOut() const {
|
||||
int32_t status = 0;
|
||||
bool retVal = HAL_GetBrownedOut(&status);
|
||||
@@ -563,13 +398,6 @@ int DriverStation::GetReplayNumber() const {
|
||||
return m_matchInfo->replayNumber;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the alliance that the driver station says it is on.
|
||||
*
|
||||
* This could return kRed or kBlue.
|
||||
*
|
||||
* @return The Alliance enum (kRed, kBlue or kInvalid)
|
||||
*/
|
||||
DriverStation::Alliance DriverStation::GetAlliance() const {
|
||||
int32_t status = 0;
|
||||
auto allianceStationID = HAL_GetAllianceStation(&status);
|
||||
@@ -587,13 +415,6 @@ DriverStation::Alliance DriverStation::GetAlliance() const {
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the driver station location on the field.
|
||||
*
|
||||
* This could return 1, 2, or 3.
|
||||
*
|
||||
* @return The location of the driver station (1-3, 0 for invalid)
|
||||
*/
|
||||
int DriverStation::GetLocation() const {
|
||||
int32_t status = 0;
|
||||
auto allianceStationID = HAL_GetAllianceStation(&status);
|
||||
@@ -612,32 +433,8 @@ int DriverStation::GetLocation() const {
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Wait until a new packet comes from the driver station.
|
||||
*
|
||||
* This blocks on a semaphore, so the waiting is efficient.
|
||||
*
|
||||
* This is a good way to delay processing until there is new driver station data
|
||||
* to act on.
|
||||
*/
|
||||
void DriverStation::WaitForData() { WaitForData(0); }
|
||||
|
||||
/**
|
||||
* Wait until a new packet comes from the driver station, or wait for a timeout.
|
||||
*
|
||||
* If the timeout is less then or equal to 0, wait indefinitely.
|
||||
*
|
||||
* Timeout is in milliseconds
|
||||
*
|
||||
* This blocks on a semaphore, so the waiting is efficient.
|
||||
*
|
||||
* This is a good way to delay processing until there is new driver station data
|
||||
* to act on.
|
||||
*
|
||||
* @param timeout Timeout time in seconds
|
||||
*
|
||||
* @return true if new data, otherwise false
|
||||
*/
|
||||
bool DriverStation::WaitForData(double timeout) {
|
||||
auto timeoutTime =
|
||||
std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
|
||||
@@ -657,31 +454,11 @@ bool DriverStation::WaitForData(double timeout) {
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the approximate match time.
|
||||
*
|
||||
* The FMS does not send an official match time to the robots, but does send an
|
||||
* approximate match time. The value will count down the time remaining in the
|
||||
* current period (auto or teleop).
|
||||
*
|
||||
* Warning: This is not an official time (so it cannot be used to dispute ref
|
||||
* calls or guarantee that a function will trigger before the match ends).
|
||||
*
|
||||
* The Practice Match function of the DS approximates the behaviour seen on the
|
||||
* field.
|
||||
*
|
||||
* @return Time remaining in current match period (auto or teleop)
|
||||
*/
|
||||
double DriverStation::GetMatchTime() const {
|
||||
int32_t status;
|
||||
return HAL_GetMatchTime(&status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Read the battery voltage.
|
||||
*
|
||||
* @return The battery voltage in Volts.
|
||||
*/
|
||||
double DriverStation::GetBatteryVoltage() const {
|
||||
int32_t status = 0;
|
||||
double voltage = HAL_GetVinVoltage(&status);
|
||||
@@ -690,6 +467,155 @@ double DriverStation::GetBatteryVoltage() const {
|
||||
return voltage;
|
||||
}
|
||||
|
||||
void DriverStation::GetData() {
|
||||
// Get the status of all of the joysticks, and save to the cache
|
||||
for (uint8_t stick = 0; stick < kJoystickPorts; stick++) {
|
||||
HAL_GetJoystickAxes(stick, &m_joystickAxesCache[stick]);
|
||||
HAL_GetJoystickPOVs(stick, &m_joystickPOVsCache[stick]);
|
||||
HAL_GetJoystickButtons(stick, &m_joystickButtonsCache[stick]);
|
||||
HAL_GetJoystickDescriptor(stick, &m_joystickDescriptorCache[stick]);
|
||||
}
|
||||
// Grab match specific data
|
||||
HAL_MatchInfo matchInfo;
|
||||
auto status = HAL_GetMatchInfo(&matchInfo);
|
||||
if (status == 0) {
|
||||
m_matchInfoCache->eventName = matchInfo.eventName;
|
||||
m_matchInfoCache->matchNumber = matchInfo.matchNumber;
|
||||
m_matchInfoCache->replayNumber = matchInfo.replayNumber;
|
||||
m_matchInfoCache->matchType =
|
||||
static_cast<DriverStation::MatchType>(matchInfo.matchType);
|
||||
m_matchInfoCache->gameSpecificMessage = matchInfo.gameSpecificMessage;
|
||||
}
|
||||
HAL_FreeMatchInfo(&matchInfo);
|
||||
|
||||
// Force a control word update, to make sure the data is the newest.
|
||||
HAL_ControlWord controlWord;
|
||||
UpdateControlWord(true, controlWord);
|
||||
|
||||
{
|
||||
// Obtain a write lock on the data, swap the cached data into the
|
||||
// main data arrays
|
||||
std::lock_guard<wpi::mutex> lock(m_cacheDataMutex);
|
||||
|
||||
for (int32_t i = 0; i < kJoystickPorts; i++) {
|
||||
// If buttons weren't pressed and are now, set flags in m_buttonsPressed
|
||||
m_joystickButtonsPressed[i] |=
|
||||
~m_joystickButtons[i].buttons & m_joystickButtonsCache[i].buttons;
|
||||
|
||||
// If buttons were pressed and aren't now, set flags in m_buttonsReleased
|
||||
m_joystickButtonsReleased[i] |=
|
||||
m_joystickButtons[i].buttons & ~m_joystickButtonsCache[i].buttons;
|
||||
}
|
||||
|
||||
m_joystickAxes.swap(m_joystickAxesCache);
|
||||
m_joystickPOVs.swap(m_joystickPOVsCache);
|
||||
m_joystickButtons.swap(m_joystickButtonsCache);
|
||||
m_joystickDescriptor.swap(m_joystickDescriptorCache);
|
||||
m_matchInfo.swap(m_matchInfoCache);
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<wpi::mutex> waitLock(m_waitForDataMutex);
|
||||
// Nofify all threads
|
||||
m_waitForDataCounter++;
|
||||
m_waitForDataCond.notify_all();
|
||||
}
|
||||
|
||||
SendMatchData();
|
||||
}
|
||||
|
||||
DriverStation::DriverStation() {
|
||||
HAL_Initialize(500, 0);
|
||||
m_waitForDataCounter = 0;
|
||||
m_joystickAxes = std::make_unique<HAL_JoystickAxes[]>(kJoystickPorts);
|
||||
m_joystickPOVs = std::make_unique<HAL_JoystickPOVs[]>(kJoystickPorts);
|
||||
m_joystickButtons = std::make_unique<HAL_JoystickButtons[]>(kJoystickPorts);
|
||||
m_joystickDescriptor =
|
||||
std::make_unique<HAL_JoystickDescriptor[]>(kJoystickPorts);
|
||||
m_matchInfo = std::make_unique<MatchInfoData>();
|
||||
m_joystickAxesCache = std::make_unique<HAL_JoystickAxes[]>(kJoystickPorts);
|
||||
m_joystickPOVsCache = std::make_unique<HAL_JoystickPOVs[]>(kJoystickPorts);
|
||||
m_joystickButtonsCache =
|
||||
std::make_unique<HAL_JoystickButtons[]>(kJoystickPorts);
|
||||
m_joystickDescriptorCache =
|
||||
std::make_unique<HAL_JoystickDescriptor[]>(kJoystickPorts);
|
||||
m_matchInfoCache = std::make_unique<MatchInfoData>();
|
||||
|
||||
m_matchDataSender = std::make_unique<MatchDataSender>();
|
||||
|
||||
// All joysticks should default to having zero axes, povs and buttons, so
|
||||
// uninitialized memory doesn't get sent to speed controllers.
|
||||
for (unsigned int i = 0; i < kJoystickPorts; i++) {
|
||||
m_joystickAxes[i].count = 0;
|
||||
m_joystickPOVs[i].count = 0;
|
||||
m_joystickButtons[i].count = 0;
|
||||
m_joystickDescriptor[i].isXbox = 0;
|
||||
m_joystickDescriptor[i].type = -1;
|
||||
m_joystickDescriptor[i].name[0] = '\0';
|
||||
|
||||
m_joystickAxesCache[i].count = 0;
|
||||
m_joystickPOVsCache[i].count = 0;
|
||||
m_joystickButtonsCache[i].count = 0;
|
||||
m_joystickDescriptorCache[i].isXbox = 0;
|
||||
m_joystickDescriptorCache[i].type = -1;
|
||||
m_joystickDescriptorCache[i].name[0] = '\0';
|
||||
|
||||
m_joystickButtonsPressed[i] = 0;
|
||||
m_joystickButtonsReleased[i] = 0;
|
||||
}
|
||||
|
||||
m_dsThread = std::thread(&DriverStation::Run, this);
|
||||
}
|
||||
|
||||
void DriverStation::ReportJoystickUnpluggedError(const wpi::Twine& message) {
|
||||
double currentTime = Timer::GetFPGATimestamp();
|
||||
if (currentTime > m_nextMessageTime) {
|
||||
ReportError(message);
|
||||
m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
|
||||
}
|
||||
}
|
||||
|
||||
void DriverStation::ReportJoystickUnpluggedWarning(const wpi::Twine& message) {
|
||||
double currentTime = Timer::GetFPGATimestamp();
|
||||
if (currentTime > m_nextMessageTime) {
|
||||
ReportWarning(message);
|
||||
m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
|
||||
}
|
||||
}
|
||||
|
||||
void DriverStation::Run() {
|
||||
m_isRunning = true;
|
||||
int safetyCounter = 0;
|
||||
while (m_isRunning) {
|
||||
HAL_WaitForDSData();
|
||||
GetData();
|
||||
|
||||
if (IsDisabled()) safetyCounter = 0;
|
||||
|
||||
if (++safetyCounter >= 4) {
|
||||
MotorSafetyHelper::CheckMotors();
|
||||
safetyCounter = 0;
|
||||
}
|
||||
if (m_userInDisabled) HAL_ObserveUserProgramDisabled();
|
||||
if (m_userInAutonomous) HAL_ObserveUserProgramAutonomous();
|
||||
if (m_userInTeleop) HAL_ObserveUserProgramTeleop();
|
||||
if (m_userInTest) HAL_ObserveUserProgramTest();
|
||||
}
|
||||
}
|
||||
|
||||
void DriverStation::UpdateControlWord(bool force,
|
||||
HAL_ControlWord& controlWord) const {
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
std::lock_guard<wpi::mutex> lock(m_controlWordMutex);
|
||||
// Update every 50 ms or on force.
|
||||
if ((now - m_lastControlWordUpdate > std::chrono::milliseconds(50)) ||
|
||||
force) {
|
||||
HAL_GetControlWord(&m_controlWordCache);
|
||||
m_lastControlWordUpdate = now;
|
||||
}
|
||||
controlWord = m_controlWordCache;
|
||||
}
|
||||
|
||||
void DriverStation::SendMatchData() {
|
||||
int32_t status = 0;
|
||||
HAL_AllianceStationID alliance = HAL_GetAllianceStation(&status);
|
||||
@@ -748,181 +674,3 @@ void DriverStation::SendMatchData() {
|
||||
std::memcpy(&wordInt, &ctlWord, sizeof(wordInt));
|
||||
m_matchDataSender->controlWord.SetDouble(wordInt);
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 loop.
|
||||
*/
|
||||
void DriverStation::GetData() {
|
||||
// Get the status of all of the joysticks, and save to the cache
|
||||
for (uint8_t stick = 0; stick < kJoystickPorts; stick++) {
|
||||
HAL_GetJoystickAxes(stick, &m_joystickAxesCache[stick]);
|
||||
HAL_GetJoystickPOVs(stick, &m_joystickPOVsCache[stick]);
|
||||
HAL_GetJoystickButtons(stick, &m_joystickButtonsCache[stick]);
|
||||
HAL_GetJoystickDescriptor(stick, &m_joystickDescriptorCache[stick]);
|
||||
}
|
||||
// Grab match specific data
|
||||
HAL_MatchInfo matchInfo;
|
||||
auto status = HAL_GetMatchInfo(&matchInfo);
|
||||
if (status == 0) {
|
||||
m_matchInfoCache->eventName = matchInfo.eventName;
|
||||
m_matchInfoCache->matchNumber = matchInfo.matchNumber;
|
||||
m_matchInfoCache->replayNumber = matchInfo.replayNumber;
|
||||
m_matchInfoCache->matchType =
|
||||
static_cast<DriverStation::MatchType>(matchInfo.matchType);
|
||||
m_matchInfoCache->gameSpecificMessage = matchInfo.gameSpecificMessage;
|
||||
}
|
||||
HAL_FreeMatchInfo(&matchInfo);
|
||||
|
||||
// Force a control word update, to make sure the data is the newest.
|
||||
HAL_ControlWord controlWord;
|
||||
UpdateControlWord(true, controlWord);
|
||||
|
||||
{
|
||||
// Obtain a write lock on the data, swap the cached data into the
|
||||
// main data arrays
|
||||
std::lock_guard<wpi::mutex> lock(m_cacheDataMutex);
|
||||
|
||||
for (int32_t i = 0; i < kJoystickPorts; i++) {
|
||||
// If buttons weren't pressed and are now, set flags in m_buttonsPressed
|
||||
m_joystickButtonsPressed[i] |=
|
||||
~m_joystickButtons[i].buttons & m_joystickButtonsCache[i].buttons;
|
||||
|
||||
// If buttons were pressed and aren't now, set flags in m_buttonsReleased
|
||||
m_joystickButtonsReleased[i] |=
|
||||
m_joystickButtons[i].buttons & ~m_joystickButtonsCache[i].buttons;
|
||||
}
|
||||
|
||||
m_joystickAxes.swap(m_joystickAxesCache);
|
||||
m_joystickPOVs.swap(m_joystickPOVsCache);
|
||||
m_joystickButtons.swap(m_joystickButtonsCache);
|
||||
m_joystickDescriptor.swap(m_joystickDescriptorCache);
|
||||
m_matchInfo.swap(m_matchInfoCache);
|
||||
}
|
||||
|
||||
{
|
||||
std::lock_guard<wpi::mutex> waitLock(m_waitForDataMutex);
|
||||
// Nofify all threads
|
||||
m_waitForDataCounter++;
|
||||
m_waitForDataCond.notify_all();
|
||||
}
|
||||
|
||||
SendMatchData();
|
||||
}
|
||||
|
||||
/**
|
||||
* DriverStation constructor.
|
||||
*
|
||||
* This is only called once the first time GetInstance() is called
|
||||
*/
|
||||
DriverStation::DriverStation() {
|
||||
HAL_Initialize(500, 0);
|
||||
m_waitForDataCounter = 0;
|
||||
m_joystickAxes = std::make_unique<HAL_JoystickAxes[]>(kJoystickPorts);
|
||||
m_joystickPOVs = std::make_unique<HAL_JoystickPOVs[]>(kJoystickPorts);
|
||||
m_joystickButtons = std::make_unique<HAL_JoystickButtons[]>(kJoystickPorts);
|
||||
m_joystickDescriptor =
|
||||
std::make_unique<HAL_JoystickDescriptor[]>(kJoystickPorts);
|
||||
m_matchInfo = std::make_unique<MatchInfoData>();
|
||||
m_joystickAxesCache = std::make_unique<HAL_JoystickAxes[]>(kJoystickPorts);
|
||||
m_joystickPOVsCache = std::make_unique<HAL_JoystickPOVs[]>(kJoystickPorts);
|
||||
m_joystickButtonsCache =
|
||||
std::make_unique<HAL_JoystickButtons[]>(kJoystickPorts);
|
||||
m_joystickDescriptorCache =
|
||||
std::make_unique<HAL_JoystickDescriptor[]>(kJoystickPorts);
|
||||
m_matchInfoCache = std::make_unique<MatchInfoData>();
|
||||
|
||||
m_matchDataSender = std::make_unique<MatchDataSender>();
|
||||
|
||||
// All joysticks should default to having zero axes, povs and buttons, so
|
||||
// uninitialized memory doesn't get sent to speed controllers.
|
||||
for (unsigned int i = 0; i < kJoystickPorts; i++) {
|
||||
m_joystickAxes[i].count = 0;
|
||||
m_joystickPOVs[i].count = 0;
|
||||
m_joystickButtons[i].count = 0;
|
||||
m_joystickDescriptor[i].isXbox = 0;
|
||||
m_joystickDescriptor[i].type = -1;
|
||||
m_joystickDescriptor[i].name[0] = '\0';
|
||||
|
||||
m_joystickAxesCache[i].count = 0;
|
||||
m_joystickPOVsCache[i].count = 0;
|
||||
m_joystickButtonsCache[i].count = 0;
|
||||
m_joystickDescriptorCache[i].isXbox = 0;
|
||||
m_joystickDescriptorCache[i].type = -1;
|
||||
m_joystickDescriptorCache[i].name[0] = '\0';
|
||||
|
||||
m_joystickButtonsPressed[i] = 0;
|
||||
m_joystickButtonsReleased[i] = 0;
|
||||
}
|
||||
|
||||
m_dsThread = std::thread(&DriverStation::Run, this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Reports errors related to unplugged joysticks
|
||||
* Throttles the errors so that they don't overwhelm the DS
|
||||
*/
|
||||
void DriverStation::ReportJoystickUnpluggedError(const wpi::Twine& message) {
|
||||
double currentTime = Timer::GetFPGATimestamp();
|
||||
if (currentTime > m_nextMessageTime) {
|
||||
ReportError(message);
|
||||
m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Reports errors related to unplugged joysticks.
|
||||
*
|
||||
* Throttles the errors so that they don't overwhelm the DS.
|
||||
*/
|
||||
void DriverStation::ReportJoystickUnpluggedWarning(const wpi::Twine& message) {
|
||||
double currentTime = Timer::GetFPGATimestamp();
|
||||
if (currentTime > m_nextMessageTime) {
|
||||
ReportWarning(message);
|
||||
m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
|
||||
}
|
||||
}
|
||||
|
||||
void DriverStation::Run() {
|
||||
m_isRunning = true;
|
||||
int safetyCounter = 0;
|
||||
while (m_isRunning) {
|
||||
HAL_WaitForDSData();
|
||||
GetData();
|
||||
|
||||
if (IsDisabled()) safetyCounter = 0;
|
||||
|
||||
if (++safetyCounter >= 4) {
|
||||
MotorSafetyHelper::CheckMotors();
|
||||
safetyCounter = 0;
|
||||
}
|
||||
if (m_userInDisabled) HAL_ObserveUserProgramDisabled();
|
||||
if (m_userInAutonomous) HAL_ObserveUserProgramAutonomous();
|
||||
if (m_userInTeleop) HAL_ObserveUserProgramTeleop();
|
||||
if (m_userInTest) HAL_ObserveUserProgramTest();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets ControlWord data from the cache. If 50ms has passed, or the force
|
||||
* parameter is set, the cached data is updated. Otherwise the data is just
|
||||
* copied from the cache.
|
||||
*
|
||||
* @param force True to force an update to the cache, otherwise update if 50ms
|
||||
* have passed.
|
||||
* @param controlWord Structure to put the return control word data into.
|
||||
*/
|
||||
void DriverStation::UpdateControlWord(bool force,
|
||||
HAL_ControlWord& controlWord) const {
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
std::lock_guard<wpi::mutex> lock(m_controlWordMutex);
|
||||
// Update every 50 ms or on force.
|
||||
if ((now - m_lastControlWordUpdate > std::chrono::milliseconds(50)) ||
|
||||
force) {
|
||||
HAL_GetControlWord(&m_controlWordCache);
|
||||
m_lastControlWordUpdate = now;
|
||||
}
|
||||
controlWord = m_controlWordCache;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user