Check if Joystick Button exists when requested and pass 0 and warn if it doesn't

Change-Id: I2194859ef8b263f1a20aba52ec154fb0a1fc8078

hal/include/HAL/HAL.hpp

@@ -178,7 +178,10 @@ struct HALJoystickPOVs {
 	int16_t povs[kMaxJoystickPOVs];
 };
 
-typedef uint32_t HALJoystickButtons;
+struct HALJoystickButtons {
+	uint32_t buttons;
+	uint8_t count;
+};
 
 inline float intToFloat(int value)
 {
@@ -213,7 +216,7 @@ extern "C"
 	int HALGetAllianceStation(enum HALAllianceStationID *allianceStation);
 	int HALGetJoystickAxes(uint8_t joystickNum, HALJoystickAxes *axes);
 	int HALGetJoystickPOVs(uint8_t joystickNum, HALJoystickPOVs *povs);
-	int HALGetJoystickButtons(uint8_t joystickNum, HALJoystickButtons *buttons, uint8_t *count);
+	int HALGetJoystickButtons(uint8_t joystickNum, HALJoystickButtons *buttons);
 	int HALGetMatchTime(float *matchTime);
 
 	void HALSetNewDataSem(pthread_cond_t *);

hal/lib/Athena/HAL.cpp

@@ -201,9 +201,9 @@ int HALGetJoystickPOVs(uint8_t joystickNum, HALJoystickPOVs *povs)
 	return FRC_NetworkCommunication_getJoystickPOVs(joystickNum, (JoystickPOV_t*) povs, kMaxJoystickPOVs);
 }
 
-int HALGetJoystickButtons(uint8_t joystickNum, HALJoystickButtons *buttons, uint8_t *count)
+int HALGetJoystickButtons(uint8_t joystickNum, HALJoystickButtons *buttons)
 {
-	return FRC_NetworkCommunication_getJoystickButtons(joystickNum, buttons, count);
+	return FRC_NetworkCommunication_getJoystickButtons(joystickNum, &buttons->buttons, &buttons->count);
 }
 
 int HALGetMatchTime(float *matchTime)

wpilibc/wpilibC++Devices/include/DriverStation.h

@@ -34,7 +34,7 @@ public:
 
 	float GetStickAxis(uint32_t stick, uint32_t axis);
 	int GetStickPOV(uint32_t stick, uint32_t pov);
-	short GetStickButtons(uint32_t stick);
+	bool GetStickButton(uint32_t stick, uint8_t button);
 
 	bool IsEnabled();
 	bool IsDisabled();

wpilibc/wpilibC++Devices/src/DriverStation.cpp

@@ -145,11 +145,8 @@ void DriverStation::GetData()
 
 	// Get the status of all of the joysticks
 	for(uint8_t stick = 0; stick < kJoystickPorts; stick++) {
-		uint8_t count;
-
 		HALGetJoystickAxes(stick, &m_joystickAxes[stick]);
 		HALGetJoystickPOVs(stick, &m_joystickPOVs[stick]);
-		HALGetJoystickButtons(stick, &m_joystickButtons[stick], &count);
 	}
 
 	if (!lastEnabled && IsEnabled())
