Add new joystick features

Axis counts other than six and POVs are both present in C++ and Java now

Add dynamic joystick axis counts, up to 12

Change-Id: Ieade5e61a89df822df8702cb32326e4635558778

Add support for POVs in C++

Change-Id: I12dc0fcaca605a256ddcf990eebde45767229171

Make POVs work in Java

Change-Id: Ie2d98adb416c1930f058bdd21c3e7d26289df503

hal/include/HAL/HAL.hpp

@@ -161,9 +161,20 @@ enum HALAllianceStationID {
 	kHALAllianceStationID_blue3,
 };
 
+/* The maximum number of axes that will be stored in a single HALJoystickAxes
+	struct. This is used for allocating buffers, not bounds checking, since
+	there are usually less axes in practice. */
+static constexpr size_t kMaxJoystickAxes = 12;
+static constexpr size_t kMaxJoystickPOVs = 12;
+
 struct HALJoystickAxes {
 	uint16_t count;
-	int16_t axes[6];
+	int16_t axes[kMaxJoystickAxes];
+};
+
+struct HALJoystickPOVs {
+	uint16_t count;
+	int16_t povs[kMaxJoystickPOVs];
 };
 
 typedef uint32_t HALJoystickButtons;
@@ -199,7 +210,8 @@ extern "C"
 
 	int HALGetControlWord(HALControlWord *data);
 	int HALGetAllianceStation(enum HALAllianceStationID *allianceStation);
-	int HALGetJoystickAxes(uint8_t joystickNum, HALJoystickAxes *axes, uint8_t maxAxes);
+	int HALGetJoystickAxes(uint8_t joystickNum, HALJoystickAxes *axes);
+	int HALGetJoystickPOVs(uint8_t joystickNum, HALJoystickPOVs *povs);
 	int HALGetJoystickButtons(uint8_t joystickNum, HALJoystickButtons *buttons, uint8_t *count);
 
 	void HALSetNewDataSem(pthread_mutex_t *);

hal/lib/Athena/HAL.cpp

@@ -125,9 +125,14 @@ int HALGetAllianceStation(enum HALAllianceStationID *allianceStation)
 	return FRC_NetworkCommunication_getAllianceStation((AllianceStationID_t*) allianceStation);
 }
 
-int HALGetJoystickAxes(uint8_t joystickNum, HALJoystickAxes *axes, uint8_t maxAxes)
+int HALGetJoystickAxes(uint8_t joystickNum, HALJoystickAxes *axes)
 {
-	return FRC_NetworkCommunication_getJoystickAxes(joystickNum, (JoystickAxes_t*) axes, maxAxes);
+	return FRC_NetworkCommunication_getJoystickAxes(joystickNum, (JoystickAxes_t*) axes, kMaxJoystickAxes);
+}
+
+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)

wpilibc/wpilibC++/include/GenericHID.h

@@ -33,4 +33,6 @@ public:
 	virtual bool GetTop(JoystickHand hand = kRightHand) = 0;
 	virtual bool GetBumper(JoystickHand hand = kRightHand) = 0;
 	virtual bool GetRawButton(uint32_t button) = 0;
+
+	virtual int GetPOV(uint32_t pov = 1) = 0;
 };

wpilibc/wpilibC++/include/WPIErrors.h

@@ -65,8 +65,8 @@ S(CompressorTaskError, 3, "Compressor task won't start");
 S(LoopTimingError, 4, "Digital module loop timing is not the expected value");
 S(NonBinaryDigitalValue, 5, "Digital output value is not 0 or 1");
 S(IncorrectBatteryChannel, 6, "Battery measurement channel is not correct value");
-S(BadJoystickIndex, 7, "Joystick index is out of range, should be 1-4");
+S(BadJoystickIndex, 7, "Joystick index is out of range, should be 0-3");
-S(BadJoystickAxis, 8, "Joystick axis is out of range");
+S(BadJoystickAxis, 8, "Joystick axis or POV is out of range");
 S(InvalidMotorIndex, 9, "Motor index is out of range, should be 0-3");
 S(DriverStationTaskError, 10, "Driver Station task won't start");
 S(EnhancedIOPWMPeriodOutOfRange, 11, "Driver Station Enhanced IO PWM Output period out of range.");

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

@@ -30,9 +30,9 @@ public:
 	static DriverStation *GetInstance();
 
 	static const uint32_t kJoystickPorts = 4;
-	static const uint32_t kJoystickAxes = 6;
 
 	float GetStickAxis(uint32_t stick, uint32_t axis);
+	int GetStickPOV(uint32_t stick, uint32_t pov);
 	short GetStickButtons(uint32_t stick);
 
 	bool IsEnabled();
@@ -98,6 +98,7 @@ private:
 	HALControlWord m_controlWord;
 	HALAllianceStationID m_allianceStationID;
 	HALJoystickAxes m_joystickAxes[kJoystickPorts];
