Added support for the Motion Plus extension

EXTRAREADME

@@ -1,7 +1,7 @@
-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/
@@ -9,15 +9,12 @@ They communicate with the Arduino via Bluetooth or USB using the USB Host Shield
 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
- 
 "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://thp.io/2010/psmove/, http://www.copenhagengamecollective.org/unimove/,
-http://www.copenhagengamecollective.org/unimove/    
+https://github.com/thp/psmoveapi and http://code.google.com/p/moveonpc/
-https://github.com/thp/psmoveapi
-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
+All the information about the Wii controllers are from these sites: http://wiibrew.org/wiki/Wiimote, 
-And the old Wii library created by Moyuchin: https://github.com/moyuchin/WiiRemote_on_Arduino
+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!

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: "));
@@ -181,55 +186,89 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                         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;                        
-                        /*
+                        wiiMotePitch = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
-                        Notify(PSTR("\r\naccX: "));
+                        wiiMoteRoll = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
-                        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;
                     }
                     switch (l2capinbuf[9]) {
-                        case 0x20: // Status Information
+                        case 0x20: // Status Information - (a1) 20 BB BB LF 00 00 VV                            
-                            // (a1) 20 BB BB LF 00 00 VV
                             if(l2capinbuf[12] & 0x02) { // Check if a extension is connected
 #ifdef DEBUG
+                                if(!unknownExtensionConnected)
                                     Notify(PSTR("\r\nExtension connected"));
 #endif
+                                unknownExtensionConnected = true;
                                 setReportMode(false,0x35); // Also read the extension
-                                activateState = 1;
                             }
                             else {
 #ifdef DEBUG
                                 Notify(PSTR("\r\nExtension disconnected"));
 #endif
+                                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
+                                // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers
-                                Notify(PSTR("\r\nGot report: "));
+                                if(l2capinbuf[15] == 0x00 && l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) { 
-                                PrintHex<uint8_t>(l2capinbuf[13]);
-                                PrintHex<uint8_t>(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<uint8_t>(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
 #ifdef DEBUG
                                     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<uint8_t>(l2capinbuf[13]);
+                                    PrintHex<uint8_t>(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<uint8_t>(l2capinbuf[15+i]);
+                                        Notify(PSTR(" "));
+                                    }
+                                }
+#endif
+                            }
+#ifdef EXTRADEBUG
                             else {
                                 Notify(PSTR("\r\nReport Error: "));
                                 PrintHex<uint8_t>(l2capinbuf[13]);
@@ -242,52 +281,117 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                             if(l2capinbuf[13] != 0x00) { // Check if there is an error
                                 Notify(PSTR("\r\nCommand failed: "));
                                 PrintHex<uint8_t>(l2capinbuf[12]);
-                            } else if(l2capinbuf[12] == 0x16 && activateState > 20)
+                            }
-                                Notify(PSTR("\r\nExtension activated"));
 #endif                                      
                             break;
-                        case 0x30: // Core buttons
+                        case 0x30: // Core buttons - (a1) 30 BB BB
-                            // (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
+                        case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE
-                            // (a1) 31 BB BB AA AA AA
+                        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 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
                             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) {
+                            if(motionPlusConnected) {
-                                activateExtension1();
+                                if(l2capinbuf[20] & 0x02) { // Check if it's a report from the Motion controller or the extension
-                                activateState = 11;
+                                    if(motionValuesReset) { // We will only use the values when the gyro value has been set
-                            } else if(activateState == 20) {
+                                        gyroYawRaw = ((l2capinbuf[15] | ((l2capinbuf[18] & 0xFC) << 6))-gyroYawZero);
-                                activateExtension2();
+                                        gyroRollRaw = ((l2capinbuf[16] | ((l2capinbuf[19] & 0xFC) << 6))-gyroRollZero);
-                                activateState = 21;                                
+                                        gyroPitchRaw = ((l2capinbuf[17] | ((l2capinbuf[20] & 0xFC) << 6))-gyroPitchZero);
-                            } 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
 
+                                        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;
-                            /*
-                            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;
+                                
+                                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();
+                pBtd->connectToWii = false;
-                l2cap_state = L2CAP_WII_STATUS_STATE;
+                wiimoteConnected = true;
+                stateCounter = 0;
+                l2cap_state = L2CAP_CHECK_MOTION_PLUS_STATE;
             }
             break;        
             
-        case L2CAP_WII_STATUS_STATE:                        
+        /* The next states are in run() */            
-            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;
             
         case L2CAP_INTERRUPT_DISCONNECT:
             if (l2cap_disconnect_response_interrupt_flag) {
@@ -402,6 +493,143 @@ void WII::Run() {
                 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,6 +771,9 @@ void WII::readExtensionType() {
 void WII::readCalData() {
     readData(0x0016,8,true);
 }
+void WII::checkMotionPresent() {
+    readData(0xA600FA,6,false);
+}
 
 /************************************************************/
 /*                    WII Commands                          */
@@ -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;

Wii.h

@@ -26,19 +26,23 @@
 #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_CHECK_MOTION_PLUS_STATE   5
-#define L2CAP_DONE                      6
+#define L2CAP_CHECK_EXTENSION_STATE     6
-#define L2CAP_INTERRUPT_DISCONNECT      7
+#define L2CAP_INIT_MOTION_PLUS_STATE    7
-#define L2CAP_CONTROL_DISCONNECT        8
+
+#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_CONTROL_CONNECTED                0x001
-#define L2CAP_FLAG_INTERRUPT_CONNECTED              0x02
+#define L2CAP_FLAG_INTERRUPT_CONNECTED              0x002
-#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS           0x04
+#define L2CAP_FLAG_CONFIG_CONTROL_SUCCESS           0x004
-#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS         0x08
+#define L2CAP_FLAG_CONFIG_INTERRUPT_SUCCESS         0x008
-#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE      0x40
+#define L2CAP_FLAG_DISCONNECT_CONTROL_RESPONSE      0x040
-#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE    0x80
+#define L2CAP_FLAG_DISCONNECT_INTERRUPT_RESPONSE    0x080
 
 /* Macros for L2CAP event flag tests */
 #define l2cap_connected_control_flag (l2cap_event_flag & L2CAP_FLAG_CONTROL_CONNECTED)
@@ -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
+    double getPitch() { return pitch; }; // Fusioned angle using a complimentary filter if the Motion Plus is connected
-    int16_t getSensor(Sensor a);
+    double getRoll() { return roll; }; // Fusioned angle using a complimentary filter if the Motion Plus is connected
-    double getAngle(Angle a);
+    double getYaw() { return gyroYaw; }; // This is the yaw calculated by the gyro
-*/
-    double getPitch() { return pitch; };
-    double getRoll() { return roll; };
-    double getNunchuckPitch() { return nunchuckPitch; };
-    double getNunchuckRoll() { return nunchuckRoll; };
     
     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,6 +176,11 @@ 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
@@ -151,23 +195,22 @@ private:
     void statusRequest();
     
     void writeData(uint32_t offset, uint8_t size, uint8_t* data);
-    void activateExtension1();
+    void initExtension1();
-    void activateExtension2();
+    void initExtension2();
     
     void readData(uint32_t offset, uint16_t size, bool EEPROM);
     void readExtensionType();
     void readCalData();
     
-    uint8_t activateState;
+    void checkMotionPresent(); // Used to see if a Motion Plus is connected to the Wiimote
-    uint8_t rumbleBit;
+    void initMotionPlus();
+    void activateMotionPlus();
 
-    double pitch;
+    double pitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected
-    double roll;
+    double roll; // Fusioned angle using a complimentary filter if the Motion Plus is connected
-    double nunchuckPitch;
-    double nunchuckRoll;
     
-    int16_t accX; // Accelerometer values used to calculate pitch and roll
+    bool activateNunchuck;
-    int16_t accY;
+    bool motionValuesReset; // This bool is true when the gyro values has been reset
-    int16_t accZ;
+    unsigned long timer;   
 };
 #endif

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);
       }
     }
   }