Merge pull request #8 from mjs513/WIP2-Bluetooth

Update  to the Joystick examples

examples/Joystick/Joystick/Joystick.ino

+// Simple test of USB Host Joystick
+//
+// This example is in the public domain
+
+#include "USBHost_t36.h"
+
+USBHost myusb;
+USBHub hub1(myusb);
+USBHIDParser hid1(myusb);
+
+#define COUNT_JOYSTICKS 4
+JoystickController joysticks[COUNT_JOYSTICKS](myusb);
+int user_axis[64];
+uint32_t buttons_prev = 0;
+
+USBDriver *drivers[] = {&hub1, &joysticks[0], &joysticks[1], &joysticks[2], &joysticks[3], &hid1};
+#define CNT_DEVICES (sizeof(drivers)/sizeof(drivers[0]))
+const char * driver_names[CNT_DEVICES] = {"Hub1", "joystick[0D]", "joystick[1D]", "joystick[2D]", "joystick[3D]",  "HID1"};
+bool driver_active[CNT_DEVICES] = {false, false, false, false};
+
+// Lets also look at HID Input devices
+USBHIDInput *hiddrivers[] = {&joysticks[0], &joysticks[1], &joysticks[2], &joysticks[3]};
+#define CNT_HIDDEVICES (sizeof(hiddrivers)/sizeof(hiddrivers[0]))
+const char * hid_driver_names[CNT_DEVICES] = {"joystick[0H]", "joystick[1H]", "joystick[2H]", "joystick[3H]"};
+bool hid_driver_active[CNT_DEVICES] = {false};
+bool show_changed_only = false;
+
+uint8_t joystick_left_trigger_value[COUNT_JOYSTICKS] = {0};
+uint8_t joystick_right_trigger_value[COUNT_JOYSTICKS] = {0};
+uint64_t joystick_full_notify_mask = (uint64_t) - 1;
+
+int psAxis[64];
+
+
+//=============================================================================
+// Setup
+//=============================================================================
+void setup()
+{
+  Serial1.begin(2000000);
+  while (!Serial) ; // wait for Arduino Serial Monitor
+  Serial.println("\n\nUSB Host Joystick Testing");
+  myusb.begin();
+}
+
+
+//=============================================================================
+// loop
+//=============================================================================
+void loop()
+{
+  myusb.Task();
+  PrintDeviceListChanges();
+
+  if (Serial.available()) {
+    int ch = Serial.read(); // get the first char.
+    while (Serial.read() != -1) ;
+    if ((ch == 'b') || (ch == 'B')) {
+      Serial.println("Only notify on Basic Axis changes");
+      for (int joystick_index = 0; joystick_index < COUNT_JOYSTICKS; joystick_index++)
+        joysticks[joystick_index].axisChangeNotifyMask(0x3ff);
+    } else if ((ch == 'f') || (ch == 'F')) {
+      Serial.println("Only notify on Full Axis changes");
+      for (int joystick_index = 0; joystick_index < COUNT_JOYSTICKS; joystick_index++)
+        joysticks[joystick_index].axisChangeNotifyMask(joystick_full_notify_mask);
+
+    } else {
+      if (show_changed_only) {
+        show_changed_only = false;
+        Serial.println("\n*** Show All fields mode ***");
+      } else {
+        show_changed_only = true;
+        Serial.println("\n*** Show only changed fields mode ***");
+      }
+    }
+  }
+
+  for (int joystick_index = 0; joystick_index < COUNT_JOYSTICKS; joystick_index++) {
+    if (joysticks[joystick_index].available()) {
+      uint64_t axis_mask = joysticks[joystick_index].axisMask();
+      uint64_t axis_changed_mask = joysticks[joystick_index].axisChangedMask();
+      uint32_t buttons = joysticks[joystick_index].getButtons();
+      Serial.printf("Joystick(%d): buttons = %x", joystick_index, buttons);
+      //Serial.printf(" AMasks: %x %x:%x", axis_mask, (uint32_t)(user_axis_mask >> 32), (uint32_t)(user_axis_mask & 0xffffffff));
+      //Serial.printf(" M: %lx %lx", axis_mask, joysticks[joystick_index].axisChangedMask());
+      if (show_changed_only) {
+        for (uint8_t i = 0; axis_changed_mask != 0; i++, axis_changed_mask >>= 1) {
+          if (axis_changed_mask & 1) {
+            Serial.printf(" %d:%d", i, joysticks[joystick_index].getAxis(i));
+          }
+        }
+
+      } else {
+        for (uint8_t i = 0; axis_mask != 0; i++, axis_mask >>= 1) {
+          if (axis_mask & 1) {
+            Serial.printf(" %d:%d", i, joysticks[joystick_index].getAxis(i));
+          }
+        }
+      }
+      uint8_t ltv;
+      uint8_t rtv;
+
+      for (uint8_t i = 0; i<64; i++) {
+          psAxis[i] = joysticks[joystick_index].getAxis(i);
+      }
+      
+      switch (joysticks[joystick_index].joystickType()) {
+        default:
+          break;
+        case JoystickController::PS4:
+          printAngles();
+          ltv = joysticks[joystick_index].getAxis(3);
+          rtv = joysticks[joystick_index].getAxis(4);
+          if ((ltv != joystick_left_trigger_value[joystick_index]) || (rtv != joystick_right_trigger_value[joystick_index])) {
+            joystick_left_trigger_value[joystick_index] = ltv;
+            joystick_right_trigger_value[joystick_index] = rtv;
+            joysticks[joystick_index].setRumble(ltv, rtv);
+          }
+          break;
+
+        case JoystickController::PS3:
+          ltv = joysticks[joystick_index].getAxis(18);
+          rtv = joysticks[joystick_index].getAxis(19);
+          if ((ltv != joystick_left_trigger_value[joystick_index]) || (rtv != joystick_right_trigger_value[joystick_index])) {
+            joystick_left_trigger_value[joystick_index] = ltv;
+            joystick_right_trigger_value[joystick_index] = rtv;
+            joysticks[joystick_index].setRumble(ltv, rtv,50);
+          }
+          break;
+
+        case JoystickController::XBOXONE:
+        case JoystickController::XBOX360:
+          ltv = joysticks[joystick_index].getAxis(3);
+          rtv = joysticks[joystick_index].getAxis(4);
+          if ((ltv != joystick_left_trigger_value[joystick_index]) || (rtv != joystick_right_trigger_value[joystick_index])) {
+            joystick_left_trigger_value[joystick_index] = ltv;
+            joystick_right_trigger_value[joystick_index] = rtv;
+            joysticks[joystick_index].setRumble(ltv, rtv,50);
+            Serial.printf(" Set Rumble %d %d", ltv, rtv);
+          }
+          break;
+      }
+      if (buttons != buttons_prev) {
+        if (joysticks[joystick_index].joystickType() == JoystickController::PS3) {
+          //joysticks[joystick_index].setLEDs((buttons >> 12) & 0xf); //  try to get to TRI/CIR/X/SQuare
+          uint8_t leds = 0;
+          if (buttons & 0x8000) leds = 1;   //Srq
+          if (buttons & 0x2000) leds = 2;   //Cir
+          if (buttons & 0x1000) leds = 4;   //Tri
+          if (buttons & 0x4000) leds = 8;   //X  //Tri
+          joysticks[joystick_index].setLEDs(leds);
+        } else {
+          uint8_t lr = (buttons & 1) ? 0xff : 0;
+          uint8_t lg = (buttons & 2) ? 0xff : 0;
+          uint8_t lb = (buttons & 4) ? 0xff : 0;
+          joysticks[joystick_index].setLEDs(lr, lg, lb);
+        }
+          buttons_prev = buttons;
+      }
+
+      Serial.println();
+      joysticks[joystick_index].joystickDataClear();
+    }
+  }
+
+}
+
+//=============================================================================
+// Show when devices are added or removed
+//=============================================================================
+void PrintDeviceListChanges() {
+  for (uint8_t i = 0; i < CNT_DEVICES; i++) {
+    if (*drivers[i] != driver_active[i]) {
+      if (driver_active[i]) {
+        Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);
+        driver_active[i] = false;
+      } else {
+        Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
+        driver_active[i] = true;
+
+        const uint8_t *psz = drivers[i]->manufacturer();
+        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
+        psz = drivers[i]->product();
+        if (psz && *psz) Serial.printf("  product: %s\n", psz);
+        psz = drivers[i]->serialNumber();
+        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
+      }
+    }
+  }
+
+  for (uint8_t i = 0; i < CNT_HIDDEVICES; i++) {
+    if (*hiddrivers[i] != hid_driver_active[i]) {
+      if (hid_driver_active[i]) {
+        Serial.printf("*** HID Device %s - disconnected ***\n", hid_driver_names[i]);
+        hid_driver_active[i] = false;
+      } else {
+        Serial.printf("*** HID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
+        hid_driver_active[i] = true;
+
+        const uint8_t *psz = hiddrivers[i]->manufacturer();
+        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
+        psz = hiddrivers[i]->product();
+        if (psz && *psz) Serial.printf("  product: %s\n", psz);
+        psz = hiddrivers[i]->serialNumber();
+        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
+      }
+    }
+  }
+}