+	HALJoystickPOVs m_joystickPOVs[kJoystickPorts];
 	HALJoystickButtons m_joystickButtons[kJoystickPorts];
 
 	MUTEX_ID m_statusDataSemaphore;

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

@@ -55,8 +55,9 @@ public:
 	virtual bool GetTrigger(JoystickHand hand = kRightHand);
 	virtual bool GetTop(JoystickHand hand = kRightHand);
 	virtual bool GetBumper(JoystickHand hand = kRightHand);
-	virtual bool GetButton(ButtonType button);
+	virtual bool GetRawButton(uint32_t button);
-	bool GetRawButton(uint32_t button);
+	virtual int GetPOV(uint32_t pov = 1);
+	bool GetButton(ButtonType button);
 	static Joystick* GetStickForPort(uint32_t port);
 
 	virtual float GetMagnitude();
@@ -74,4 +75,3 @@ private:
 };
 
 #endif
- 

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

@@ -24,7 +24,6 @@ TLogLevel dsLogLevel = logDEBUG;
 	else Log().Get(level)
 
 const uint32_t DriverStation::kJoystickPorts;
-const uint32_t DriverStation::kJoystickAxes;
 DriverStation* DriverStation::m_instance = NULL;
 
 /**
@@ -46,6 +45,13 @@ DriverStation::DriverStation()
 {
 	memset(&m_controlWord, 0, sizeof(m_controlWord));
 
+	// All joysticks should default to having zero axes and povs, 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;
+	}
+
 	// Create a new semaphore
 	m_packetDataAvailableSem = initializeMutexNormal();
 	m_newControlData = initializeSemaphore(SEMAPHORE_EMPTY);
@@ -135,7 +141,8 @@ void DriverStation::GetData()
 	for(uint8_t stick = 0; stick < kJoystickPorts; stick++) {
 		uint8_t count;
 
-		HALGetJoystickAxes(stick, &m_joystickAxes[stick], kJoystickAxes);
+		HALGetJoystickAxes(stick, &m_joystickAxes[stick]);
+		HALGetJoystickPOVs(stick, &m_joystickPOVs[stick]);
 		HALGetJoystickButtons(stick, &m_joystickButtons[stick], &count);
 	}
 
@@ -185,7 +192,7 @@ float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis)
 		return 0;
 	}
 
-	if (axis < 1 || axis > kJoystickAxes)
+	if (axis < 1 || axis > m_joystickAxes[stick].count)
 	{
 		wpi_setWPIError(BadJoystickAxis);
 		return 0.0f;
@@ -203,6 +210,27 @@ float DriverStation::GetStickAxis(uint32_t stick, uint32_t 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(uint32_t stick, uint32_t pov) {
+	if (stick >= kJoystickPorts)
+	{
+		wpi_setWPIError(BadJoystickIndex);
+		return 0;
+	}
+
+	if (pov < 1 || pov > m_joystickPOVs[stick].count)
+	{
+		wpi_setWPIError(BadJoystickAxis);
+		return 0;
+	}
+
+	return m_joystickPOVs[stick].povs[pov - 1];
+}
+
 /**
  * The state of the buttons on the joystick.
  * 12 buttons (4 msb are unused) from the joystick.

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

@@ -231,6 +231,15 @@ bool Joystick::GetRawButton(uint32_t button)
 	return ((0x1 << (button-1)) & m_ds->GetStickButtons(m_port)) != 0;
 }
 
+/**
+ * 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 Joystick::GetPOV(uint32_t pov) {
+	return m_ds->GetStickPOV(m_port, pov);
+}
+
 /**
  * Get buttons based on an enumerated type.
  *

wpilibc/wpilibC++Sim/include/Joystick.h

@@ -55,8 +55,9 @@ public:
 	virtual bool GetTrigger(JoystickHand hand = kRightHand);
 	virtual bool GetTop(JoystickHand hand = kRightHand);
 	virtual bool GetBumper(JoystickHand hand = kRightHand);
-	virtual bool GetButton(ButtonType button);
+	virtual bool GetRawButton(uint32_t button);
-	bool GetRawButton(uint32_t button);
+	virtual int GetPOV(uint32_t pov = 1);
+	bool GetButton(ButtonType button);
 	static Joystick* GetStickForPort(uint32_t port);
 
 	virtual float GetMagnitude();
@@ -74,4 +75,3 @@ private:
 };
 
 #endif
- 

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

@@ -224,6 +224,15 @@ bool Joystick::GetRawButton(uint32_t button)
     return m_ds->GetStickButton(m_port, button);
 }
 
+/**
+* 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 Joystick::GetPOV(uint32_t pov) {
+	return 0; // TODO
+}
+
 /**
  * Get buttons based on an enumerated type.
  *

wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/GenericHID.java

@@ -152,4 +152,10 @@ public abstract class GenericHID {
      * @return true if the button is pressed
      */
     public abstract boolean getRawButton(int button);
+
+    public abstract int getPOV(int pov);
+
+    public int getPOV() {
+        return getPOV(1);
+    }
 }

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

