Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Reordered DriverStation functions in wpilibc and wpilibj to match wpilibc header (#166)

Browse Source
This commit is contained in:
Tyler Veness
2016-07-14 20:50:38 -07:00
committed by Peter Johnson
parent c93b5bedf9
commit 1bf2d58db1
3 changed files with 479 additions and 476 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpilibc/athena/include/DriverStation.h
View File

@@ -90,12 +90,10 @@ class DriverStation : public SensorBase, public RobotStateInterface {
void InTest(bool entering) { m_userInTest = entering; }
protected:
DriverStation();
void GetData();
private:
static DriverStation* m_instance;
DriverStation();
void ReportJoystickUnpluggedError(std::string message);
void ReportJoystickUnpluggedWarning(std::string message);
void Run();

512
wpilibc/athena/src/DriverStation.cpp
View File

@@ -19,73 +19,11 @@ const double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
const uint32_t DriverStation::kJoystickPorts;
/**
* DriverStation constructor.
*
* This is only called once the first time GetInstance() is called
*/
DriverStation::DriverStation() {
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_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);
// 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_task = Task("DriverStation", &DriverStation::Run, this);
}
DriverStation::~DriverStation() {
m_isRunning = false;
m_task.join();
}
void DriverStation::Run() {
m_isRunning = true;
int period = 0;
while (m_isRunning) {
HAL_WaitForDSData();
GetData();
{
std::lock_guard<priority_mutex> lock(m_waitForDataMutex);
m_updatedControlLoopData = true;
}
m_waitForDataCond.notify_all();
if (++period >= 4) {
MotorSafetyHelper::CheckMotors();
period = 0;
}
if (m_userInDisabled) HAL_ObserveUserProgramDisabled();
if (m_userInAutonomous) HAL_ObserveUserProgramAutonomous();
if (m_userInTeleop) HAL_ObserveUserProgramTeleop();
if (m_userInTest) HAL_ObserveUserProgramTest();
}
}
/**
* Return a pointer to the singleton DriverStation.
*
@@ -97,171 +35,34 @@ DriverStation& DriverStation::GetInstance() {
}
/**
* Copy data from the DS task for the user.
* Report an error to the DriverStation messages window.
*
* If no new data exists, it will just be returned, otherwise
* the data will be copied from the DS polling loop.
* The error is also printed to the program console.
*/
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]);
}
// Obtain a write lock on the data, swap the cached data into the
// main data arrays
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
m_joystickAxes.swap(m_joystickAxesCache);
m_joystickPOVs.swap(m_joystickPOVsCache);
m_joystickButtons.swap(m_joystickButtonsCache);
m_joystickDescriptor.swap(m_joystickDescriptorCache);
m_newControlData.give();
void DriverStation::ReportError(std::string error) {
HAL_SendError(1, 1, 0, error.c_str(), "", "", 1);
}
/**
* Read the battery voltage.
* Report a warning to the DriverStation messages window.
*
* @return The battery voltage in Volts.
* The warning is also printed to the program console.
*/
float DriverStation::GetBatteryVoltage() const {
int32_t status = 0;
float voltage = HAL_GetVinVoltage(&status);
wpi_setErrorWithContext(status, "getVinVoltage");
return voltage;
void DriverStation::ReportWarning(std::string error) {
HAL_SendError(0, 1, 0, error.c_str(), "", "", 1);
}
/**
* Reports errors related to unplugged joysticks
* Throttles the errors so that they don't overwhelm the DS
*/
void DriverStation::ReportJoystickUnpluggedError(std::string message) {
double currentTime = Timer::GetFPGATimestamp();
if (currentTime > m_nextMessageTime) {
ReportError(message);
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
}
}
/**
* Reports errors related to unplugged joysticks.
* Report an error to the DriverStation messages window.
*
* Throttles the errors so that they don't overwhelm the DS.
* The error is also printed to the program console.
*/
void DriverStation::ReportJoystickUnpluggedWarning(std::string message) {
double currentTime = Timer::GetFPGATimestamp();
if (currentTime > m_nextMessageTime) {
ReportWarning(message);
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
}
}
/**
* 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(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
return m_joystickAxes[stick].count;
}
/**
* 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(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
std::string retVal(m_joystickDescriptor[stick].name);
return retVal;
}
/**
* 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(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return -1;
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
return static_cast<int>(m_joystickDescriptor[stick].type);
}
/**
* 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(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return false;
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
return static_cast<bool>(m_joystickDescriptor[stick].isXbox);
}
/**
* 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(uint32_t stick, uint8_t axis) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return -1;
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
return m_joystickDescriptor[stick].axisTypes[axis];
}
/**
* 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(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
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(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
return m_joystickButtons[stick].count;
void DriverStation::ReportError(bool is_error, int32_t code,
const std::string& error,
const std::string& location,
const std::string& stack) {
HAL_SendError(is_error, code, 0, error.c_str(), location.c_str(),
stack.c_str(), 1);
}
/**
@@ -364,6 +165,111 @@ bool DriverStation::GetStickButton(uint32_t stick, uint8_t button) {
return ((0x1 << (button - 1)) & m_joystickButtons[stick].buttons) != 0;
}
/**
* 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(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
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(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
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(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return 0;
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
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(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return false;
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
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(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return -1;
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
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(uint32_t stick) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
std::string retVal(m_joystickDescriptor[stick].name);
return retVal;
}
/**
* 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(uint32_t stick, uint8_t axis) const {
if (stick >= kJoystickPorts) {
wpi_setWPIError(BadJoystickIndex);
return -1;
}
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
return m_joystickDescriptor[stick].axisTypes[axis];
}
/**
* Check if the DS has enabled the robot.
*
@@ -430,6 +336,31 @@ bool DriverStation::IsDSAttached() const {
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 m_newControlData.tryTake() == false;
}
/**
* 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;
HAL_GetControlWord(&controlWord);
return controlWord.fmsAttached;
}
/**
* Check if the FPGA outputs are enabled.
*
@@ -457,31 +388,6 @@ bool DriverStation::IsBrownedOut() const {
return retVal;
}
/**
* 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 m_newControlData.tryTake() == false;
}
/**
* 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;
HAL_GetControlWord(&controlWord);
return controlWord.fmsAttached;
}
/**
* Return the alliance that the driver station says it is on.
*
@@ -568,32 +474,126 @@ double DriverStation::GetMatchTime() const {
}
/**
* Report an error to the DriverStation messages window.
* Read the battery voltage.
*
* The error is also printed to the program console.
* @return The battery voltage in Volts.
*/
void DriverStation::ReportError(std::string error) {
HAL_SendError(1, 1, 0, error.c_str(), "", "", 1);
float DriverStation::GetBatteryVoltage() const {
int32_t status = 0;
float voltage = HAL_GetVinVoltage(&status);
wpi_setErrorWithContext(status, "getVinVoltage");
return voltage;
}
/**
* Report a warning to the DriverStation messages window.
* Copy data from the DS task for the user.
*
* The warning is also printed to the program console.
* If no new data exists, it will just be returned, otherwise
* the data will be copied from the DS polling loop.
*/
void DriverStation::ReportWarning(std::string error) {
HAL_SendError(0, 1, 0, error.c_str(), "", "", 1);
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]);
}
// Obtain a write lock on the data, swap the cached data into the
// main data arrays
std::lock_guard<priority_mutex> lock(m_joystickDataMutex);
m_joystickAxes.swap(m_joystickAxesCache);
m_joystickPOVs.swap(m_joystickPOVsCache);
m_joystickButtons.swap(m_joystickButtonsCache);
m_joystickDescriptor.swap(m_joystickDescriptorCache);
m_newControlData.give();
}
/**
* Report an error to the DriverStation messages window.
* DriverStation constructor.
*
* The error is also printed to the program console.
* This is only called once the first time GetInstance() is called
*/
void DriverStation::ReportError(bool is_error, int32_t code,
const std::string& error,
const std::string& location,
const std::string& stack) {
HAL_SendError(is_error, code, 0, error.c_str(), location.c_str(),
stack.c_str(), 1);
DriverStation::DriverStation() {
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_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);
// 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_task = Task("DriverStation", &DriverStation::Run, this);
}
/**
* Reports errors related to unplugged joysticks
* Throttles the errors so that they don't overwhelm the DS
*/
void DriverStation::ReportJoystickUnpluggedError(std::string message) {
double currentTime = Timer::GetFPGATimestamp();
if (currentTime > m_nextMessageTime) {
ReportError(message);
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
}
}
/**
* Reports errors related to unplugged joysticks.
*
* Throttles the errors so that they don't overwhelm the DS.
*/
void DriverStation::ReportJoystickUnpluggedWarning(std::string message) {
double currentTime = Timer::GetFPGATimestamp();
if (currentTime > m_nextMessageTime) {
ReportWarning(message);
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
}
}
void DriverStation::Run() {
m_isRunning = true;
int period = 0;
while (m_isRunning) {
HAL_WaitForDSData();
GetData();
{
std::lock_guard<priority_mutex> lock(m_waitForDataMutex);
m_updatedControlLoopData = true;
}
m_waitForDataCond.notify_all();
if (++period >= 4) {
MotorSafetyHelper::CheckMotors();
period = 0;
}
if (m_userInDisabled) HAL_ObserveUserProgramDisabled();
if (m_userInAutonomous) HAL_ObserveUserProgramAutonomous();
if (m_userInTeleop) HAL_ObserveUserProgramTeleop();
if (m_userInTest) HAL_ObserveUserProgramTest();
}
}