@@ -245,20 +242,25 @@ int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) {
 
 /**
  * The state of the buttons on the joystick.
- * 12 buttons (4 msb are unused) from the joystick.
  *
  * @param stick The joystick to read.
  * @return The state of the buttons on the joystick.
  */
-short DriverStation::GetStickButtons(uint32_t stick)
+bool DriverStation::GetStickButton(uint32_t stick, uint8_t button)
 {
 	if (stick >= kJoystickPorts)
 	{
 		wpi_setWPIError(BadJoystickIndex);
 		return 0;
 	}
-
-	return m_joystickButtons[stick];
+	HALJoystickButtons joystickButtons;
+	HALGetJoystickButtons(stick, &joystickButtons);
+	if(button >= joystickButtons.count)
+	{
+		ReportError("WARNING: Joystick Button missing, check if all controllers are plugged in\n");
+		return false;
+	}
+	return ((0x1 << (button-1)) & joystickButtons.buttons) !=0;
 }
 
 bool DriverStation::IsEnabled()

wpilibc/wpilibC++Devices/src/Joystick.cpp

@@ -228,7 +228,7 @@ bool Joystick::GetBumper(JoystickHand hand)
  **/
 bool Joystick::GetRawButton(uint32_t button)
 {
-	return ((0x1 << (button-1)) & m_ds->GetStickButtons(m_port)) != 0;
+	return m_ds->GetStickButton(m_port, button);
 }
 
 /**

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -181,7 +181,6 @@ public class DriverStation implements RobotState.Interface {
 
         // Get the status of all of the joysticks
         for(byte stick = 0; stick < kJoystickPorts; stick++) {
-            m_joystickButtons[stick] = FRCNetworkCommunicationsLibrary.HALGetJoystickButtons(stick);
             m_joystickAxes[stick] = FRCNetworkCommunicationsLibrary.HALGetJoystickAxes(stick);
             m_joystickPOVs[stick] = FRCNetworkCommunicationsLibrary.HALGetJoystickPOVs(stick);
         }
@@ -274,12 +273,20 @@ public class DriverStation implements RobotState.Interface {
      * @param stick The joystick to read.
      * @return The state of the buttons on the joystick.
      */
-    public int getStickButtons(final int stick) {
+    public boolean getStickButton(final int stick, byte button) {
         if(stick < 0 || stick >= kJoystickPorts) {
             throw new RuntimeException("Joystick index is out of range, should be 0-3");
         }
-
-        return (int)m_joystickButtons[stick];
+		
+		ByteBuffer countBuffer = ByteBuffer.allocateDirect(1);
+		int buttons = FRCNetworkCommunicationsLibrary.HALGetJoystickButtons((byte)stick, countBuffer);
+		byte count = 0;
+		count = countBuffer.get();
+		if(button >= count) {
+			reportError("WARNING: Joystick Button " + button + " on port " + stick + " not available, check if controller is plugged in\n", false);
+            return false;
+		}
+		return ((0x1 << (button - 1)) & buttons) != 0;
     }
 
     /**

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Joystick.java

@@ -276,7 +276,7 @@ public class Joystick extends GenericHID {
      * @return The state of the button.
      */
     public boolean getRawButton(final int button) {
-        return ((0x1 << (button - 1)) & m_ds.getStickButtons(m_port)) != 0;
+        return m_ds.getStickButton(m_port, (byte)button);
     }
 
     /**

wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/communication/FRCNetworkCommunicationsLibrary.java

@@ -467,7 +467,7 @@ public class FRCNetworkCommunicationsLibrary extends JNIWrapper {
 		case 4:
 			return HALAllianceStationID.Blue2;
 		case 5:
-			return HALAllianceStationID.Blue3;
+			return HALAllianceStationID.Blue3;
 		default:
 			return null;
 		}

wpilibj/wpilibJavaJNI/lib/FRCNetworkCommunicationsLibrary.cpp

@@ -324,14 +324,17 @@ JNIEXPORT jshortArray JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetwor
  * Signature: (B)S
  */
 JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickButtons
-  (JNIEnv *, jclass, jbyte joystickNum)
+  (JNIEnv * env, jclass, jbyte joystickNum, jobject count)
 {
-    HALJoystickButtons buttons;
-    uint8_t count;
-
-    HALGetJoystickButtons(joystickNum, &buttons, &count);
-
-    return buttons;
+	NETCOMM_LOG(logDEBUG) << "Calling HALJoystickButtons";
+    HALJoystickButtons joystickButtons;
+    HALGetJoystickButtons(joystickNum, &joystickButtons);
+	jbyte *countPtr = (jbyte*)env->GetDirectBufferAddress(count);
+	NETCOMM_LOG(logDEBUG) << "Buttons = " << joystickButtons.buttons;
+	NETCOMM_LOG(logDEBUG) << "Count = " << (jint)joystickButtons.count;
+	*countPtr = joystickButtons.count;
+	NETCOMM_LOG(logDEBUG) << "CountBuffer = " << (jint)*countPtr;
+    return joystickButtons.buttons;
 }
 
 /*