From 827341caa251adbb571de530e09e31cda219e47f Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Thu, 11 Dec 2014 16:44:55 -0500 Subject: [PATCH] Squashed commit of the following: commit c8543f97f77a0fef282b6598ae094ac75ea1dd22 Author: Kevin O'Connor Date: Thu Dec 11 16:41:08 2014 -0500 Go back to buffering Joystick data. Change-Id: I0b4204bfc6e81f50dc4a01c58cfbe14a771e902f commit a8ddee2a923749903aafe2a8121171b1d70750e7 Author: Kevin O'Connor Date: Fri Dec 5 17:38:58 2014 -0500 Add error for using non-existent button 0. Fixes artf3870. Change-Id: I5b83cc7e3f0e4ab957279a877c76eeab6cb4b77b Change-Id: Iae36482fd82176a9e09da1cfdfb69591411b2be2 --- .../wpilibC++Devices/include/DriverStation.h | 4 + .../wpilibC++Devices/src/DriverStation.cpp | 59 ++++++++++----- .../edu/wpi/first/wpilibj/DriverStation.java | 75 ++++++++++++------- 3 files changed, 94 insertions(+), 44 deletions(-) diff --git a/wpilibc/wpilibC++Devices/include/DriverStation.h b/wpilibc/wpilibC++Devices/include/DriverStation.h index 2b1b8075ec..827a29f0fb 100644 --- a/wpilibc/wpilibC++Devices/include/DriverStation.h +++ b/wpilibc/wpilibC++Devices/include/DriverStation.h @@ -89,12 +89,16 @@ public: protected: DriverStation(); + void GetData(); private: static void InitTask(DriverStation *ds); static DriverStation *m_instance; void ReportJoystickUnpluggedError(std::string message); void Run(); + HALJoystickAxes m_joystickAxes[kJoystickPorts]; + HALJoystickPOVs m_joystickPOVs[kJoystickPorts]; + HALJoystickButtons m_joystickButtons[kJoystickPorts]; Task m_task; SEMAPHORE_ID m_newControlData; MULTIWAIT_ID m_packetDataAvailableMultiWait; diff --git a/wpilibc/wpilibC++Devices/src/DriverStation.cpp b/wpilibc/wpilibC++Devices/src/DriverStation.cpp index b3a62f29b4..d39728f7a9 100644 --- a/wpilibc/wpilibC++Devices/src/DriverStation.cpp +++ b/wpilibc/wpilibC++Devices/src/DriverStation.cpp @@ -28,7 +28,7 @@ const uint32_t DriverStation::kJoystickPorts; DriverStation* DriverStation::m_instance = NULL; /** - * DriverStation contructor. + * DriverStation constructor. * * This is only called once the first time GetInstance() is called */ @@ -43,6 +43,13 @@ DriverStation::DriverStation() , m_userInTest(false) , m_nextMessageTime(0) { + // 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; + } // Create a new semaphore m_packetDataAvailableMultiWait = initializeMultiWait(); m_newControlData = initializeSemaphore(SEMAPHORE_EMPTY); @@ -84,15 +91,12 @@ void DriverStation::InitTask(DriverStation *ds) void DriverStation::Run() { - HALControlWord controlWord; int period = 0; while (true) { - // need to get the controlWord to keep the motors enabled - HALGetControlWord(&controlWord); takeMultiWait(m_packetDataAvailableMultiWait, m_packetDataAvailableMutex, 0); + GetData(); giveMultiWait(m_waitForDataSem); - giveSemaphore(m_newControlData); if (++period >= 4) { @@ -122,6 +126,23 @@ DriverStation* DriverStation::GetInstance() return m_instance; } +/** + * 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 + for(uint8_t stick = 0; stick < kJoystickPorts; stick++) { + HALGetJoystickAxes(stick, &m_joystickAxes[stick]); + HALGetJoystickPOVs(stick, &m_joystickPOVs[stick]); + HALGetJoystickButtons(stick, &m_joystickButtons[stick]); + } + giveSemaphore(m_newControlData); +} + /** * Read the battery voltage. * @@ -211,9 +232,7 @@ float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) return 0; } - HALJoystickAxes joystickAxes; - HALGetJoystickAxes(stick, &joystickAxes); - if (axis >= joystickAxes.count) + if (axis >= m_joystickAxes[stick].count) { if (axis >= kMaxJoystickAxes) wpi_setWPIError(BadJoystickAxis); @@ -222,7 +241,7 @@ float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) return 0.0f; } - int8_t value = joystickAxes.axes[axis]; + int8_t value = m_joystickAxes[stick].axes[axis]; if(value < 0) { @@ -245,10 +264,8 @@ int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) { wpi_setWPIError(BadJoystickIndex); return 0; } - - HALJoystickPOVs joystickPOVs; - HALGetJoystickPOVs(stick, &joystickPOVs); - if (pov >= joystickPOVs.count) + + if (pov >= m_joystickPOVs[stick].count) { if (pov >= kMaxJoystickPOVs) wpi_setWPIError(BadJoystickAxis); @@ -257,7 +274,7 @@ int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) { return 0; } - return joystickPOVs.povs[pov]; + return m_joystickPOVs[stick].povs[pov]; } /** @@ -271,16 +288,20 @@ bool DriverStation::GetStickButton(uint32_t stick, uint8_t button) if (stick >= kJoystickPorts) { wpi_setWPIError(BadJoystickIndex); - return 0; + return false; } - HALJoystickButtons joystickButtons; - HALGetJoystickButtons(stick, &joystickButtons); - if(button > joystickButtons.count) + + if(button > m_joystickButtons[stick].count) { ReportJoystickUnpluggedError("WARNING: Joystick Button missing, check if all controllers are plugged in\n"); return false; } - return ((0x1 << (button-1)) & joystickButtons.buttons) !=0; + if(button == 0) + { + ReportJoystickUnpluggedError("ERROR: Button indexes begin at 1 in WPILib for C++ and Java"); + return false; + } + return ((0x1 << (button-1)) & m_joystickButtons[stick].buttons) !=0; } bool DriverStation::IsEnabled() diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index f4734d94ce..787066cfa9 100644 --- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -25,6 +25,11 @@ public class DriverStation implements RobotState.Interface { * Number of Joystick Ports */ public static final int kJoystickPorts = 6; + + private class HALJoystickButtons { + public int buttons; + public byte count; + } /** * The robot alliance that the robot is a part of @@ -49,6 +54,9 @@ public class DriverStation implements RobotState.Interface { private static DriverStation instance = new DriverStation(); + private short[][] m_joystickAxes = new short[kJoystickPorts][FRCNetworkCommunicationsLibrary.kMaxJoystickAxes]; + private short[][] m_joystickPOVs = new short[kJoystickPorts][FRCNetworkCommunicationsLibrary.kMaxJoystickPOVs]; + private HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[kJoystickPorts]; private Thread m_thread; private final Object m_dataSem; @@ -78,6 +86,10 @@ public class DriverStation implements RobotState.Interface { */ protected DriverStation() { m_dataSem = new Object(); + for(int i=0; i= 5) { + if (++safetyCounter >= 4) { MotorSafetyHelper.checkMotors(); safetyCounter = 0; } @@ -149,6 +161,24 @@ public class DriverStation implements RobotState.Interface { } } } + /** + * 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 synchronized void getData() { + + // Get the status of all of the joysticks + for(byte stick = 0; stick < kJoystickPorts; stick++) { + m_joystickAxes[stick] = FRCNetworkCommunicationsLibrary.HALGetJoystickAxes(stick); + m_joystickPOVs[stick] = FRCNetworkCommunicationsLibrary.HALGetJoystickPOVs(stick); + ByteBuffer countBuffer = ByteBuffer.allocateDirect(1); + m_joystickButtons[stick].buttons = FRCNetworkCommunicationsLibrary.HALGetJoystickButtons((byte)stick, countBuffer); + m_joystickButtons[stick].count = countBuffer.get(); + } + + m_newControlData = true; + } /** * Read the battery voltage. @@ -187,15 +217,13 @@ public class DriverStation implements RobotState.Interface { if (axis < 0 || axis >= FRCNetworkCommunicationsLibrary.kMaxJoystickAxes) { throw new RuntimeException("Joystick axis is out of range"); } - - short[] joystickAxes = FRCNetworkCommunicationsLibrary.HALGetJoystickAxes((byte)stick); - if (axis >= joystickAxes.length) { + if (axis >= m_joystickAxes[stick].length) { reportJoystickUnpluggedError("WARNING: Joystick axis " + axis + " on port " + stick + " not available, check if controller is plugged in\n"); return 0.0; } - byte value = (byte)joystickAxes[axis]; + byte value = (byte)m_joystickAxes[stick][axis]; if(value < 0) { return value / 128.0; @@ -215,7 +243,7 @@ public class DriverStation implements RobotState.Interface { throw new RuntimeException("Joystick index is out of range, should be 0-5"); } - return FRCNetworkCommunicationsLibrary.HALGetJoystickAxes((byte)stick).length; + return m_joystickAxes[stick].length; } /** @@ -231,15 +259,13 @@ public class DriverStation implements RobotState.Interface { if (pov < 0 || pov >= FRCNetworkCommunicationsLibrary.kMaxJoystickPOVs) { throw new RuntimeException("Joystick POV is out of range"); } - - short[] joystickPOVs = FRCNetworkCommunicationsLibrary.HALGetJoystickPOVs((byte)stick); - if (pov >= joystickPOVs.length) { + if (pov >= m_joystickPOVs[stick].length) { reportJoystickUnpluggedError("WARNING: Joystick POV " + pov + " on port " + stick + " not available, check if controller is plugged in\n"); return 0; } - return joystickPOVs[pov]; + return m_joystickPOVs[stick][pov]; } /** @@ -253,7 +279,7 @@ public class DriverStation implements RobotState.Interface { throw new RuntimeException("Joystick index is out of range, should be 0-5"); } - return FRCNetworkCommunicationsLibrary.HALGetJoystickPOVs((byte)stick).length; + return m_joystickPOVs[stick].length; } /** @@ -268,15 +294,17 @@ public class DriverStation implements RobotState.Interface { throw new RuntimeException("Joystick index is out of range, should be 0-3"); } - ByteBuffer countBuffer = ByteBuffer.allocateDirect(1); - int buttons = FRCNetworkCommunicationsLibrary.HALGetJoystickButtons((byte)stick, countBuffer); - byte count = 0; - count = countBuffer.get(); - if(button > count) { + + if(button > m_joystickButtons[stick].count) { reportJoystickUnpluggedError("WARNING: Joystick Button " + button + " on port " + stick + " not available, check if controller is plugged in\n"); return false; } - return ((0x1 << (button - 1)) & buttons) != 0; + if(button == 0) + { + reportJoystickUnpluggedError("ERROR: Button indexes begin at 1 in WPILib for C++ and Java"); + return false; + } + return ((0x1 << (button - 1)) & m_joystickButtons[stick].buttons) != 0; } /** @@ -290,11 +318,8 @@ public class DriverStation implements RobotState.Interface { throw new RuntimeException("Joystick index is out of range, should be 0-3"); } - ByteBuffer countBuffer = ByteBuffer.allocateDirect(1); - - int buttons = FRCNetworkCommunicationsLibrary.HALGetJoystickButtons((byte)stick, countBuffer); - byte count = countBuffer.get(); - return count; + + return m_joystickButtons[stick].count; } /**