diff --git a/wpilibc/athena/include/DriverStation.h b/wpilibc/athena/include/DriverStation.h index 0ed6487759..00d87068a9 100644 --- a/wpilibc/athena/include/DriverStation.h +++ b/wpilibc/athena/include/DriverStation.h @@ -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(); diff --git a/wpilibc/athena/src/DriverStation.cpp b/wpilibc/athena/src/DriverStation.cpp index 05db0dccae..73ec86b778 100644 --- a/wpilibc/athena/src/DriverStation.cpp +++ b/wpilibc/athena/src/DriverStation.cpp @@ -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(kJoystickPorts); - m_joystickPOVs = std::make_unique(kJoystickPorts); - m_joystickButtons = std::make_unique(kJoystickPorts); - m_joystickDescriptor = - std::make_unique(kJoystickPorts); - m_joystickAxesCache = std::make_unique(kJoystickPorts); - m_joystickPOVsCache = std::make_unique(kJoystickPorts); - m_joystickButtonsCache = - std::make_unique(kJoystickPorts); - m_joystickDescriptorCache = - std::make_unique(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 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 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 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 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 lock(m_joystickDataMutex); - return static_cast(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 lock(m_joystickDataMutex); - return static_cast(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 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 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 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 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 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 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 lock(m_joystickDataMutex); + return static_cast(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 lock(m_joystickDataMutex); + return static_cast(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 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 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 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(kJoystickPorts); + m_joystickPOVs = std::make_unique(kJoystickPorts); + m_joystickButtons = std::make_unique(kJoystickPorts); + m_joystickDescriptor = + std::make_unique(kJoystickPorts); + m_joystickAxesCache = std::make_unique(kJoystickPorts); + m_joystickPOVsCache = std::make_unique(kJoystickPorts); + m_joystickButtonsCache = + std::make_unique(kJoystickPorts); + m_joystickDescriptorCache = + std::make_unique(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 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(); + } } diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java index 4118a72537..dabf577d52 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java @@ -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 { *

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(); + } + } + } }