439
wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java
View File

@@ -67,7 +67,7 @@ public class DriverStation implements RobotState.Interface {
}
public void run() {
m_ds.task();
m_ds.run();
}
} /* DriverStationTask */
@@ -116,7 +116,7 @@ public class DriverStation implements RobotState.Interface {
* <p>The single DriverStation instance is created statically with the instance static member
* variable.
*/
protected DriverStation() {
private DriverStation() {
m_dataSem = new Object();
m_joystickMutex = new Object();
m_newControlDataMutex = new Object();
@@ -144,128 +144,46 @@ public class DriverStation implements RobotState.Interface {
}
/**
* Provides the service routine for the DS polling m_thread.
*/
private void task() {
int safetyCounter = 0;
while (m_threadKeepAlive) {
HAL.waitForDSData();
getData();
synchronized (m_dataSem) {
m_updatedControlLoopData = true;
m_dataSem.notifyAll();
}
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();
}
}
}
/**
* Wait for new data from the driver station.
*/
public void waitForData() {
waitForData(0);
}
/**
* Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data
* only.
* Report error to Driver Station. Also prints error to System.err Optionally appends Stack trace
* to error message.
*
* @param timeout The maximum time in milliseconds to wait.
* @param printTrace If true, append stack trace to error string
*/
public void waitForData(long timeout) {
synchronized (m_dataSem) {
try {
while (!m_updatedControlLoopData) {
m_dataSem.wait(timeout);
}
m_updatedControlLoopData = false;
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
public static void reportError(String error, boolean printTrace) {
reportErrorImpl(true, 1, error, printTrace);
}
/**
* Report warning to Driver Station. Also prints error to System.err Optionally appends Stack
* trace to warning message.
*
* @param printTrace If true, append stack trace to warning string
*/
public static void reportWarning(String error, boolean printTrace) {
reportErrorImpl(false, 1, error, printTrace);
}
private static void reportErrorImpl(boolean isError, int code, String error, boolean
printTrace) {
StackTraceElement[] traces = Thread.currentThread().getStackTrace();
String locString;
if (traces.length > 3) {
locString = traces[3].toString();
} else {
locString = new String();
}
boolean haveLoc = false;
String traceString = " at ";
for (int i = 3; i < traces.length; i++) {
String loc = traces[i].toString();
traceString += loc + "\n";
// get first user function
if (!haveLoc && !loc.startsWith("edu.wpi.first.wpilibj")) {
locString = loc;
haveLoc = true;
}
}
}
/**
* 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.
*/
protected void getData() {
// Get the status of all of the joysticks
for (byte stick = 0; stick < kJoystickPorts; stick++) {
m_joystickAxesCache[stick].m_count =
HAL.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes);
m_joystickPOVsCache[stick].m_count =
HAL.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs);
m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer);
m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0);
}
// lock joystick mutex to swap cache data
synchronized (m_joystickMutex) {
// move cache to actual data
HALJoystickAxes[] currentAxes = m_joystickAxes;
m_joystickAxes = m_joystickAxesCache;
m_joystickAxesCache = currentAxes;
HALJoystickButtons[] currentButtons = m_joystickButtons;
m_joystickButtons = m_joystickButtonsCache;
m_joystickButtonsCache = currentButtons;
HALJoystickPOVs[] currentPOVs = m_joystickPOVs;
m_joystickPOVs = m_joystickPOVsCache;
m_joystickPOVsCache = currentPOVs;
}
//Lock new control data mutex and set new control data.
synchronized (m_newControlDataMutex) {
m_newControlData = true;
}
}
/**
* Read the battery voltage.
*
* @return The battery voltage in Volts.
*/
public double getBatteryVoltage() {
return PowerJNI.getVinVoltage();
}
/**
* Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
* the DS.
*/
private void reportJoystickUnpluggedError(String message) {
double currentTime = Timer.getFPGATimestamp();
if (currentTime > m_nextMessageTime) {
reportError(message, false);
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
}
}
/**
* Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
* the DS.
*/
private void reportJoystickUnpluggedWarning(String message) {
double currentTime = Timer.getFPGATimestamp();
if (currentTime > m_nextMessageTime) {
reportWarning(message, false);
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
}
HAL.sendError(isError, code, false, error, locString, printTrace ? traceString : "", true);
}
/**
@@ -302,21 +220,6 @@ public class DriverStation implements RobotState.Interface {
return retVal;
}
/**
* 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
*/
public int getStickAxisCount(int stick) {
if (stick < 0 || stick >= kJoystickPorts) {
throw new RuntimeException("Joystick index is out of range, should be 0-5");
}
synchronized (m_joystickMutex) {
return m_joystickAxes[stick].m_count;
}
}
/**
* Get the state of a POV on the joystick.
*
@@ -346,21 +249,6 @@ public class DriverStation implements RobotState.Interface {
return retVal;
}
/**
* 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
*/
public int getStickPOVCount(int stick) {
if (stick < 0 || stick >= kJoystickPorts) {
throw new RuntimeException("Joystick index is out of range, should be 0-5");
}
synchronized (m_joystickMutex) {
return m_joystickPOVs[stick].m_count;
}
}
/**
* The state of the buttons on the joystick.
*
@@ -408,6 +296,36 @@ public class DriverStation implements RobotState.Interface {
return retVal;
}
/**
* 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
*/
public int getStickAxisCount(int stick) {
if (stick < 0 || stick >= kJoystickPorts) {
throw new RuntimeException("Joystick index is out of range, should be 0-5");
}
synchronized (m_joystickMutex) {
return m_joystickAxes[stick].m_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
*/
public int getStickPOVCount(int stick) {
if (stick < 0 || stick >= kJoystickPorts) {
throw new RuntimeException("Joystick index is out of range, should be 0-5");
}
synchronized (m_joystickMutex) {
return m_joystickPOVs[stick].m_count;
}
}
/**
* Gets the number of buttons on a joystick.
*
@@ -540,6 +458,16 @@ public class DriverStation implements RobotState.Interface {
return controlWord.getAutonomous();
}
/**
* Gets a value indicating whether the Driver Station requires the robot to be running in
* operator-controlled mode.
*
* @return True if operator-controlled mode should be enabled, false otherwise.
*/
public boolean isOperatorControl() {
return !(isAutonomous() || isTest());
}
/**
* Gets a value indicating whether the Driver Station requires the robot to be running in test
* mode.
@@ -552,13 +480,38 @@ public class DriverStation implements RobotState.Interface {
}
/**
* Gets a value indicating whether the Driver Station requires the robot to be running in
* operator-controlled mode.
* Gets a value indicating whether the Driver Station is attached.
*
* @return True if operator-controlled mode should be enabled, false otherwise.
* @return True if Driver Station is attached, false otherwise.
*/
public boolean isOperatorControl() {
return !(isAutonomous() || isTest());
public boolean isDSAttached() {
ControlWord controlWord = HAL.getControlWord();
return controlWord.getDSAttached();
}
/**
* Has a new control packet from the driver station arrived since the last time this function was
* called?
*
* @return True if the control data has been updated since the last call.
*/
public boolean isNewControlData() {
synchronized (m_newControlDataMutex) {
boolean result = m_newControlData;
m_newControlData = false;
return result;
}
}
/**
* Is the driver station attached to a Field Management System? Note: This does not work with the
* Blue DS.
*
* @return True if the robot is competing on a field being controlled by a Field Management System
*/
public boolean isFMSAttached() {
ControlWord controlWord = HAL.getControlWord();
return controlWord.getFMSAttached();
}
/**
@@ -580,20 +533,6 @@ public class DriverStation implements RobotState.Interface {
return HAL.getBrownedOut();
}
/**
* Has a new control packet from the driver station arrived since the last time this function was
* called?
*
* @return True if the control data has been updated since the last call.
*/
public boolean isNewControlData() {
synchronized (m_newControlDataMutex) {
boolean result = m_newControlData;
m_newControlData = false;
return result;
}
}
/**
* Get the current alliance from the FMS.
*
@@ -650,19 +589,29 @@ public class DriverStation implements RobotState.Interface {
}
/**
* Is the driver station attached to a Field Management System? Note: This does not work with the
* Blue DS.
*
* @return True if the robot is competing on a field being controlled by a Field Management System
* Wait for new data from the driver station.
*/
public boolean isFMSAttached() {
ControlWord controlWord = HAL.getControlWord();
return controlWord.getFMSAttached();
public void waitForData() {
waitForData(0);
}
public boolean isDSAttached() {
ControlWord controlWord = HAL.getControlWord();
return controlWord.getDSAttached();
/**
* Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data
* only.
*
* @param timeout The maximum time in milliseconds to wait.
*/
public void waitForData(long timeout) {
synchronized (m_dataSem) {
try {
while (!m_updatedControlLoopData) {
m_dataSem.wait(timeout);
}
m_updatedControlLoopData = false;
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
}
}
}
/**
@@ -679,46 +628,12 @@ public class DriverStation implements RobotState.Interface {
}
/**
* Report error to Driver Station. Also prints error to System.err Optionally appends Stack trace
* to error message.
* Read the battery voltage.
*
* @param printTrace If true, append stack trace to error string
* @return The battery voltage in Volts.
*/
public static void reportError(String error, boolean printTrace) {
reportErrorImpl(true, 1, error, printTrace);
}
/**
* Report warning to Driver Station. Also prints error to System.err Optionally appends Stack
* trace to warning message.
*
* @param printTrace If true, append stack trace to warning string
*/
public static void reportWarning(String error, boolean printTrace) {
reportErrorImpl(false, 1, error, printTrace);
}
private static void reportErrorImpl(boolean isError, int code, String error, boolean
printTrace) {
StackTraceElement[] traces = Thread.currentThread().getStackTrace();
String locString;
if (traces.length > 3) {
locString = traces[3].toString();
} else {
locString = new String();
}
boolean haveLoc = false;
String traceString = " at ";
for (int i = 3; i < traces.length; i++) {
String loc = traces[i].toString();
traceString += loc + "\n";
// get first user function
if (!haveLoc && !loc.startsWith("edu.wpi.first.wpilibj")) {
locString = loc;
haveLoc = true;
}
}
HAL.sendError(isError, code, false, error, locString, printTrace ? traceString : "", true);
public double getBatteryVoltage() {
return PowerJNI.getVinVoltage();
}
/**
@@ -764,4 +679,94 @@ public class DriverStation implements RobotState.Interface {
public void InTest(boolean entering) {
m_userInTest = entering;
}
/**
* 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.
*/
protected void getData() {
// Get the status of all of the joysticks
for (byte stick = 0; stick < kJoystickPorts; stick++) {
m_joystickAxesCache[stick].m_count =
HAL.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes);
m_joystickPOVsCache[stick].m_count =
HAL.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs);
m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer);
m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0);
}
// lock joystick mutex to swap cache data
synchronized (m_joystickMutex) {
// move cache to actual data
HALJoystickAxes[] currentAxes = m_joystickAxes;
m_joystickAxes = m_joystickAxesCache;
m_joystickAxesCache = currentAxes;
HALJoystickButtons[] currentButtons = m_joystickButtons;
m_joystickButtons = m_joystickButtonsCache;
m_joystickButtonsCache = currentButtons;
HALJoystickPOVs[] currentPOVs = m_joystickPOVs;
m_joystickPOVs = m_joystickPOVsCache;
m_joystickPOVsCache = currentPOVs;
}
//Lock new control data mutex and set new control data.
synchronized (m_newControlDataMutex) {
m_newControlData = true;
}
}
/**
* Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
* the DS.
*/
private void reportJoystickUnpluggedError(String message) {
double currentTime = Timer.getFPGATimestamp();
if (currentTime > m_nextMessageTime) {
reportError(message, false);
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
}
}
/**
* Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
* the DS.
*/
private void reportJoystickUnpluggedWarning(String message) {
double currentTime = Timer.getFPGATimestamp();
if (currentTime > m_nextMessageTime) {
reportWarning(message, false);
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
}
}
/**
* Provides the service routine for the DS polling m_thread.
*/
private void run() {
int safetyCounter = 0;
while (m_threadKeepAlive) {
HAL.waitForDSData();
getData();
synchronized (m_dataSem) {
m_updatedControlLoopData = true;
m_dataSem.notifyAll();
}
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();
}
}
}
}