examples/Joystick/Joystick/ps4Helpers.ino

+float gx, gy, gz;
+float ax, ay, az;
+float pitch, roll;
+uint16_t xc, yc; 
+uint8_t isTouch;
+int16_t xc_old, yc_old;
+
+void printAngles(){
+  //test function calls
+  float gx, gy, gz;
+  getAccel(ax, ay, az);
+  Serial.printf("Accel-g's: %f, %f, %f\n", ax, ay, az);
+  getGyro(gx, gy, gz);
+  Serial.printf("Gyro-deg/sec: %f, %f, %f\n", gx, gy, gz);
+
+  getAngles(pitch, roll);
+  Serial.printf("Pitch/Roll: %f, %f\n", pitch, roll);
+
+  getCoords(xc, yc, isTouch);
+}
+
+void getCoords(uint16_t &xc, uint16_t &yc, uint8_t &isTouch){
+
+  //uint8_t finger = 0;  //only getting finger 1
+  uint8_t Id = 0;
+
+
+  // Trackpad touch 1: id, active, x, y
+  xc = ((psAxis[37] & 0x0f) << 8) | psAxis[36];
+  yc = psAxis[38] << 4 | ((psAxis[37] & 0xf0) >> 4),
+
+  isTouch = psAxis[35] >> 7;
+  if(xc != xc_old || yc != yc_old){
+    Serial.printf("Touch: %d, %d, %d, %d\n", psAxis[33], isTouch, xc, yc);
+    xc_old = xc;
+    yc_old = yc;
+  }
+}
+
+void getAccel( float &ax,  float &ay,  float &az){
+      int accelx = (int16_t)(psAxis[20]<<8) | psAxis[19];
+      int accelz = (int16_t)(psAxis[22]<<8) | psAxis[21];
+      int accely = (int16_t)(psAxis[24]<<8) | psAxis[23];
+  
+      ax = (float) accelx/8192;
+      ay = (float) accely/8192;
+      az = (float) accelz/8192;
+}
+
+void getAngles(float &p, float &r){
+  getAccel( ax,  ay,  az);
+  p = (atan2f(ay, az) + PI) * RAD_TO_DEG;
+  r = (atan2f(ax, az) + PI) * RAD_TO_DEG;
+}
+
+void getGyro(float &gx, float &gy, float &gz){
+  int gyroy = (int16_t)(psAxis[14]<<8) | psAxis[13];
+  int gyroz = (int16_t)(psAxis[16]<<8) | psAxis[15];
+  int gyrox = (int16_t)(psAxis[18]<<8) | psAxis[17];
+
+  gx = (float) gyrox * RAD_TO_DEG/1024;
+  gy = (float) gyroy * RAD_TO_DEG/1024;
+  gz = (float) gyroz * RAD_TO_DEG/1024;
+}
+  

