From 7eb2f39597249e036736ee23ec1f7df58518b416 Mon Sep 17 00:00:00 2001 From: Kristian Lauszus Date: Tue, 21 Aug 2012 17:36:37 +0200 Subject: [PATCH] Added getPitch and getRoll --- BTD.cpp | 2 +- Wii.cpp | 155 +++++++++++++++++++------ Wii.h | 17 ++- examples/Bluetooth/Wiimote/Wiimote.ino | 9 ++ keywords.txt | 4 +- 5 files changed, 146 insertions(+), 41 deletions(-) diff --git a/BTD.cpp b/BTD.cpp index 1ed3e84c..23c9988b 100644 --- a/BTD.cpp +++ b/BTD.cpp @@ -554,7 +554,7 @@ void BTD::HCI_task() { if(wiiServiceID != -1) { // Check if it should try to connect to a wiimote if(disc_bdaddr[5] == 0 && disc_bdaddr[4] == 0 && disc_bdaddr[3] == 0 && disc_bdaddr[2] == 0 && disc_bdaddr[1] == 0 && disc_bdaddr[0] == 0) { #ifdef DEBUG - Notify(PSTR("\r\nStarting inquiry\r\nPress A & B on the Wiimote")); + Notify(PSTR("\r\nStarting inquiry\r\nPress 1 & 2 on the Wiimote")); #endif hci_inquiry(); hci_state = HCI_INQUIRY_STATE; diff --git a/Wii.cpp b/Wii.cpp index 9293ba58..d61c8b40 100644 --- a/Wii.cpp +++ b/Wii.cpp @@ -161,33 +161,95 @@ void WII::ACLData(uint8_t* l2capinbuf) { if(connected) { /* Read Report */ if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT - switch (l2capinbuf[9]) { - case 0x30: // Core buttons - ButtonState = (uint16_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8)); - ButtonClickState = ButtonState; // Update click state variable - + if(l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) { // These reports include the buttons + ButtonState = (uint16_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8)); + ButtonClickState = ButtonState; // Update click state variable #ifdef PRINTREPORT - Notify(PSTR("ButtonState: ")); - PrintHex(ButtonState); - Notify(PSTR("\r\n")); + Notify(PSTR("ButtonState: ")); + PrintHex(ButtonState); + Notify(PSTR("\r\n")); #endif - - if(ButtonState != OldButtonState) { - buttonChanged = true; - if(ButtonState != 0x0000) { - buttonPressed = true; - buttonReleased = false; - } else { - buttonPressed = false; - buttonReleased = true; - } - } - else { - buttonChanged = false; - buttonPressed = false; + if(ButtonState != OldButtonState) { + buttonChanged = true; + if(ButtonState != 0x0000) { + buttonPressed = true; buttonReleased = false; - } - OldButtonState = ButtonState; + } else { + buttonPressed = false; + buttonReleased = true; + } + } + else { + buttonChanged = false; + buttonPressed = false; + buttonReleased = false; + } + OldButtonState = ButtonState; + } + if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x35) { // Read the accelerometer + int16_t accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5))-500; + int16_t accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4))-500; + int16_t accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5))-500; + /* + Notify(PSTR("\r\naccX: ")); + Serial.print(accX); + Notify(PSTR("\taccY: ")); + Serial.print(accY); + Notify(PSTR("\taccZ: ")); + Serial.print(accZ); + */ + pitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; + roll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; + /* + Notify(PSTR("\r\nPitch: ")); + Serial.print(pitch); + Notify(PSTR("\tRoll: ")); + Serial.print(roll); + */ + } + switch (l2capinbuf[9]) { + case 0x20: // Status Information + // (a1) 20 BB BB LF 00 00 VV + if(l2capinbuf[12] & 0x02) // Check if a extension is connected + setReportMode(false,0x35); // Also read the extension + else + setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer + break; + case 0x30: // Core buttons + // (a1) 30 BB BB + break; + case 0x31: // Core Buttons and Accelerometer + // (a1) 31 BB BB AA AA AA + break; + case 0x32: // Core Buttons with 8 Extension bytes + // (a1) 32 BB BB EE EE EE EE EE EE EE EE + /* + Notify(PSTR("\r\n")); + for (uint8_t i = 0; i < 8; i++) { + Serial.print(l2capinbuf[12+i]); + Notify(PSTR(" ")); + } + */ + break; + case 0x34: // Core Buttons with 19 Extension bytes + // (a1) 34 BB BB EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE + /* + Notify(PSTR("\r\n")); + for (uint8_t i = 0; i < 19; i++) { + Serial.print(l2capinbuf[12+i]); + Notify(PSTR(" ")); + } + */ + break; + case 0x35: // Core Buttons and Accelerometer with 16 Extension Bytes + // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE + /* + Notify(PSTR("\r\n")); + for (uint8_t i = 0; i < 16; i++) { + Serial.print(l2capinbuf[15+i]); + Notify(PSTR(" ")); + } + */ break; #ifdef DEBUG default: @@ -243,17 +305,22 @@ void WII::L2CAP_task() { #ifdef DEBUG Notify(PSTR("\r\nHID Channels Established")); #endif - connected = true; - pBtd->connectToWii = false; - ButtonState = 0; - OldButtonState = 0; - ButtonClickState = 0; - setLedOn(LED1); - l2cap_state = L2CAP_DONE; + statusRequest(); + l2cap_state = L2CAP_WII_STATUS_STATE; } break; + + case L2CAP_WII_STATUS_STATE: + connected = true; + pBtd->connectToWii = false; + ButtonState = 0; + OldButtonState = 0; + ButtonClickState = 0; + setLedOn(LED1); + l2cap_state = L2CAP_DONE; + break; /* - case L2CAP_WIIREMOTE_CAL_STATE: + case L2CAP_WIIREMOTE_CAL_STATE: //Todo enable support for Motion Plus break; */ @@ -337,9 +404,27 @@ void WII::setLedOn(LED a) { HID_Command(HIDBuffer, 3); } void WII::setLedToggle(LED a) { - HIDBuffer[1] = 0x11; - HIDBuffer[2] ^= (uint8_t)a; - HID_Command(HIDBuffer, 3); + HIDBuffer[1] = 0x11; + HIDBuffer[2] ^= (uint8_t)a; + HID_Command(HIDBuffer, 3); +} +void WII::setReportMode(bool continuous, uint8_t mode) { + uint8_t cmd_buf[4]; + cmd_buf[0] = 0xA2; // HID BT DATA_request (0x50) | Report Type (Output 0x02) + cmd_buf[1] = 0x12; + if(continuous) + cmd_buf[2] = 0x04; + else + cmd_buf[2] = 0x00; + cmd_buf[3] = mode; + HID_Command(cmd_buf, 4); +} +void WII::statusRequest() { + uint8_t cmd_buf[3]; + cmd_buf[0] = 0xA2; // HID BT DATA_request (0x50) | Report Type (Output 0x02) + cmd_buf[1] = 0x15; + cmd_buf[2] = 0x00; + HID_Command(cmd_buf, 3); } /************************************************************/ diff --git a/Wii.h b/Wii.h index 6298da9f..18ba3dc5 100644 --- a/Wii.h +++ b/Wii.h @@ -26,10 +26,11 @@ #define L2CAP_CONTROL_CONFIG_REQUEST 2 #define L2CAP_INTERRUPT_CONNECT_REQUEST 3 #define L2CAP_INTERRUPT_CONFIG_REQUEST 4 +#define L2CAP_WII_STATUS_STATE 5 //#define L2CAP_WIIREMOTE_CAL_STATE 9 /* TODO: Enable support for Motion Plus */ -#define L2CAP_DONE 5 -#define L2CAP_INTERRUPT_DISCONNECT 6 -#define L2CAP_CONTROL_DISCONNECT 7 +#define L2CAP_DONE 6 +#define L2CAP_INTERRUPT_DISCONNECT 7 +#define L2CAP_CONTROL_DISCONNECT 8 /* L2CAP event flags */ #define L2CAP_FLAG_CONTROL_CONNECTED 0x01 @@ -93,13 +94,18 @@ public: int16_t getSensor(Sensor a); double getAngle(Angle a); */ + double getPitch() { return pitch; }; + double getRoll() { return roll; }; + void setAllOff(); // Turn both rumble and all LEDs off void setRumbleOff(); void setRumbleOn(); void setRumbleToggle(); void setLedOff(LED a); void setLedOn(LED a); - void setLedToggle(LED a); + void setLedToggle(LED a); + void setReportMode(bool continuous, uint8_t mode); + void statusRequest(); bool connected;// Variable used to indicate if a Wiimote is connected bool buttonChanged;//Indicate if a button has been changed @@ -134,5 +140,8 @@ private: /* HID Commands */ void HID_Command(uint8_t* data, uint8_t nbytes); + + double pitch; + double roll; }; #endif \ No newline at end of file diff --git a/examples/Bluetooth/Wiimote/Wiimote.ino b/examples/Bluetooth/Wiimote/Wiimote.ino index 065ab2b1..3d154dc0 100644 --- a/examples/Bluetooth/Wiimote/Wiimote.ino +++ b/examples/Bluetooth/Wiimote/Wiimote.ino @@ -11,6 +11,8 @@ BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so WII Wii(&Btd); // This will start inquiry which will connect to any Wiimote //WII Wii(&Btd,0x00,0x26,0x59,0x48,0xFF,0xFB); // This will connect to the Wiimote with that specific Bluetooth Address +bool printAngle; + void setup() { Serial.begin(115200); if (Usb.Init() == -1) { @@ -64,6 +66,7 @@ void loop() { } if(Wii.getButtonClick(A)) { + printAngle = !printAngle; Serial.print(F("\r\nA")); } if(Wii.getButtonClick(B)) { @@ -72,6 +75,12 @@ void loop() { } } } + if(printAngle) { + Serial.print(F("\r\nPitch: ")); + Serial.print(Wii.getPitch()); + Serial.print(F("\tRoll: ")); + Serial.print(Wii.getRoll()); + } } } diff --git a/keywords.txt b/keywords.txt index 57fe5b46..33313303 100644 --- a/keywords.txt +++ b/keywords.txt @@ -219,4 +219,6 @@ WII KEYWORD1 getButtonPress KEYWORD2 getButtonClick KEYWORD2 -setRumbleToggle KEYWORD2 \ No newline at end of file +setRumbleToggle KEYWORD2 +getPitch KEYWORD2 +getRoll KEYWORD2 \ No newline at end of file