diff --git a/EXTRAREADME b/EXTRAREADME index 619e4d5b..66293a3f 100644 --- a/EXTRAREADME +++ b/EXTRAREADME @@ -1,23 +1,20 @@ -The BTD.cpp, BTD.h, SPP.cpp, SPP.h, PS3BT.cpp, PS3BT.h, Wii.cpp, Wii.h PS3USB.cpp, PS3USB.h, XBOXUSB.cpp, and XBOXUSB.h is developed by Kristian Lauszus +BTD.cpp, BTD.h, SPP.cpp, SPP.h, PS3BT.cpp, PS3BT.h, Wii.cpp, Wii.h, PS3USB.cpp, PS3USB.h, XBOXUSB.cpp, and XBOXUSB.h are developed by Kristian Lauszus For more information regarding the PS3 protocol etc. visit my blog at: http://blog.tkjelectronics.dk/ or send me an email at kristianl at tkjelectronics dot dk. -You could also visit the official wiki: https://github.com/TKJElectronics/USB_Host_Shield_2.0/wiki for information. +You could also visit the official wiki: https://github.com/felis/USB_Host_Shield_2.0/wiki/PS3-Information for information. All three PS3 Controllers are supported (Dualshock 3-, Navigation-, and Motioncontroller). They communicate with the Arduino via Bluetooth or USB using the USB Host Shield from http://www.circuitsathome.com/ A special thanks go to the following people: "Richard Ibbotson" who made this excellent guide: http://www.circuitsathome.com/mcu/ps3-and-wiimote-game-controllers-on-the-arduino-host-shield-part-1 -- It inspired me to get starting and had a lot of good information for the USB communication - +- It inspired me to get starting and had a lot of good information for the USB communication "Tomoyuki Tanaka" for releasing his code for the Arduino USB Host shield connected to the wiimote: http://www.circuitsathome.com/mcu/rc-car-controlled-by-wii-remote-on-arduino -- It helped me a lot to see the structure of the bluetooth communication +- It helped me a lot to see the structure of the Bluetooth communication Also I would like to thank all the people behind these sites about the Motion controller: -http://thp.io/2010/psmove/ -http://www.copenhagengamecollective.org/unimove/ -https://github.com/thp/psmoveapi -http://code.google.com/p/moveonpc/ +http://thp.io/2010/psmove/, http://www.copenhagengamecollective.org/unimove/, +https://github.com/thp/psmoveapi and http://code.google.com/p/moveonpc/ All the information regarding the Xbox 360 controller protocol are form these two sites: http://tattiebogle.net/index.php/ProjectRoot/Xbox360Controller/UsbInfo @@ -26,7 +23,9 @@ http://pingus.seul.org/~grumbel/xboxdrv/ To implement the RFCOMM protocol I used a bluetooth sniffing tool called PacketLogger developed by Apple. It enables me to see the bluetooth communication between my Mac and any device. -All the information about the Wii controller is from this site: http://wiibrew.org/wiki/Wiimote -And the old Wii library created by Moyuchin: https://github.com/moyuchin/WiiRemote_on_Arduino +All the information about the Wii controllers are from these sites: http://wiibrew.org/wiki/Wiimote, +http://wiibrew.org/wiki/Wiimote/Extension_Controllers, http://wiibrew.org/wiki/Wiimote/Extension_Controllers/Nunchuck, +and http://wiibrew.org/wiki/Wiimote/Extension_Controllers/Wii_Motion_Plus +The old library created by Tomoyuki Tanaka: https://github.com/moyuchin/WiiRemote_on_Arduino also helped a lot. And at last I would like to thank Oleg from http://www.circuitsathome.com/ for making such an awesome shield! \ No newline at end of file diff --git a/Wii.cpp b/Wii.cpp index d0cffbd0..6d1c7d70 100644 --- a/Wii.cpp +++ b/Wii.cpp @@ -46,6 +46,7 @@ pBtd(p) // pointer to USB class instance - mandatory void WII::Reset() { wiimoteConnected = false; nunchuckConnected = false; + motionPlusConnected = false; l2cap_event_flag = 0; // Reset flags l2cap_state = L2CAP_WAIT; } @@ -123,8 +124,6 @@ void WII::ACLData(uint8_t* l2capinbuf) { #ifdef DEBUG Notify(PSTR("\r\nDisconnect Request: Control Channel")); #endif - wiimoteConnected = false; - nunchuckConnected = false; identifier = l2capinbuf[9]; pBtd->l2cap_disconnection_response(hci_handle,identifier,control_dcid,control_scid); Reset(); @@ -133,8 +132,6 @@ void WII::ACLData(uint8_t* l2capinbuf) { #ifdef DEBUG Notify(PSTR("\r\nDisconnect Request: Interrupt Channel")); #endif - wiimoteConnected = false; - nunchuckConnected = false; identifier = l2capinbuf[9]; pBtd->l2cap_disconnection_response(hci_handle,identifier,interrupt_dcid,interrupt_scid); Reset(); @@ -164,9 +161,17 @@ void WII::ACLData(uint8_t* l2capinbuf) { if(wiimoteConnected) { if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT if(l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) { // These reports include the buttons - if(nunchuckConnected) + if(motionPlusConnected) { + if(l2capinbuf[20] & 0x02) // Only update the wiimote buttons, since the extension bytes are from the Motion Plus + ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(ButtonState & 0xFFFF0000))); + else if (nunchuckConnected) // Update if it's a report from the Nunchuck + ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(l2capinbuf[20] & 0x0C) << 14)); + //else if(classicControllerConnected) // Update if it's a report from the Classic Controller + } + else if(nunchuckConnected) // The Nunchuck is directly connected ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8) | ((uint32_t)(l2capinbuf[20] & 0x03) << 16)); - else + //else if(classicControllerConnected) // The Classic Controller is directly connected + else if(!unknownExtensionConnected) ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8)); #ifdef PRINTREPORT Notify(PSTR("ButtonState: ")); @@ -180,114 +185,213 @@ void WII::ACLData(uint8_t* l2capinbuf) { if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x35) { // Read the accelerometer accX = ((l2capinbuf[12] << 2) | (l2capinbuf[10] & 0x60 >> 5))-500; accY = ((l2capinbuf[13] << 2) | (l2capinbuf[11] & 0x20 >> 4))-500; - 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; + accZ = ((l2capinbuf[14] << 2) | (l2capinbuf[11] & 0x40 >> 5))-500; + wiiMotePitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; + wiiMoteRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; } switch (l2capinbuf[9]) { - case 0x20: // Status Information - // (a1) 20 BB BB LF 00 00 VV + case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV if(l2capinbuf[12] & 0x02) { // Check if a extension is connected #ifdef DEBUG - Notify(PSTR("\r\nExtension connected")); + if(!unknownExtensionConnected) + Notify(PSTR("\r\nExtension connected")); #endif - setReportMode(false,0x35); // Also read the extension - activateState = 1; + unknownExtensionConnected = true; + setReportMode(false,0x35); // Also read the extension } else { #ifdef DEBUG Notify(PSTR("\r\nExtension disconnected")); #endif - nunchuckConnected = false; - setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer + if(motionPlusConnected) { +#ifdef DEBUG + Notify(PSTR(" - from Motion Plus")); +#endif + l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED; + if(!activateNunchuck) // If it's already trying to initialize the Nunchuck don't set it to false + nunchuckConnected = false; + //else if(classicControllerConnected) + } + else if(nunchuckConnected) { +#ifdef DEBUG + Notify(PSTR(" - Nunchuck")); +#endif + nunchuckConnected = false; // It must be the Nunchuck controller then + l2cap_event_flag &= ~WII_FLAG_NUNCHUCK_CONNECTED; + setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer + } else { + setReportMode(false,0x31); // If there is no extension connected we will read the button and accelerometer + } } break; case 0x21: // Read Memory Data if((l2capinbuf[12] & 0x0F) == 0) { // No error -#ifdef EXTRADEBUG - Notify(PSTR("\r\nGot report: ")); - PrintHex(l2capinbuf[13]); - PrintHex(l2capinbuf[14]); - Notify(PSTR("\r\nData: ")); - for(uint8_t i = 0; i < ((l2capinbuf[12] >> 4)+1); i++) // bit 4-7 is the length-1 - PrintHex(l2capinbuf[15+i]); -#endif - if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) { // See // http://wiibrew.org/wiki/Wiimote/Extension_Controllers + // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers + if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) { #ifdef DEBUG - Notify(PSTR("\r\nNunchuck connected")); + Notify(PSTR("\r\nNunchuck connected")); #endif + l2cap_event_flag |= WII_FLAG_NUNCHUCK_CONNECTED; + } else if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA6 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x05) { +#ifdef DEBUG + Notify(PSTR("\r\nMotion Plus connected")); +#endif + l2cap_event_flag |= WII_FLAG_MOTION_PLUS_CONNECTED; + } else if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x04 && l2capinbuf[20] == 0x05) { +#ifdef DEBUG + Notify(PSTR("\r\nMotion Plus activated in normal mode")); +#endif + motionPlusConnected = true; + } else if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x05 && l2capinbuf[20] == 0x05) { +#ifdef DEBUG + Notify(PSTR("\r\nMotion Plus activated in Nunchuck pass-through mode")); +#endif + activateNunchuck = false; + motionPlusConnected = true; nunchuckConnected = true; - ButtonState |= (Z | C); // Since the Nunchuck button are cleared when pressed we set the buttonstates like so - ButtonClickState |= (Z | C); + + ButtonState |= (Z | C); + ButtonState |= ((Z | C)<<2); + ButtonClickState = ButtonState; + OldButtonState = ButtonState; } - } #ifdef DEBUG + else { + Notify(PSTR("\r\nUnknown Device: ")); + PrintHex(l2capinbuf[13]); + PrintHex(l2capinbuf[14]); + Notify(PSTR("\r\nData: ")); + for(uint8_t i = 0; i < ((l2capinbuf[12] >> 4)+1); i++) { // bit 4-7 is the length-1 + PrintHex(l2capinbuf[15+i]); + Notify(PSTR(" ")); + } + } +#endif + } +#ifdef EXTRADEBUG else { Notify(PSTR("\r\nReport Error: ")); PrintHex(l2capinbuf[13]); PrintHex(l2capinbuf[14]); } -#endif +#endif break; case 0x22: // Acknowledge output report, return function result #ifdef DEBUG if(l2capinbuf[13] != 0x00) { // Check if there is an error Notify(PSTR("\r\nCommand failed: ")); PrintHex(l2capinbuf[12]); - } else if(l2capinbuf[12] == 0x16 && activateState > 20) - Notify(PSTR("\r\nExtension activated")); + } #endif break; - case 0x30: // Core buttons - // (a1) 30 BB BB + case 0x30: // Core buttons - (a1) 30 BB BB + case 0x31: // Core Buttons and Accelerometer - (a1) 31 BB BB AA AA AA + pitch = wiiMotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected + roll = wiiMoteRoll; 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 - 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 + case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE + 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 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 - if(activateState == 10) { - activateExtension1(); - activateState = 11; - } else if(activateState == 20) { - activateExtension2(); - activateState = 21; - } else if(activateState == 30) { - readExtensionType(); - activateState = 31; - } else if(activateState < 31) - activateState++; // We make this counter as there has to be a short delay between the commands + // (a1) 35 BB BB AA AA AA EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE EE + if(motionPlusConnected) { + if(l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension + if(motionValuesReset) { // We will only use the values when the gyro value has been set + gyroYawRaw = ((l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6))-gyroYawZero); + gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6))-gyroRollZero); + gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6))-gyroPitchZero); - hatValues[0] = l2capinbuf[15]; - hatValues[1] = l2capinbuf[16]; - - accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2))-416; - accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4))-416; - accZ = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6))-416; - /* - Notify(PSTR("\r\naccX: ")); - Serial.print(accX); - Notify(PSTR("\taccY: ")); - Serial.print(accY); - Notify(PSTR("\taccZ: ")); - Serial.print(accZ); - */ - nunchuckPitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; - nunchuckRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; + yawGyroSpeed = (double)gyroYawRaw/((double)gyroYawZero/yawGyroScale); + rollGyroSpeed = -(double)gyroRollRaw/((double)gyroRollZero/rollGyroScale); // We invert these values so they will fit the acc values + pitchGyroSpeed = (double)gyroPitchRaw/((double)gyroPitchZero/pitchGyroScale); + + /* The onboard gyro has two ranges for slow and fast mode */ + if(!(l2capinbuf[18] & 0x02)) // Check if fast more is used + yawGyroSpeed *= 4.545; + if(!(l2capinbuf[18] & 0x01)) // Check if fast more is used + pitchGyroSpeed *= 4.545; + if(!(l2capinbuf[19] & 0x02)) // Check if fast more is used + rollGyroSpeed *= 4.545; + + pitch = (0.93*(pitch+(pitchGyroSpeed*(double)(micros()-timer)/1000000)))+(0.07*wiiMotePitch); // Use a complimentary filter to calculate the angle + roll = (0.93*(roll+(rollGyroSpeed*(double)(micros()-timer)/1000000)))+(0.07*wiiMoteRoll); + + gyroYaw += (yawGyroSpeed*((double)(micros()-timer)/1000000)); + gyroRoll += (rollGyroSpeed*((double)(micros()-timer)/1000000)); + gyroPitch += (pitchGyroSpeed*((double)(micros()-timer)/1000000)); + timer = micros(); + /* + // Uncomment these lines to tune the gyro scale variabels + Serial.print("\r\ngyroYaw: "); + Serial.print(gyroYaw); + Serial.print("\tgyroRoll: "); + Serial.print(gyroRoll); + Serial.print("\tgyroPitch: "); + Serial.print(gyroPitch); + */ + /* + Serial.print("\twiiMoteRoll: "); + Serial.print(wiiMoteRoll); + Serial.print("\twiiMotePitch: "); + Serial.print(wiiMotePitch); + */ + } else { + if((micros() - timer) > 1000000) { // Loop for 1 sec before resetting the values +#ifdef DEBUG + Notify(PSTR("\r\nThe gyro values has been reset")); +#endif + gyroYawZero = (l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6)); + gyroRollZero = (l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6)); + gyroPitchZero = (l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6)); + + rollGyroScale = 500; // You might need to adjust these + pitchGyroScale = 400; + yawGyroScale = 415; + + motionValuesReset = true; + timer = micros(); + } + } + } else { + if(nunchuckConnected) { + hatValues[0] = l2capinbuf[15]; + hatValues[1] = l2capinbuf[16]; + accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x10 >> 3))-416; + accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x20 >> 4))-416; + accZ = (((l2capinbuf[19] & 0xFE) << 2) | (l2capinbuf[20] & 0xC0 >> 5))-416; + nunchuckPitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; + nunchuckRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; + } + //else if(classicControllerConnected) { } + } + if(l2capinbuf[19] & 0x01) { + if(!extensionConnected) { + extensionConnected = true; + unknownExtensionConnected = true; + Serial.print("\r\nExtension connected to Motion Plus"); + } + } + else { + if(extensionConnected && !unknownExtensionConnected) { + extensionConnected = false; + unknownExtensionConnected = true; + Serial.print("\r\nExtension disconnected from Motion Plus"); + nunchuckConnected = false; // There is no extension connected to the Motion Plus if this report is sent + } + } + + } else if(nunchuckConnected) { + hatValues[0] = l2capinbuf[15]; + hatValues[1] = l2capinbuf[16]; + accX = ((l2capinbuf[17] << 2) | (l2capinbuf[20] & 0x0C >> 2))-416; + accY = ((l2capinbuf[18] << 2) | (l2capinbuf[20] & 0x30 >> 4))-416; + accZ = ((l2capinbuf[19] << 2) | (l2capinbuf[20] & 0xC0 >> 6))-416; + nunchuckPitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG; + nunchuckRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG; + + pitch = wiiMotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus connected + roll = wiiMoteRoll; + } break; #ifdef DEBUG default: @@ -343,27 +447,14 @@ void WII::L2CAP_task() { #ifdef DEBUG Notify(PSTR("\r\nHID Channels Established")); #endif - statusRequest(); - l2cap_state = L2CAP_WII_STATUS_STATE; + pBtd->connectToWii = false; + wiimoteConnected = true; + stateCounter = 0; + l2cap_state = L2CAP_CHECK_MOTION_PLUS_STATE; } - break; + break; - case L2CAP_WII_STATUS_STATE: - wiimoteConnected = true; - pBtd->connectToWii = false; - ButtonState = 0; - OldButtonState = 0; - ButtonClickState = 0; - setLedOn(LED1); - l2cap_state = L2CAP_DONE; - break; -/* - case L2CAP_WIIREMOTE_CAL_STATE: - //Todo enable support for Motion Plus - break; -*/ - case L2CAP_DONE: - break; + /* The next states are in run() */ case L2CAP_INTERRUPT_DISCONNECT: if (l2cap_disconnect_response_interrupt_flag) { @@ -401,6 +492,143 @@ void WII::Run() { pBtd->l2cap_connection_request(hci_handle,identifier,control_dcid,HID_CTRL_PSM); l2cap_state = L2CAP_CONTROL_CONNECT_REQUEST; } + break; + + + + case L2CAP_CHECK_MOTION_PLUS_STATE: +#ifdef DEBUG + if(stateCounter == 0) // Only print onnce + Notify(PSTR("\r\nChecking if a Motion Plus is connected")); +#endif + stateCounter++; + if(stateCounter%100 == 0) + checkMotionPresent(); // Check if there is a motion plus connected + if(motion_plus_connected_flag) { + stateCounter = 0; + l2cap_state = L2CAP_INIT_MOTION_PLUS_STATE; + timer = micros(); + + if(unknownExtensionConnected) { +#ifdef DEBUG + Notify(PSTR("\r\nA extension is also connected")); +#endif + activateNunchuck = true; // For we will just set this to true as this the only extension supported so far + } + + } + else if(stateCounter == 301) { // We will try three times to check for the motion plus +#ifdef DEBUG + Notify(PSTR("\r\nNo Motion Plus was detected")); +#endif + stateCounter = 0; + l2cap_state = L2CAP_CHECK_EXTENSION_STATE; + } + break; + + case L2CAP_CHECK_EXTENSION_STATE: // This is used to check if there is anything plugged in to the extension port +#ifdef DEBUG + if(stateCounter == 0) // Only print onnce + Notify(PSTR("\r\nChecking if there is any extension connected")); +#endif + stateCounter++; // We use this counter as there has to be a short delay between the commands + if(stateCounter == 1) + statusRequest(); // See if a new device has connected + if(stateCounter == 100) { + if(unknownExtensionConnected) // Check if there is a extension is connected to the port + initExtension1(); + else + stateCounter = 399; + } else if(stateCounter == 200) + initExtension2(); + else if(stateCounter == 300) { + readExtensionType(); + unknownExtensionConnected = false; + } else if(stateCounter == 400) { + stateCounter = 0; + l2cap_state = L2CAP_LED_STATE; + } + break; + + case L2CAP_INIT_MOTION_PLUS_STATE: + stateCounter++; + if(stateCounter == 1) + initMotionPlus(); + else if(stateCounter == 100) + activateMotionPlus(); + else if(stateCounter == 200) + readExtensionType(); // Check if it has been activated + else if(stateCounter == 300) { + stateCounter = 0; + unknownExtensionConnected = false; // The motion plus will send a status report when it's activated, we will set this to false so it doesn't reinitialize the Motion Plus + l2cap_state = L2CAP_LED_STATE; + } + break; + + case L2CAP_LED_STATE: + if(nunchuck_connected_flag) { + nunchuckConnected = true; + ButtonState |= (Z | C); // Since the Nunchuck button are cleared when pressed we set the buttonstates like so + ButtonState |= ((Z | C)<<2); // And like this when it's connected since the bytes are shifter two to the left + ButtonClickState = ButtonState; + OldButtonState = ButtonState; + } + setLedOn(LED1); + l2cap_state = L2CAP_DONE; + break; + + case L2CAP_DONE: + if(unknownExtensionConnected) { +#ifdef DEBUG + if(stateCounter == 0) // Only print once + Notify(PSTR("\r\nChecking extension port")); +#endif + stateCounter++; // We will use this counter as there has to be a short delay between the commands + if(stateCounter == 50) + statusRequest(); + else if(stateCounter == 100) + initExtension1(); + else if(stateCounter == 150) + if((extensionConnected && motionPlusConnected) || (unknownExtensionConnected && !motionPlusConnected)) + initExtension2(); + else + stateCounter = 299; // There is no extension connected + else if(stateCounter == 200) + readExtensionType(); + else if(stateCounter == 250) { + if(nunchuck_connected_flag) { +#ifdef DEBUG + Notify(PSTR("\r\nNunchuck was reconnected")); +#endif + activateNunchuck = true; + nunchuckConnected = true; + ButtonState |= (Z | C); // Since the Nunchuck button are cleared when pressed we set the buttonstates like so + ButtonState |= ((Z | C)<<2); // And like this when it's connected since the bytes are shifter two to the left + ButtonClickState = ButtonState; + OldButtonState = ButtonState; + } + if(!motionPlusConnected) + stateCounter = 449; + } + else if (stateCounter == 300) { + if(motionPlusConnected) { +#ifdef DEBUG + Notify(PSTR("\r\nReactivating the Motion Plus")); +#endif + initMotionPlus(); + } else + stateCounter = 449; + } + else if(stateCounter == 350) + activateMotionPlus(); + else if(stateCounter == 400) + readExtensionType(); // Check if it has been activated + else if(stateCounter == 450) { + stateCounter = 0; + unknownExtensionConnected = false; + } + } else + stateCounter = 0; break; } } @@ -488,16 +716,39 @@ void WII::writeData(uint32_t offset, uint8_t size, uint8_t* data) { cmd_buf[7+i] = 0x00; HID_Command(cmd_buf,23); } -void WII::activateExtension1() { +void WII::initExtension1() { uint8_t buf[1]; buf[0] = 0x55; writeData(0xA400F0,1,buf); } -void WII::activateExtension2() { +void WII::initExtension2() { uint8_t buf[1]; buf[0] = 0x00; writeData(0xA400FB,1,buf); } +void WII::initMotionPlus() { + uint8_t buf[1]; + buf[0] = 0x55; + writeData(0xA600F0,1,buf); +} +void WII::activateMotionPlus() { + uint8_t buf[1]; + if(activateNunchuck) { +#ifdef DEBUG + Notify(PSTR("\r\nActivating Motion Plus in pass-through mode")); +#endif + buf[0] = 0x05; // Activate nunchuck pass-through mode + } + //else if(classicControllerConnected && extensionConnected) + //buf[0] = 0x07; + else { +#ifdef DEBUG + Notify(PSTR("\r\nActivating Motion Plus in normal mode")); +#endif + buf[0] = 0x04; // Don't use any extension + } + writeData(0xA600FE,1,buf); +} void WII::readData(uint32_t offset, uint16_t size, bool EEPROM) { uint8_t cmd_buf[8]; cmd_buf[0] = 0xA2; // HID BT DATA_request (0x50) | Report Type (Output 0x02) @@ -520,13 +771,16 @@ void WII::readExtensionType() { void WII::readCalData() { readData(0x0016,8,true); } +void WII::checkMotionPresent() { + readData(0xA600FA,6,false); +} /************************************************************/ /* WII Commands */ /************************************************************/ bool WII::getButtonPress(Button b) { // Return true when a button is pressed - bool press = (ButtonState & (uint32_t)b); + bool press = (ButtonState & (uint32_t)b); if(b == Z || b == C) return !press; // The nunchuck buttons are cleared when pressed else @@ -543,10 +797,10 @@ bool WII::getButtonClick(Button b) { // Only return true when a button is clicke } uint8_t WII::getAnalogHat(AnalogHat a) { if(!nunchuckConnected) - return 127; // Center position + return 127; // Return center position else { uint8_t output = hatValues[(uint8_t)a]; - if(output == 0xFF) // The joystick will only read 255 when the cable is unplugged, so we will just return the center position + if(output == 0xFF || output == 0x00) // The joystick will only read 255 or 0 when the cable is unplugged or initializing, so we will just return the center position return 127; else return output; diff --git a/Wii.h b/Wii.h index 4e479638..048143cf 100644 --- a/Wii.h +++ b/Wii.h @@ -26,21 +26,25 @@ #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 6 -#define L2CAP_INTERRUPT_DISCONNECT 7 -#define L2CAP_CONTROL_DISCONNECT 8 + +#define L2CAP_CHECK_MOTION_PLUS_STATE 5 +#define L2CAP_CHECK_EXTENSION_STATE 6 +#define L2CAP_INIT_MOTION_PLUS_STATE 7 + +#define L2CAP_LED_STATE 8 +#define L2CAP_DONE 9 +#define L2CAP_INTERRUPT_DISCONNECT 10 +#define L2CAP_CONTROL_DISCONNECT 11 /* L2CAP event flags */ -#define L2CAP_FLAG_CONTROL_CONNECTED 0x01 -#define L2CAP_FLAG_INTERRUPT_CONNECTED 0x02 -#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS 0x04 -#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS 0x08 -#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE 0x40 -#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE 0x80 +#define L2CAP_FLAG_CONTROL_CONNECTED 0x001 +#define L2CAP_FLAG_INTERRUPT_CONNECTED 0x002 +#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS 0x004 +#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS 0x008 +#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE 0x040 +#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE 0x080 -/*Macros for L2CAP event flag tests */ +/* Macros for L2CAP event flag tests */ #define l2cap_connected_control_flag (l2cap_event_flag & L2CAP_FLAG_CONTROL_CONNECTED) #define l2cap_connected_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_INTERRUPT_CONNECTED) #define l2cap_config_success_control_flag (l2cap_event_flag & L2CAP_FLAG_CONFIG_CONTROL_SUCCESS) @@ -48,6 +52,13 @@ #define l2cap_disconnect_response_control_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE) #define l2cap_disconnect_response_interrupt_flag (l2cap_event_flag & L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE) +/* Wii event flags */ +#define WII_FLAG_MOTION_PLUS_CONNECTED 0x100 +#define WII_FLAG_NUNCHUCK_CONNECTED 0x200 + +#define motion_plus_connected_flag (l2cap_event_flag & WII_FLAG_MOTION_PLUS_CONNECTED) +#define nunchuck_connected_flag (l2cap_event_flag & WII_FLAG_NUNCHUCK_CONNECTED) + enum LED { LED1 = 0x10, LED2 = 0x20, @@ -97,15 +108,10 @@ public: bool getButtonPress(Button b); // This will read true as long as the button is held down bool getButtonClick(Button b); // This will only be true when the button is clicked the first time uint8_t getAnalogHat(AnalogHat a); // Used to read the joystick of the Nunchuck -/* - TODO: Enable support for Motion Plus - int16_t getSensor(Sensor a); - double getAngle(Angle a); -*/ - double getPitch() { return pitch; }; - double getRoll() { return roll; }; - double getNunchuckPitch() { return nunchuckPitch; }; - double getNunchuckRoll() { return nunchuckRoll; }; + + double getPitch() { return pitch; }; // Fusioned angle using a complimentary filter if the Motion Plus is connected + double getRoll() { return roll; }; // Fusioned angle using a complimentary filter if the Motion Plus is connected + double getYaw() { return gyroYaw; }; // This is the yaw calculated by the gyro void setAllOff(); // Turn both rumble and all LEDs off void setRumbleOff(); @@ -117,6 +123,39 @@ public: bool wiimoteConnected; // Variable used to indicate if a Wiimote is connected bool nunchuckConnected; // Variable used to indicate if a Nunchuck controller is connected + bool motionPlusConnected; // Variable used to indicate if a Nunchuck controller is connected + + /* IMU Data, might be usefull if you need to do something more advanced than just calculating the angle */ + + double wiiMotePitch; // Pitch and roll calculated from the accelerometer inside the Wiimote + double wiiMoteRoll; + double nunchuckPitch; // Pitch and roll calculated from the accelerometer inside the Nunchuck + double nunchuckRoll; + + int16_t accX; // Accelerometer values used to calculate pitch and roll + int16_t accY; + int16_t accZ; + + /* Variables for the gyro inside the Motion Plus */ + double gyroPitch; // This is the pitch calculated by the gyro - use this to tune pitchGyroScale + double gyroRoll; // This is the roll calculated by the gyro - use this to tune rollGyroScale + double gyroYaw; // This is the yaw calculated by the gyro - use this to tune yawGyroScale + + double pitchGyroSpeed; // The speed in deg/s from the gyro + double rollGyroSpeed; + double yawGyroSpeed; + + uint16_t pitchGyroScale; // You might need to fine-tune these values + uint16_t rollGyroScale; + uint16_t yawGyroScale; + + int16_t gyroYawRaw; // Raw value read directly from the Motion Plus + int16_t gyroRollRaw; + int16_t gyroPitchRaw; + + int16_t gyroYawZero; // These values are set when the controller is first initialized + int16_t gyroRollZero; + int16_t gyroPitchZero; private: /* Mandatory members */ @@ -137,13 +176,18 @@ private: uint8_t hatValues[2]; uint8_t HIDBuffer[3];// Used to store HID commands + uint8_t rumbleBit; + + uint16_t stateCounter; + bool unknownExtensionConnected; + bool extensionConnected; /* L2CAP Channels */ uint8_t control_scid[2]; // L2CAP source CID for HID_Control - uint8_t control_dcid[2]; //0x0060 + uint8_t control_dcid[2]; // 0x0060 uint8_t interrupt_scid[2]; // L2CAP source CID for HID_Interrupt - uint8_t interrupt_dcid[2]; //0x0061 - uint8_t identifier; //Identifier for connection + uint8_t interrupt_dcid[2]; // 0x0061 + uint8_t identifier; // Identifier for connection /* HID Commands */ void HID_Command(uint8_t* data, uint8_t nbytes); @@ -151,23 +195,22 @@ private: void statusRequest(); void writeData(uint32_t offset, uint8_t size, uint8_t* data); - void activateExtension1(); - void activateExtension2(); + void initExtension1(); + void initExtension2(); void readData(uint32_t offset, uint16_t size, bool EEPROM); void readExtensionType(); void readCalData(); - uint8_t activateState; - uint8_t rumbleBit; + void checkMotionPresent(); // Used to see if a Motion Plus is connected to the Wiimote + void initMotionPlus(); + void activateMotionPlus(); + + double pitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected + double roll; // Fusioned angle using a complimentary filter if the Motion Plus is connected - double pitch; - double roll; - double nunchuckPitch; - double nunchuckRoll; - - int16_t accX; // Accelerometer values used to calculate pitch and roll - int16_t accY; - int16_t accZ; + bool activateNunchuck; + bool motionValuesReset; // This bool is true when the gyro values has been reset + unsigned long timer; }; #endif \ No newline at end of file diff --git a/examples/Bluetooth/Wii/Wii.ino b/examples/Bluetooth/Wii/Wii.ino index bbf39d77..2fdd15b7 100644 --- a/examples/Bluetooth/Wii/Wii.ino +++ b/examples/Bluetooth/Wii/Wii.ino @@ -50,19 +50,15 @@ void loop() { Serial.print(F("\r\nUp")); } - if(Wii.getButtonClick(PLUS)) { + if(Wii.getButtonClick(PLUS)) Serial.print(F("\r\nPlus")); - } - if(Wii.getButtonClick(MINUS)) { + if(Wii.getButtonClick(MINUS)) Serial.print(F("\r\nMinus")); - } - if(Wii.getButtonClick(ONE)) { + if(Wii.getButtonClick(ONE)) Serial.print(F("\r\nOne")); - } - if(Wii.getButtonClick(TWO)) { + if(Wii.getButtonClick(TWO)) Serial.print(F("\r\nTwo")); - } if(Wii.getButtonClick(A)) { printAngle = !printAngle; @@ -78,11 +74,15 @@ void loop() { Serial.print(Wii.getPitch()); Serial.print(F("\tRoll: ")); Serial.print(Wii.getRoll()); + if(Wii.motionPlusConnected) { + Serial.print(F("\tYaw: ")); + Serial.print(Wii.getYaw()); + } if(Wii.nunchuckConnected) { Serial.print(F("\tNunchuck Pitch: ")); - Serial.print(Wii.getNunchuckPitch()); + Serial.print(Wii.nunchuckPitch); Serial.print(F("\tNunchuck Roll: ")); - Serial.print(Wii.getNunchuckRoll()); + Serial.print(Wii.nunchuckRoll); } } }