examples/JoystickBT/JoystickBT/JoystickBT.ino

+
+// Simple test of USB Host Mouse/Keyboard
+//
+// This example is in the public domain
+
+#include "USBHost_t36.h"
+//#include "debug_tt.h"
+
+USBHost myusb;
+USBHub hub1(myusb);
+USBHub hub2(myusb);
+USBHIDParser hid1(myusb);
+USBHIDParser hid2(myusb);
+USBHIDParser hid3(myusb);
+USBHIDParser hid4(myusb);
+USBHIDParser hid5(myusb);
+JoystickController joystick1(myusb);
+/* For PS3 controller it must be USB paired in advance - 
+ * - plug in cable to PS3 - then Bt Dongle to see non-zero 
+ * BDADDR:
+ * To initiate Bt pair - while on USB:: Hold select button and 
+ * press PS button to set controller associate. After that you can
+ * comment out the bluetooth paring construcor and use second 
+ * constructor for already paired devices
+ */
+//BluetoothController bluet(myusb, true, "0000");   // Version does pairing to device
+BluetoothController bluet(myusb);   // version assumes it already was paired
+int user_axis[64];
+uint32_t buttons_prev = 0;
+uint32_t buttons;
+
+float pitch, roll;
+float ax, ay, az;
+
+RawHIDController rawhid1(myusb);
+RawHIDController rawhid2(myusb, 0xffc90004);
+
+USBDriver *drivers[] = {&hub1, &hub2, &joystick1, &bluet, &hid1, &hid2, &hid3, &hid4, &hid5};
+
+#define CNT_DEVICES (sizeof(drivers)/sizeof(drivers[0]))
+const char * driver_names[CNT_DEVICES] = {"Hub1","Hub2", "JOY1D", "Bluet", "HID1" , "HID2", "HID3", "HID4", "HID5"};
+
+bool driver_active[CNT_DEVICES] = {false, false, false, false};
+
+// Lets also look at HID Input devices
+USBHIDInput *hiddrivers[] = {&joystick1, &rawhid1, &rawhid2};
+
+#define CNT_HIDDEVICES (sizeof(hiddrivers)/sizeof(hiddrivers[0]))
+const char * hid_driver_names[CNT_DEVICES] = {"Joystick1", "RawHid1", "RawHid2"};
+
+bool hid_driver_active[CNT_DEVICES] = {false, false, false};
+bool show_changed_only = false; 
+bool show_raw_data = false;
+bool show_changed_data = false;
+
+uint8_t joystick_left_trigger_value = 0;
+uint8_t joystick_right_trigger_value = 0;
+uint64_t joystick_full_notify_mask = (uint64_t)-1;
+
+int psAxis[64];
+bool first_joystick_message = true;
+uint8_t last_bdaddr[6]={0,0,0,0,0,0};
+
+void setup()
+{
+  while (!Serial) ; // wait for Arduino Serial Monitor
+  Serial.println("\n\nUSB Host Testing");
+  Serial.println(sizeof(USBHub), DEC);
+  myusb.begin();
+
+  delay(2000);
+
+  rawhid1.attachReceive(OnReceiveHidData);
+  rawhid2.attachReceive(OnReceiveHidData);
+}
+
+
+void loop()
+{
+  myusb.Task();
+ 
+  if (Serial.available()) {
+    int ch = Serial.read(); // get the first char. 
+    while (Serial.read() != -1) ; 
+    if ((ch == 'b') || (ch == 'B')) {
+      Serial.println("Only notify on Basic Axis changes");
+      joystick1.axisChangeNotifyMask(0x3ff);
+    } else if ((ch == 'f') || (ch == 'F')) {
+      Serial.println("Only notify on Full Axis changes");
+      joystick1.axisChangeNotifyMask(joystick_full_notify_mask);
+
+    } else {
+      if (show_changed_only) {
+        show_changed_only = false;
+        Serial.println("\n*** Show All fields mode ***");
+      } else {
+        show_changed_only = true;
+        Serial.println("\n*** Show only changed fields mode ***");
+      }
+    }
+ }
+
+  for (uint8_t i = 0; i < CNT_DEVICES; i++) {
+    if (*drivers[i] != driver_active[i]) {
+      if (driver_active[i]) {
+        Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);
+        driver_active[i] = false;
+      } else {
+        Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());
+        driver_active[i] = true;
+
+        const uint8_t *psz = drivers[i]->manufacturer();
+        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
+        psz = drivers[i]->product();
+        if (psz && *psz) Serial.printf("  product: %s\n", psz);
+        psz = drivers[i]->serialNumber();
+        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
+
+        if (drivers[i] == &bluet) {
+          const uint8_t *bdaddr = bluet.myBDAddr();
+          // remember it...
+          Serial.printf("  BDADDR: %x:%x:%x:%x:%x:%x\n", bdaddr[0], bdaddr[1], bdaddr[2], bdaddr[3], bdaddr[4], bdaddr[5]);
+          for (uint8_t i=0;i<6;i++) last_bdaddr[i] = bdaddr[i];
+        }
+      }
+    }
+  }
+
+  for (uint8_t i = 0; i < CNT_HIDDEVICES; i++) {
+    if (*hiddrivers[i] != hid_driver_active[i]) {
+      if (hid_driver_active[i]) {
+        Serial.printf("*** HID Device %s - disconnected ***\n", hid_driver_names[i]);
+        hid_driver_active[i] = false;
+      } else {
+        Serial.printf("*** HID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());
+        hid_driver_active[i] = true;
+
+        const uint8_t *psz = hiddrivers[i]->manufacturer();
+        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
+        psz = hiddrivers[i]->product();
+        if (psz && *psz) Serial.printf("  product: %s\n", psz);
+        psz = hiddrivers[i]->serialNumber();
+        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);
+      }
+    }
+  }
+
+  if (joystick1.available()) {
+      if (first_joystick_message) {
+        Serial.printf("*** First Joystick message %x:%x ***\n", 
+            joystick1.idVendor(), joystick1.idProduct());
+        first_joystick_message = false;
+
+        const uint8_t *psz = joystick1.manufacturer();
+        if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);
+        psz = joystick1.product();
+        if (psz && *psz) Serial.printf("  product: %s\n", psz);
+        psz =joystick1.serialNumber();
+        if (psz && *psz) Serial.printf("  Serial: %s\n", psz);    
+
+        // lets try to reduce number of fields that update
+         joystick1.axisChangeNotifyMask(0xFFFFFl);    
+      }
+      
+      for (uint8_t i = 0; i<64; i++) {
+          psAxis[i] = joystick1.getAxis(i);
+      }
+      switch (joystick1.joystickType()) {
+        case JoystickController::UNKNOWN:
+        case JoystickController::PS4:
+          displayPS4Data();
+          break;
+        case JoystickController::PS3:
+          displayPS3Data();
+          break;
+        default:  
+        case JoystickController::XBOXONE:
+        case JoystickController::XBOX360:;
+          displayRawData();
+          break;
+      }
+      //for (uint8_t i = 0; i < 24; i++) {
+      //  Serial.printf(" %d:%d", i, psAxis[i]);
+      //}
+      //Serial.println();
+        
+     delay(100);
+     joystick1.joystickDataClear();
+  }
+
+  // See if we have some RAW data
+  if (rawhid1) {
+    int ch;
+    uint8_t buffer[64];
+    uint8_t count_chars = 0; 
+    memset(buffer, 0, sizeof(buffer));
+    if (Serial.available()) {
+      while (((ch = Serial.read()) != -1) && (count_chars < sizeof(buffer))) {
+        buffer[count_chars++] = ch;
+      }
+      rawhid1.sendPacket(buffer);
+    }
+  }
+}
+
+void displayPS4Data()
+{
+  buttons = joystick1.getButtons();
+  Serial.printf("LX: %d, LY: %d, RX: %d, RY: %d \r\n", psAxis[0], psAxis[1], psAxis[2], psAxis[5]);
+  Serial.printf("L-Trig: %d, R-Trig: %d\r\n", psAxis[3], psAxis[4]);
+  Serial.printf("Buttons: %x\r\n", buttons);
+  Serial.printf("Battery Status: %d\n", ((psAxis[30] & (1 << 4) - 1)*10));
+  printAngles();
+  Serial.println();
+
+  uint8_t ltv;
+  uint8_t rtv;
+
+    ltv = psAxis[3];
+    rtv = psAxis[4];
+
+    if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {
+      joystick_left_trigger_value = ltv;
+      joystick_right_trigger_value = rtv;
+      Serial.printf("Rumbling: %d, %d\r\n", ltv, rtv);
+      joystick1.setRumble(ltv, rtv);
+    }
+   
+
+  if (buttons != buttons_prev) {
+      uint8_t lr = 0;
+      uint8_t lg = 0;
+      uint8_t lb = 0;
+      if(buttons == 0x10008){//Srq
+        lr = 0xff;
+      }
+      if(buttons == 0x40008){//Circ
+        lg = 0xff;
+      }
+      if(buttons == 0x80008){//Tri
+        lb = 0xff;
+      }
+      
+      Serial.print(buttons,HEX); Serial.print(", ");
+      Serial.print(lr); Serial.print(", "); 
+      Serial.print(lg); Serial.print(", "); 
+      Serial.println(lb); 
+      joystick1.setLEDs(lr, lg, lb);
+      buttons_prev = buttons;  
+  }
+
+}
+
+void displayPS3Data()
+
+{
+  buttons = joystick1.getButtons();
+  //buttons = psAxis[2] | ((uint16_t)psAxis[3] << 8);
+
+  // Use L3 (Left joystick button) to toggle Show Raw or not...
+  if ((buttons & 0x02) && !(buttons_prev & 0x02)) show_raw_data = !show_raw_data;
+  if ((buttons & 0x04) && !(buttons_prev & 0x04)) show_changed_data = !show_changed_data;
+
+  // See about maybe pair... 
+  if ((buttons & 0x10000) && !(buttons_prev & 0x10000) && (buttons & 0x0001)) {
+    // PS button just pressed and select button pressed act like PS4 share like...
+    Serial.print("\nPS3 Pairing Request");
+    if (!last_bdaddr[0] && !last_bdaddr[1] && !last_bdaddr[2] && !last_bdaddr[3] && !last_bdaddr[4] && !last_bdaddr[5]) {
+      Serial.println(" - failed - no Bluetooth adapter has been plugged in");
+    } else if (!hiddrivers[0]) {  // Kludge see if we are connected as HID?
+      Serial.println(" - failed - PS3 device not plugged into USB");
+    } else {
+      Serial.printf(" - Attempt pair to: %x:%x:%x:%x:%x:%x\n", last_bdaddr[0], last_bdaddr[1], last_bdaddr[2], last_bdaddr[3], last_bdaddr[4], last_bdaddr[5]);
+
+      if (! joystick1.PS3Pair(last_bdaddr)) {
+        Serial.println("  Pairing call Failed");
+      } else {
+        Serial.println("  Pairing complete (I hope), make sure Bluetooth adapter is plugged in and try PS3 without USB");
+      }
+    }
+  } 
+
+
+  if (show_raw_data)  {
+    displayRawData();
+  } else {
+    Serial.printf("LX: %d, LY: %d, RX: %d, RY: %d \r\n", psAxis[0], psAxis[1], psAxis[2], psAxis[5]);
+    Serial.printf("L-Trig: %d, R-Trig: %d\r\n", psAxis[3], psAxis[4]);
+    Serial.printf("Buttons: %x\r\n", buttons);
+  }
+    uint8_t ltv;
+    uint8_t rtv;
+
+    ltv = psAxis[3];
+    rtv = psAxis[4];
+
+    if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {
+      joystick_left_trigger_value = ltv;
+      joystick_right_trigger_value = rtv;
+      Serial.printf("Rumbling: %d, %d\r\n", ltv, rtv);
+      joystick1.setRumble(ltv, rtv, 50);
+    }
+
+    if (buttons != buttons_prev) {
+      uint8_t leds = 0;
+      if (buttons & 0x8000) leds = 1;   //Srq
+      if (buttons & 0x2000) leds = 2;   //Cir
+      if (buttons & 0x1000) leds = 4;   //Tri
+      if (buttons & 0x4000) leds = 8;   //X      joystick1.setLEDs(leds);
+      buttons_prev = buttons;
+  }
+}
+ 
+
+void displayRawData() {
+  uint64_t axis_mask = joystick1.axisMask();
+  uint64_t changed_mask = joystick1.axisChangedMask();
+  if (!changed_mask) return;
+
+  if (show_changed_data) {
+    if (!changed_mask) return;
+    changed_mask &= 0xfffffffffL; // try reducing which ones show...
+    Serial.printf("%0x - ", joystick1.getButtons());
+
+    for (uint16_t index=0; changed_mask; index++) {
+      if (changed_mask & 1) {
+        Serial.printf("%d:%02x ", index, psAxis[index]);
+      }
+      changed_mask >>= 1;
+    }
+
+  } else {
+    axis_mask &= 0xffffff;
+    Serial.printf("%06x%06x: %06x - ", (uint32_t)(changed_mask >> 32), (uint32_t)(changed_mask & 0xffffffff), joystick1.getButtons());
+
+    for (uint16_t index=0; axis_mask; index++) {
+      Serial.printf("%02x ", psAxis[index]);
+      axis_mask >>= 1;
+    }
+  }
+ Serial.println();
+ 
+}
+
+
+bool OnReceiveHidData(uint32_t usage, const uint8_t *data, uint32_t len) {
+  // Called for maybe both HIDS for rawhid basic test.  One is for the Teensy
+  // to output to Serial. while still having Raw Hid... 
+  if (usage == 0xffc90004) {
+    // Lets trim off trailing null characters.
+    while ((len > 0) && (data[len-1] == 0)) {
+      len--;
+    }
+    if (len) {
+      Serial.print("RawHid Serial: ");
+      Serial.write(data, len);
+    }
+  } else {
+    Serial.print("RawHID data: ");
+    Serial.println(usage, HEX);
+    while (len) {
+      uint8_t cb = (len > 16)? 16 : len;
+      const uint8_t *p = data;
+      uint8_t i;
+      for (i = 0; i < cb; i++) {
+        Serial.printf("%02x ", *p++);
+      }
+      Serial.print(": ");
+      for (i = 0; i < cb; i++) {
+        Serial.write(((*data >= ' ')&&(*data <= '~'))? *data : '.');
+        data++;
+      }
+      len -= cb;
+      Serial.println();
+    }
+  }
+
+  return true;
+}