@@ -30,10 +30,6 @@ public class DriverStation implements RobotState.Interface {
      * Number of Joystick Ports
      */
     public static final int kJoystickPorts = 4;
-    /**
-     * Number of Joystick Axes
-     */
-    public static final int kJoystickAxes = 6;
     /**
      * Convert from raw values to volts
      */
@@ -61,7 +57,8 @@ public class DriverStation implements RobotState.Interface {
 
     private HALControlWord m_controlWord;
     private HALAllianceStationID m_allianceStationID;
-    private short[][] m_joystickAxes = new short[kJoystickAxes][kJoystickPorts];
+    private short[][] m_joystickAxes = new short[FRCNetworkCommunicationsLibrary.kMaxJoystickAxes][kJoystickPorts];
+    private short[][] m_joystickPOVs = new short[FRCNetworkCommunicationsLibrary.kMaxJoystickPOVs][kJoystickPorts];
     private int[] m_joystickButtons = new int[kJoystickPorts];
 
     private Thread m_thread;
@@ -185,6 +182,7 @@ public class DriverStation implements RobotState.Interface {
         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);
         }
 
         if (!lastEnabled && isEnabled()) {
@@ -228,7 +226,7 @@ public class DriverStation implements RobotState.Interface {
             throw new RuntimeException("Joystick index is out of range, should be 0-3");
         }
 
-        if (axis < 1 || axis > kJoystickAxes) {
+        if (axis < 1 || axis > m_joystickAxes[stick].length) {
             throw new RuntimeException("Joystick axis is out of range");
         }
 
@@ -241,6 +239,23 @@ public class DriverStation implements RobotState.Interface {
         }
     }
 
+    /**
+     * 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.
+     */
+    public int getStickPOV(int stick, int pov) {
+        if(stick < 0 || stick >= kJoystickPorts) {
+            throw new RuntimeException("Joystick index is out of range, should be 0-3");
+        }
+
+        if (pov < 1 || pov > m_joystickPOVs[stick].length) {
+            throw new RuntimeException("Joystick POV is out of range");
+        }
+
+        return m_joystickPOVs[stick][pov - 1];
+    }
+
     /**
      * The state of the buttons on the joystick.
      * 12 buttons (4 msb are unused) from the joystick.

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

@@ -279,6 +279,15 @@ public class Joystick extends GenericHID {
         return ((0x1 << (button - 1)) & m_ds.getStickButtons(m_port)) != 0;
     }
 
+    /**
+     * 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.
+     */
+    public int getPOV(int pov) {
+        return m_ds.getStickPOV(m_port, pov - 1);
+    }
+
     /**
      * Get buttons based on an enumerated type.
      *

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

@@ -463,7 +463,10 @@ public class FRCNetworkCommunicationsLibrary extends JNIWrapper {
 		case 2:
 			return HALAllianceStationID.Red3;
 		case 3:
+			return HALAllianceStationID.Blue1;
+		case 4:
 			return HALAllianceStationID.Blue2;
+		case 5:
 			return HALAllianceStationID.Blue3;
 		default:
 			return null;

wpilibj/wpilibJavaJNI/lib/FRCNetworkCommunicationsLibrary.cpp

@@ -293,7 +293,7 @@ JNIEXPORT jshortArray JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetwor
   (JNIEnv * env, jclass, jbyte joystickNum)
 {
     HALJoystickAxes axes;
-    HALGetJoystickAxes(joystickNum, &axes, 6);
+    HALGetJoystickAxes(joystickNum, &axes);
 
     jshortArray axesArray = env->NewShortArray(axes.count);
     env->SetShortArrayRegion(axesArray, 0, axes.count, axes.axes);
@@ -301,6 +301,23 @@ JNIEXPORT jshortArray JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetwor
     return axesArray;
 }
 
+/*
+ * Class:     edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary
+ * Method:    HALGetJoystickPOVs
+ * Signature: (B)[S
+ */
+JNIEXPORT jshortArray JNICALL Java_edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary_HALGetJoystickPOVs
+  (JNIEnv * env, jclass, jbyte joystickNum)
+{
+    HALJoystickPOVs povs;
+    HALGetJoystickPOVs(joystickNum, &povs);
+
+    jshortArray povsArray = env->NewShortArray(povs.count);
+    env->SetShortArrayRegion(povsArray, 0, povs.count, povs.povs);
+
+    return povsArray;
+}
+
 /*
  * Class:     edu_wpi_first_wpilibj_communication_FRCNetworkCommunicationsLibrary
  * Method:    HALGetJoystickButtons

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

@@ -276,6 +276,15 @@ public class Joystick extends GenericHID {
         return m_ds.getStickButton(m_port, button);
     }
 
+    /**
+     * 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.
+     */
+    public int getPOV(int pov) {
+        return 0; // TODO
+    }
+
     /**
      * Get buttons based on an enumerated type.
      *