visus
/
teensy-usbhost
关注 1
点赞 0
派生 0
代码 工单 0 合并请求 0 版本发布 0 百科 动态
您最多选择25个主题 主题必须以字母或数字开头，可以包含连字符 (-)，并且长度不得超过35个字符
304 提交
1 分支
目录树: 2c5cbccebc
分支列表 标签列表
${ item.name }
创建分支 ${ searchTerm }
从 '2c5cbccebc'
${ noResults }
teensy-usbhost/examples/Bluetooth/JoystickBT/JoystickBT.ino

587 行
20KB
原始文件 Blame 文件历史 

  1. 

  2. // Simple test of USB Host Mouse/Keyboard

  3. //

  4. // This example is in the public domain

  5. 

  6. #include "USBHost_t36.h"

  7. //#include "debug_tt.h"

  8. 

  9. USBHost myusb;

  10. USBHub hub1(myusb);

  11. USBHub hub2(myusb);

  12. USBHIDParser hid1(myusb);

  13. USBHIDParser hid2(myusb);

  14. USBHIDParser hid3(myusb);

  15. USBHIDParser hid4(myusb);

  16. USBHIDParser hid5(myusb);

  17. JoystickController joystick1(myusb);

  18. //BluetoothController bluet(myusb, true, "0000");   // Version does pairing to device

  19. BluetoothController bluet(myusb);   // version assumes it already was paired

  20. int user_axis[64];

  21. uint32_t buttons_prev = 0;

  22. uint32_t buttons;

  23. RawHIDController rawhid1(myusb);

  24. RawHIDController rawhid2(myusb, 0xffc90004);

  25. 

  26. USBDriver *drivers[] = {&hub1, &hub2, &joystick1, &bluet, &hid1, &hid2, &hid3, &hid4, &hid5};

  27. 

  28. #define CNT_DEVICES (sizeof(drivers)/sizeof(drivers[0]))

  29. const char * driver_names[CNT_DEVICES] = {"Hub1", "Hub2", "JOY1D", "Bluet", "HID1" , "HID2", "HID3", "HID4", "HID5"};

  30. 

  31. bool driver_active[CNT_DEVICES] = {false, false, false, false};

  32. 

  33. // Lets also look at HID Input devices

  34. USBHIDInput *hiddrivers[] = {&joystick1, &rawhid1, &rawhid2};

  35. 

  36. #define CNT_HIDDEVICES (sizeof(hiddrivers)/sizeof(hiddrivers[0]))

  37. const char * hid_driver_names[CNT_DEVICES] = {"Joystick1", "RawHid1", "RawHid2"};

  38. 

  39. bool hid_driver_active[CNT_DEVICES] = {false, false, false};

  40. 

  41. BTHIDInput *bthiddrivers[] = {&joystick1};

  42. #define CNT_BTHIDDEVICES (sizeof(bthiddrivers)/sizeof(bthiddrivers[0]))

  43. const char * bthid_driver_names[CNT_HIDDEVICES] = {"joystick"};

  44. bool bthid_driver_active[CNT_HIDDEVICES] = {false};

  45. 

  46. 

  47. bool show_changed_only = false;

  48. bool show_raw_data = false;

  49. bool show_changed_data = false;

  50. 

  51. uint8_t joystick_left_trigger_value = 0;

  52. uint8_t joystick_right_trigger_value = 0;

  53. uint64_t joystick_full_notify_mask = (uint64_t) - 1;

  54. 

  55. int psAxis[64];

  56. bool first_joystick_message = true;

  57. uint8_t last_bdaddr[6] = {0, 0, 0, 0, 0, 0};

  58. 

  59. // ps3 motion on USB does not do much, but see if we can pair it and maybe change

  60. // color of bulb... 

  61. uint32_t PS3_MOTION_timer = 0;

  62. uint8_t  PS3_MOTION_tried_to_pair_state = 0;

  63. #define PS3_MOTION_PERIOD 2500 // not sure yet what would be good period for this..

  64. 

  65. //=============================================================================

  66. // Setup

  67. //=============================================================================

  68. void setup()

  69. {

  70. 

  71.   Serial1.begin(2000000);

  72.   while (!Serial) ; // wait for Arduino Serial Monitor

  73. 

  74.   Serial.println("\n\nUSB Host Testing");

  75.   Serial.println(sizeof(USBHub), DEC);

  76.   myusb.begin();

  77. 

  78.   delay(2000);

  79. 

  80.   rawhid1.attachReceive(OnReceiveHidData);

  81.   rawhid2.attachReceive(OnReceiveHidData);

  82. }

  83. 

  84. //=============================================================================

  85. // Loop

  86. //=============================================================================

  87. void loop()

  88. {

  89.   myusb.Task();

  90. 

  91.   if (Serial.available()) {

  92.     int ch = Serial.read(); // get the first char.

  93.     while (Serial.read() != -1) ;

  94.     if ((ch == 'b') || (ch == 'B')) {

  95.       Serial.println("Only notify on Basic Axis changes");

  96.       joystick1.axisChangeNotifyMask(0x3ff);

  97.     } else if ((ch == 'f') || (ch == 'F')) {

  98.       Serial.println("Only notify on Full Axis changes");

  99.       joystick1.axisChangeNotifyMask(joystick_full_notify_mask);

  100. 

  101.     } else {

  102.       if (show_changed_only) {

  103.         show_changed_only = false;

  104.         Serial.println("\n*** Show All fields mode ***");

  105.       } else {

  106.         show_changed_only = true;

  107.         Serial.println("\n*** Show only changed fields mode ***");

  108.       }

  109.     }

  110.   }

  111.   // check to see if the device list has changed:

  112.   UpdateActiveDeviceInfo();

  113.   processPS3MotionTimer();

  114. 

  115.   if (joystick1.available()) {

  116.     if (first_joystick_message) {

  117.       Serial.printf("*** First Joystick message %x:%x ***\n",

  118.                     joystick1.idVendor(), joystick1.idProduct());

  119.       first_joystick_message = false;

  120. 

  121.       const uint8_t *psz = joystick1.manufacturer();

  122.       if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);

  123.       psz = joystick1.product();

  124.       if (psz && *psz) Serial.printf("  product: %s\n", psz);

  125.       psz = joystick1.serialNumber();

  126.       if (psz && *psz) Serial.printf("  Serial: %s\n", psz);

  127. 

  128.       // lets try to reduce number of fields that update

  129.       joystick1.axisChangeNotifyMask(0xFFFFFl);

  130.     }

  131. 

  132.     for (uint8_t i = 0; i < 64; i++) {

  133.       psAxis[i] = joystick1.getAxis(i);

  134.     }

  135.     switch (joystick1.joystickType()) {

  136.       case JoystickController::UNKNOWN:

  137.       case JoystickController::PS4:

  138.         displayPS4Data();

  139.         break;

  140.       case JoystickController::PS3:

  141.         displayPS3Data();

  142.         break;

  143.       case JoystickController::PS3_MOTION:

  144.         displayPS3MotionData();

  145.         break;

  146.       case JoystickController::XBOXONE:

  147.       case JoystickController::XBOX360:

  148.         displayXBoxData();

  149.         break;

  150.       default:

  151.         displayRawData();

  152.         break;

  153.     }

  154.     //for (uint8_t i = 0; i < 24; i++) {

  155.     //  Serial.printf(" %d:%d", i, psAxis[i]);

  156.     //}

  157.     //Serial.println();

  158. 

  159.     delay(100);

  160.     joystick1.joystickDataClear();

  161.   }

  162. 

  163.   // See if we have some RAW data

  164.   if (rawhid1) {

  165.     int ch;

  166.     uint8_t buffer[64];

  167.     uint8_t count_chars = 0;

  168.     memset(buffer, 0, sizeof(buffer));

  169.     if (Serial.available()) {

  170.       while (((ch = Serial.read()) != -1) && (count_chars < sizeof(buffer))) {

  171.         buffer[count_chars++] = ch;

  172.       }

  173.       rawhid1.sendPacket(buffer);

  174.     }

  175.   }

  176. }

  177. 

  178. //=============================================================================

  179. // UpdateActiveDeviceInfo

  180. //=============================================================================

  181. void UpdateActiveDeviceInfo() {

  182.   for (uint8_t i = 0; i < CNT_DEVICES; i++) {

  183.     if (*drivers[i] != driver_active[i]) {

  184.       if (driver_active[i]) {

  185.         Serial.printf("*** Device %s - disconnected ***\n", driver_names[i]);

  186.         driver_active[i] = false;

  187.       } else {

  188.         Serial.printf("*** Device %s %x:%x - connected ***\n", driver_names[i], drivers[i]->idVendor(), drivers[i]->idProduct());

  189.         driver_active[i] = true;

  190. 

  191.         const uint8_t *psz = drivers[i]->manufacturer();

  192.         if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);

  193.         psz = drivers[i]->product();

  194.         if (psz && *psz) Serial.printf("  product: %s\n", psz);

  195.         psz = drivers[i]->serialNumber();

  196.         if (psz && *psz) Serial.printf("  Serial: %s\n", psz);

  197. 

  198.         if (drivers[i] == &bluet) {

  199.           const uint8_t *bdaddr = bluet.myBDAddr();

  200.           // remember it...

  201.           Serial.printf("  BDADDR: %x:%x:%x:%x:%x:%x\n", bdaddr[0], bdaddr[1], bdaddr[2], bdaddr[3], bdaddr[4], bdaddr[5]);

  202.           for (uint8_t i = 0; i < 6; i++) last_bdaddr[i] = bdaddr[i];

  203.         }

  204.       }

  205.     }

  206.   }

  207. 

  208.   for (uint8_t i = 0; i < CNT_HIDDEVICES; i++) {

  209.     if (*hiddrivers[i] != hid_driver_active[i]) {

  210.       if (hid_driver_active[i]) {

  211.         Serial.printf("*** HID Device %s - disconnected ***\n", hid_driver_names[i]);

  212.         hid_driver_active[i] = false;

  213.       } else {

  214.         Serial.printf("*** HID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());

  215.         hid_driver_active[i] = true;

  216. 

  217.         const uint8_t *psz = hiddrivers[i]->manufacturer();

  218.         if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);

  219.         psz = hiddrivers[i]->product();

  220.         if (psz && *psz) Serial.printf("  product: %s\n", psz);

  221.         psz = hiddrivers[i]->serialNumber();

  222.         if (psz && *psz) Serial.printf("  Serial: %s\n", psz);

  223. 

  224.         // See if this is our joystick object...

  225.         if (hiddrivers[i] == &joystick1) {

  226.           Serial.printf("  Joystick type: %d\n", joystick1.joystickType());

  227.           if (joystick1.joystickType() == JoystickController::PS3_MOTION) {

  228.             Serial.println("  PS3 Motion detected");

  229.             PS3_MOTION_timer = millis();  // set time for last event

  230.             PS3_MOTION_tried_to_pair_state = 0; 

  231.           }

  232.         }

  233. 

  234.       }

  235.     }

  236.   }

  237.   // Then Bluetooth devices

  238.   for (uint8_t i = 0; i < CNT_BTHIDDEVICES; i++) {

  239.     if (*bthiddrivers[i] != bthid_driver_active[i]) {

  240.       if (bthid_driver_active[i]) {

  241.         Serial.printf("*** BTHID Device %s - disconnected ***\n", hid_driver_names[i]);

  242.         bthid_driver_active[i] = false;

  243.       } else {

  244.         Serial.printf("*** BTHID Device %s %x:%x - connected ***\n", hid_driver_names[i], hiddrivers[i]->idVendor(), hiddrivers[i]->idProduct());

  245.         bthid_driver_active[i] = true;

  246. 

  247.         const uint8_t *psz = bthiddrivers[i]->manufacturer();

  248.         if (psz && *psz) Serial.printf("  manufacturer: %s\n", psz);

  249.         psz = bthiddrivers[i]->product();

  250.         if (psz && *psz) Serial.printf("  product: %s\n", psz);

  251.         psz = bthiddrivers[i]->serialNumber();

  252.         if (psz && *psz) Serial.printf("  Serial: %s\n", psz);

  253.       }

  254.     }

  255.   }

  256. }

  257. 

  258. //=============================================================================

  259. // displayPS4Data

  260. //=============================================================================

  261. void displayPS4Data()

  262. {

  263.   buttons = joystick1.getButtons();

  264.   Serial.printf("LX: %d, LY: %d, RX: %d, RY: %d \r\n", psAxis[0], psAxis[1], psAxis[2], psAxis[5]);

  265.   Serial.printf("L-Trig: %d, R-Trig: %d\r\n", psAxis[3], psAxis[4]);

  266.   Serial.printf("Buttons: %x\r\n", buttons);

  267.   Serial.printf("Battery Status: %d\n", ((psAxis[30] & ((1 << 4) - 1))*10));

  268.   printAngles();

  269.   Serial.println();

  270. 

  271.   uint8_t ltv;

  272.   uint8_t rtv;

  273. 

  274.   ltv = psAxis[3];

  275.   rtv = psAxis[4];

  276. 

  277.   if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {

  278.     joystick_left_trigger_value = ltv;

  279.     joystick_right_trigger_value = rtv;

  280.     Serial.printf("Rumbling: %d, %d\r\n", ltv, rtv);

  281.     joystick1.setRumble(ltv, rtv);

  282.   }

  283.    

  284. 

  285.   if (buttons != buttons_prev) {

  286.       uint8_t lr = 0;

  287.       uint8_t lg = 0;

  288.       uint8_t lb = 0;

  289.       if(buttons == 0x10008){//Srq

  290.         lr = 0xff;

  291.       }

  292.       if(buttons == 0x40008){//Circ

  293.         lg = 0xff;

  294.       }

  295.       if(buttons == 0x80008){//Tri

  296.         lb = 0xff;

  297.       }

  298.       

  299.       Serial.print(buttons,HEX); Serial.print(", ");

  300.       Serial.print(lr); Serial.print(", "); 

  301.       Serial.print(lg); Serial.print(", "); 

  302.       Serial.println(lb); 

  303.       joystick1.setLEDs(lr, lg, lb);

  304.       buttons_prev = buttons;  

  305.   }

  306. 

  307. }

  308. 

  309. //=============================================================================

  310. // displayPS3Data

  311. //=============================================================================

  312. void displayPS3Data()

  313. 

  314. {

  315.   buttons = joystick1.getButtons();

  316.   //buttons = psAxis[2] | ((uint16_t)psAxis[3] << 8);

  317. 

  318.   // Use L3 (Left joystick button) to toggle Show Raw or not...

  319.   if ((buttons & 0x02) && !(buttons_prev & 0x02)) show_raw_data = !show_raw_data;

  320.   if ((buttons & 0x04) && !(buttons_prev & 0x04)) show_changed_data = !show_changed_data;

  321. 

  322.   // See about maybe pair...

  323.   if ((buttons & 0x10000) && !(buttons_prev & 0x10000) && (buttons & 0x0C01)) {

  324.     // PS button just pressed and select button pressed act like PS4 share like...

  325.     // Note: you can use either R1 or L1 with the PS button, to work with Sony Move Navigation...

  326.     Serial.print("\nPS3 Pairing Request");

  327.     if (!last_bdaddr[0] && !last_bdaddr[1] && !last_bdaddr[2] && !last_bdaddr[3] && !last_bdaddr[4] && !last_bdaddr[5]) {

  328.       Serial.println(" - failed - no Bluetooth adapter has been plugged in");

  329.     } else if (!hiddrivers[0]) {  // Kludge see if we are connected as HID?

  330.       Serial.println(" - failed - PS3 device not plugged into USB");

  331.     } else {

  332.       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]);

  333. 

  334.       if (! joystick1.PS3Pair(last_bdaddr)) {

  335.         Serial.println("  Pairing call Failed");

  336.       } else {

  337.         Serial.println("  Pairing complete (I hope), make sure Bluetooth adapter is plugged in and try PS3 without USB");

  338.       }

  339.     }

  340.   }

  341. 

  342. 

  343.   if (show_raw_data)  {

  344.     displayRawData();

  345.   } else {

  346.     Serial.printf("LX: %d, LY: %d, RX: %d, RY: %d \r\n", psAxis[0], psAxis[1], psAxis[2], psAxis[5]);

  347.     Serial.printf("L-Trig: %d, R-Trig: %d\r\n", psAxis[3], psAxis[4]);

  348.     Serial.printf("Buttons: %x\r\n", buttons);

  349.   }

  350.   uint8_t ltv;

  351.   uint8_t rtv;

  352. 

  353.   ltv = psAxis[3];

  354.   rtv = psAxis[4];

  355. 

  356.   if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {

  357.     joystick_left_trigger_value = ltv;

  358.     joystick_right_trigger_value = rtv;

  359.     Serial.printf("Rumbling: %d, %d\r\n", ltv, rtv);

  360.     joystick1.setRumble(ltv, rtv);

  361.   }

  362. 

  363.   if (buttons != buttons_prev) {

  364.     uint8_t leds = 0;

  365.     if (buttons & 0x8000) leds = 1;   //Srq

  366.     if (buttons & 0x2000) leds = 2;   //Cir

  367.     if (buttons & 0x1000) leds = 3;   //Tri

  368.     //Cross = 2

  369.     joystick1.setLEDs(leds);

  370.     buttons_prev = buttons;

  371.   }

  372. }

  373. //=============================================================================

  374. // displayPS3MotionData

  375. //=============================================================================

  376. void displayPS3MotionData()

  377. 

  378. {

  379.   buttons = joystick1.getButtons();

  380. 

  381.   // Hard to know what is best here. for now just copy raw data over... 

  382.   // will do this for now... Format of thought to be data.

  383.   //  data[1-3] Buttons (mentioned 4 as well but appears to be counter

  384.   // axis[0-1] data[5] Trigger, Previous trigger value

  385.   // 2-5 Unknown probably place holders for Axis like data for other PS3

  386.   // 6 - Time stamp

  387.   // 7 - Battery

  388.   // 8-19 - Accel: XL, XH, YL, YH, ZL, ZH, XL2, XH2, YL2, YH2, ZL2, ZH2

  389.   // 20-31 - Gyro: Xl,Xh,Yl,Yh,Zl,Zh,Xl2,Xh2,Yl2,Yh2,Zl2,Zh2

  390.   // 32 - Temp High

  391.   // 33 - Temp Low (4 bits)  Maybe Magneto x High on other?? 

  392. 

  393.   // Use Select button to choose raw or not

  394.   if ((buttons & 0x01) && !(buttons_prev & 0x01)) show_raw_data = !show_raw_data;

  395.   if ((buttons & 0x04) && !(buttons_prev & 0x04)) show_changed_data = !show_changed_data;

  396. 

  397.   if (show_raw_data)  {

  398.     displayRawData();

  399.   } else {

  400.     uint64_t changed_mask = joystick1.axisChangedMask();

  401.     Serial.printf("Changed: %08x Buttons: %x: Trig: %d\r\n", (uint32_t)changed_mask, buttons, psAxis[0]);

  402.     Serial.printf("Battery Status: %d\n", psAxis[7]);

  403.     printPS3MotionAngles();

  404.     Serial.println();

  405.   }

  406. 

  407.   uint8_t ltv = psAxis[0];

  408. 

  409.   if ((ltv != joystick_left_trigger_value) ) {

  410.     joystick_left_trigger_value = ltv;

  411.     Serial.printf("Rumbling: %d\r\n", ltv);

  412.     joystick1.setRumble(ltv, 0);

  413.   }

  414. 

  415.   if (buttons != buttons_prev) {

  416.     uint8_t ledsR = (buttons & 0x8000)? 0xff : 0;   //Srq

  417.     uint8_t ledsG = (buttons & 0x2000)? 0xff : 0;   //Cir

  418.     uint8_t ledsB = (buttons & 0x1000)? 0xff : 0;   //Tri

  419.     Serial.printf("Set Leds %x %x %x\r\n", ledsR, ledsG, ledsB );

  420.     joystick1.setLEDs(ledsR, ledsG, ledsB);

  421.     buttons_prev = buttons;

  422.   }

  423. }

  424. 

  425. //=============================================================================

  426. // displayXBoxData

  427. //=============================================================================

  428. void displayXBoxData()

  429. {

  430.   buttons = joystick1.getButtons();

  431. 

  432.   // Use L3 (Left joystick button) to toggle Show Raw or not...

  433.   if ((buttons & 0x4000) && !(buttons_prev & 0x4000)) show_raw_data = !show_raw_data;

  434.   if ((buttons & 0x8000) && !(buttons_prev & 0x8000)) show_changed_data = !show_changed_data;

  435. 

  436.   if (show_raw_data)  {

  437.     displayRawData();

  438.   } else {

  439.     Serial.printf("LX: %d, LY: %d, RX: %d, RY: %d \r\n", psAxis[0], psAxis[1], psAxis[2], psAxis[5]);

  440.     Serial.printf("L-Trig: %d, R-Trig: %d\r\n", psAxis[3], psAxis[4]);

  441.     Serial.printf("Buttons: %x\r\n", buttons);

  442.   }

  443.   uint8_t ltv;

  444.   uint8_t rtv;

  445. 

  446.   ltv = psAxis[3];

  447.   rtv = psAxis[4];

  448. 

  449.   if ((ltv != joystick_left_trigger_value) || (rtv != joystick_right_trigger_value)) {

  450.     joystick_left_trigger_value = ltv;

  451.     joystick_right_trigger_value = rtv;

  452.     Serial.printf("Rumbling: %d, %d\r\n", ltv, rtv);

  453.     joystick1.setRumble(ltv, rtv);

  454.   }

  455. 

  456.   if (buttons != buttons_prev) {

  457.     uint8_t leds = 0;

  458.     if (buttons & 0x8000) leds = 1;   //Srq

  459.     if (buttons & 0x2000) leds = 2;   //Cir

  460.     if (buttons & 0x1000) leds = 3;   //Tri

  461.     //Cross = 2

  462.     joystick1.setLEDs(leds);

  463.     buttons_prev = buttons;

  464.   }

  465. }

  466. 

  467. //=============================================================================

  468. // displayRawData

  469. //=============================================================================

  470. void displayRawData() {

  471.   uint64_t axis_mask = joystick1.axisMask();

  472.   uint64_t changed_mask = joystick1.axisChangedMask();

  473. 

  474.   buttons = joystick1.getButtons();

  475. 

  476.   if (!changed_mask && (buttons == buttons_prev)) return;

  477. 

  478.   if (show_changed_data) {

  479.     if (!changed_mask) return;

  480.     changed_mask &= 0xfffffffffL; // try reducing which ones show...

  481.     Serial.printf("%0x - ", joystick1.getButtons());

  482. 

  483.     for (uint16_t index = 0; changed_mask; index++) {

  484.       if (changed_mask & 1) {

  485.         Serial.printf("%d:%02x ", index, psAxis[index]);

  486.       }

  487.       changed_mask >>= 1;

  488.     }

  489. 

  490.   } else {

  491.     axis_mask &= 0xffffff;

  492.     Serial.printf("%06x%06x: %06x - ", (uint32_t)(changed_mask >> 32), (uint32_t)(changed_mask & 0xffffffff), joystick1.getButtons());

  493. 

  494.     for (uint16_t index = 0; axis_mask; index++) {

  495.       Serial.printf("%02x ", psAxis[index]);

  496.       axis_mask >>= 1;

  497.     }

  498.   }

  499.   Serial.println();

  500.   buttons_prev = buttons;

  501. 

  502. }

  503. 

  504. 

  505. //=============================================================================

  506. // OnReceiveHidData

  507. //=============================================================================

  508. bool OnReceiveHidData(uint32_t usage, const uint8_t *data, uint32_t len) {

  509.   // Called for maybe both HIDS for rawhid basic test.  One is for the Teensy

  510.   // to output to Serial. while still having Raw Hid...

  511.   if (usage == 0xffc90004) {

  512.     // Lets trim off trailing null characters.

  513.     while ((len > 0) && (data[len - 1] == 0)) {

  514.       len--;

  515.     }

  516.     if (len) {

  517.       Serial.print("RawHid Serial: ");

  518.       Serial.write(data, len);

  519.     }

  520.   } else {

  521.     Serial.print("RawHID data: ");

  522.     Serial.println(usage, HEX);

  523.     while (len) {

  524.       uint8_t cb = (len > 16) ? 16 : len;

  525.       const uint8_t *p = data;

  526.       uint8_t i;

  527.       for (i = 0; i < cb; i++) {

  528.         Serial.printf("%02x ", *p++);

  529.       }

  530.       Serial.print(": ");

  531.       for (i = 0; i < cb; i++) {

  532.         Serial.write(((*data >= ' ') && (*data <= '~')) ? *data : '.');

  533.         data++;

  534.       }

  535.       len -= cb;

  536.       Serial.println();

  537.     }

  538.   }

  539. 

  540.   return true;

  541. }

  542. 

  543. //=============================================================================

  544. // processPS3MotionTimer

  545. //=============================================================================

  546. static const uint32_t PS3_MOTION_colors[] = {0, 0xff, 0xff00, 0xff0000, 0xffff, 0xff00ff, 0xffff00, 0xffffff};

  547. 

  548. uint8_t PS3_MOTION_colors_index = 0;

  549. 

  550. void processPS3MotionTimer() {

  551.   // See if we have a PS3_MOTION connected and we have run for a certain amount of time

  552. 

  553.   if (PS3_MOTION_timer && ((millis()-PS3_MOTION_timer) >= PS3_MOTION_PERIOD)) {

  554.     Serial.println("PS3 Motion Timer"); Serial.flush();

  555.     if (joystick1) {

  556.       PS3_MOTION_timer = millis(); // joystick not there any more...

  557. 

  558.       // We will first try to set feedback color for the PS3, maybe alternate colors

  559.       if (++PS3_MOTION_colors_index >= sizeof(PS3_MOTION_colors)/sizeof(PS3_MOTION_colors[0])) PS3_MOTION_colors_index = 0;

  560.       joystick1.setLEDs(PS3_MOTION_colors[PS3_MOTION_colors_index]);

  561. 

  562.       // Next see if we can try to pair.

  563.       if (PS3_MOTION_tried_to_pair_state == 0) {

  564.         Serial.println("PS3_MOTION Connected");

  565.         if (!last_bdaddr[0] && !last_bdaddr[1] && !last_bdaddr[2] && !last_bdaddr[3] && !last_bdaddr[4] && !last_bdaddr[5]) {

  566.           Serial.println(" - No Bluetooth adapter has been plugged in - so will not try to pair");

  567.           PS3_MOTION_tried_to_pair_state = 1;

  568.         }

  569.       } 

  570.       if ((PS3_MOTION_tried_to_pair_state < 2)  &&

  571.           (last_bdaddr[0] || last_bdaddr[1] || last_bdaddr[2] || last_bdaddr[3] || last_bdaddr[4] || last_bdaddr[5])) {

  572.         Serial.println("  - Bluetooth device detected, will try to pair");

  573.         // Lets try to pair

  574.         if (! joystick1.PS3Pair(last_bdaddr)) {

  575.           Serial.println(" - Pairing call Failed");

  576.         } else {

  577.           Serial.println(" - Pairing complete (I hope), make sure Bluetooth adapter is plugged in and try PS3 without USB");

  578.         }

  579.         PS3_MOTION_tried_to_pair_state = 2; // don't try again...

  580.       }

  581.     }  else {

  582.       Serial.println("PS3 Motion Joystick no longer detected");

  583.       PS3_MOTION_timer = 0; // joystick not there any more...

  584.     }

  585.   }

  586. }