examples/JoystickBT/JoystickBT/defs.h

+

examples/JoystickBT/JoystickBT/helperPS.ino

+float gx, gy, gz;
+uint16_t xc, yc; 
+uint8_t isTouch;
+int16_t xc_old, yc_old;
+
+void printAngles(){
+  //test function calls
+  float gx, gy, gz;
+  getAccel(ax, ay, az);
+  Serial.printf("Accel-g's: %f, %f, %f\n", ax, ay, az);
+  getGyro(gx, gy, gz);
+  Serial.printf("Gyro-deg/sec: %f, %f, %f\n", gx, gy, gz);
+
+  getAngles(pitch, roll);
+  Serial.printf("Pitch/Roll: %f, %f\n", pitch, roll);
+
+  getCoords(xc, yc, isTouch);
+}
+
+void getCoords(uint16_t &xc, uint16_t &yc, uint8_t &isTouch){
+
+	//uint8_t finger = 0;  //only getting finger 1
+	uint8_t Id = 0;
+
+
+  // Trackpad touch 1: id, active, x, y
+  xc = ((psAxis[37] & 0x0f) << 8) | psAxis[36];
+  yc = psAxis[38] << 4 | ((psAxis[37] & 0xf0) >> 4),
+
+	isTouch = psAxis[35] >> 7;
+  if(xc != xc_old || yc != yc_old){
+    Serial.printf("Touch: %d, %d, %d, %d\n", psAxis[33], isTouch, xc, yc);
+    xc_old = xc;
+    yc_old = yc;
+  }
+}
+
+void getAccel( float &ax,  float &ay,  float &az){
+      int accelx = (int16_t)(psAxis[20]<<8) | psAxis[19];
+      int accelz = (int16_t)(psAxis[22]<<8) | psAxis[21];
+      int accely = (int16_t)(psAxis[24]<<8) | psAxis[23];
+  
+      ax = (float) accelx/8192;
+      ay = (float) accely/8192;
+      az = (float) accelz/8192;
+}
+
+void getAngles(float &p, float &r){
+	getAccel( ax,  ay,  az);
+	p = (atan2f(ay, az) + PI) * RAD_TO_DEG;
+	r = (atan2f(ax, az) + PI) * RAD_TO_DEG;
+}
+
+void getGyro(float &gx, float &gy, float &gz){
+	int gyroy = (int16_t)(psAxis[14]<<8) | psAxis[13];
+	int gyroz = (int16_t)(psAxis[16]<<8) | psAxis[15];
+	int gyrox = (int16_t)(psAxis[18]<<8) | psAxis[17];
+
+	gx = (float) gyrox * RAD_TO_DEG/1024;
+	gy = (float) gyroy * RAD_TO_DEG/1024;
+	gz = (float) gyroz * RAD_TO_DEG/1024;
+}
+