Track up to four objects using the IR camera

The state of the Wiimote can now also be read including the battery
level

Wii.cpp

@@ -191,13 +191,15 @@ void WII::ACLData(uint8_t* l2capinbuf) {
             //Serial.print("\r\nL2CAP Interrupt");
             if(wiimoteConnected) {
                 if(l2capinbuf[8] == 0xA1) { // HID_THDR_DATA_INPUT
-                    if((l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons
+                    if((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || (l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons
-                        if(motionPlusConnected) {
+                        if((l2capinbuf[9] >= 0x20 && l2capinbuf[9] <= 0x22) || l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33) // These reports have no extensions bytes
+                            ButtonState = (uint32_t)((l2capinbuf[10] & 0x1F) | ((uint16_t)(l2capinbuf[11] & 0x9F) << 8));
+                        else 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(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));
@@ -214,7 +216,7 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                             OldButtonState = ButtonState;
                         }
                     }
-                    if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x35) { // Read the accelerometer
+                    if(l2capinbuf[9] == 0x31 || l2capinbuf[9] == 0x33 || l2capinbuf[9] == 0x35 || l2capinbuf[9] == 0x37) { // 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;                        
@@ -222,7 +224,14 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                         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
+                            wiiState = l2capinbuf[12]; // (0x01: Battery is nearly empty), (0x02:  An Extension Controller is connected), (0x04: Speaker enabled), (0x08: IR enabled), (0x10: LED1, 0x20: LED2, 0x40: LED3, 0x80: LED4)
+                            batteryLevel = l2capinbuf[15]; // Update battery level
+                            if(l2capinbuf[12] & 0x01) {
+#ifdef DEBUG
+                                Notify(PSTR("\r\nWARNING: Battery is nearly empty"));
+#endif                                
+                            }
                             if(l2capinbuf[12] & 0x02) { // Check if a extension is connected
 #ifdef DEBUG
                                 if(!unknownExtensionConnected)
@@ -325,7 +334,9 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                             roll = wiiMoteRoll;
                             break;
                         case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE
-                        case 0x33: // Core Buttons with Accelerometer and 12 IR bytes - (a1) 33 BB BB EE EE EE EE EE EE EE EE
+                        case 0x33: // Core Buttons with Accelerometer and 12 IR bytes - (a1) 33 BB BB AA AA AA II II II II II II II II II II II II
+                            pitch = wiiMotePitch; // The pitch is just equal to the angle calculated from the wiimote as there is no Motion Plus data available
+                            roll = wiiMoteRoll;
 #ifdef WIICAMERA
                             // Read the IR data                            
                             IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position
@@ -335,6 +346,14 @@ void WII::ACLData(uint8_t* l2capinbuf) {
                             IR_object_x2 = (l2capinbuf[18] | ((uint16_t)(l2capinbuf[20] & 0x30) << 4));
                             IR_object_y2 = (l2capinbuf[19] | ((uint16_t)(l2capinbuf[20] & 0xC0) << 2));
                             IR_object_s2 = (l2capinbuf[20] & 0x0F);
+
+                            IR_object_x3 = (l2capinbuf[21] | ((uint16_t)(l2capinbuf[23] & 0x30) << 4));
+                            IR_object_y3 = (l2capinbuf[22] | ((uint16_t)(l2capinbuf[23] & 0xC0) << 2));
+                            IR_object_s3 = (l2capinbuf[23] & 0x0F);
+
+                            IR_object_x4 = (l2capinbuf[24] | ((uint16_t)(l2capinbuf[26] & 0x30) << 4));
+                            IR_object_y4 = (l2capinbuf[25] | ((uint16_t)(l2capinbuf[26] & 0xC0) << 2));
+                            IR_object_s4 = (l2capinbuf[26] & 0x0F);
 #endif
                             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
@@ -965,13 +984,17 @@ void WII::IRinitialize(){ // Turns on and initialises the IR camera
 #endif
         delay(80);
 
-        setReportMode(false, 0x33); // Note wiiMotePitch won't return values anymore because it uses output report 0x31 or 0x35
+        setReportMode(false, 0x33);
         //setReportMode(false, 0x3f); // For full reporting mode, doesn't work yet
 #ifdef DEBUG
         Notify(PSTR("\r\nSet Report Mode to 0x33"));
 #endif
-
+        delay(80);
-        Notify(PSTR("\r\nIR enabled and Initialized"));
+        
+        statusRequest(); // Used to update wiiState - call isIRCameraEnabled() afterwards to check if it actually worked
+#ifdef DEBUG
+        Notify(PSTR("\r\nIR Initialized"));
+#endif        
 }
 
 void WII::enableIRCamera1(){
@@ -1018,7 +1041,7 @@ void WII::write0x08Value(){
     writeData(0xb00030, 1, &cmd);
 }
 
-void WII::setWiiModeNumber(uint8_t mode_number){ //mode_number in hex i.e. 0x03 for mode extended mode
+void WII::setWiiModeNumber(uint8_t mode_number){ // mode_number in hex i.e. 0x03 for extended mode
     writeData(0xb00033,1,&mode_number);
 }
 #endif

Wii.h

@@ -184,19 +184,30 @@ public:
     int16_t gyroRollZero;
     int16_t gyroPitchZero;
 
-    void statusRequest();
+    uint8_t getBatteryLevel() { return batteryLevel; };
+    uint8_t getWiiState() { return wiiState; };
 
 #ifdef WIICAMERA
     /* These are functions for the IR camera */
     void IRinitialize(); // Initialises the camera as per the steps from http://wiibrew.org/wiki/Wiimote#IR_Camera    
 
-    int16_t getIRx1() { return IR_object_x1; }; // IR object 1 x position (0-1023)
+    uint16_t getIRx1() { return IR_object_x1; }; // IR object 1 x position (0-1023)
-    int16_t getIRy1() { return IR_object_y1; }; // IR object 1 y position (0-767)
+    uint16_t getIRy1() { return IR_object_y1; }; // IR object 1 y position (0-767)
-    int8_t getIRs1() { return IR_object_s1; }; // IR object 1 size (0-15)
+    uint8_t getIRs1() { return IR_object_s1; }; // IR object 1 size (0-15)
 
-    int16_t getIRx2() { return IR_object_x2; };
+    uint16_t getIRx2() { return IR_object_x2; };
-    int16_t getIRy2() { return IR_object_y2; };
+    uint16_t getIRy2() { return IR_object_y2; };
-    int8_t getIRs2() { return IR_object_s2; };
+    uint8_t getIRs2() { return IR_object_s2; };
+
+    uint16_t getIRx3() { return IR_object_x3; };
+    uint16_t getIRy3() { return IR_object_y3; };
+    uint8_t getIRs3() { return IR_object_s3; };
+
+    uint16_t getIRx4() { return IR_object_x4; };
+    uint16_t getIRy4() { return IR_object_y4; };
+    uint8_t getIRs4() { return IR_object_s4; };
+
+    bool isIRCameraEnabled() { return (wiiState & 0x08); };
 #endif
     
 private:
@@ -234,6 +245,7 @@ private:
     /* HID Commands */
     void HID_Command(uint8_t* data, uint8_t nbytes);
     void setReportMode(bool continuous, uint8_t mode);
+    void statusRequest();
     
     void writeData(uint32_t offset, uint8_t size, uint8_t* data);
     void initExtension1();
@@ -253,6 +265,9 @@ private:
     bool activateNunchuck;
     bool motionValuesReset; // This bool is true when the gyro values has been reset
     unsigned long timer;
+
+    uint8_t wiiState; // Stores the value in l2capinbuf[12] - (0x01: Battery is nearly empty), (0x02:  An Extension Controller is connected), (0x04: Speaker enabled), (0x08: IR enabled), (0x10: LED1, 0x20: LED2, 0x40: LED3, 0x80: LED4)
+    uint8_t batteryLevel;
     
 #ifdef WIICAMERA
     /* Private function and variables for the readings from teh IR Camera */
@@ -262,14 +277,19 @@ private:
     void writeSensitivityBlock2();
     void write0x08Value();
     void setWiiModeNumber(uint8_t mode_number);
-
+    
-    int8_t IR_state; //stores the value in l2capinbuf[12] (0x08 means IR enabled)
+    uint16_t IR_object_x1; // IR x position 10 bits
-    int16_t IR_object_x1; // IR x position data 10 bits
+    uint16_t IR_object_y1; // IR y position 10 bits
-    int16_t IR_object_y1;  //IR y position data 10 bits
+    uint8_t IR_object_s1; // IR size value
-    int8_t IR_object_s1; // IR size value
+    uint16_t IR_object_x2;
-    int16_t IR_object_x2;
+    uint16_t IR_object_y2;
-    int16_t IR_object_y2;
+    uint8_t IR_object_s2;
-    int8_t IR_object_s2;
+    uint16_t IR_object_x3; // IR x position 10 bits
+    uint16_t IR_object_y3; // IR y position 10 bits
+    uint8_t IR_object_s3; // IR size value
+    uint16_t IR_object_x4;
+    uint16_t IR_object_y4;
+    uint8_t IR_object_s4;
 #endif
 };
 #endif

examples/Bluetooth/WiiIRCamera/WiiIRCamera.ino

@@ -21,8 +21,8 @@ BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
 WII Wii(&Btd,PAIR); // This will start an inquiry and then pair with your Wiimote - you only have to do this once
 //WII Wii(&Btd); // After the wiimote pairs once with the line of code above, you can simply create the instance like so and re upload and then press any button on the Wiimote
 
-bool printIR1;
+bool printAngle;
-bool printIR2;
+uint8_t printObjects;
 
 void setup() {
   Serial.begin(115200);
@@ -41,42 +41,70 @@ void loop() {
       Wii.disconnect(); // Disconnect the Wiimote - it will establish the connection again since the Wiimote automatically reconnects
     }    
     else {
-      if(Wii.getButtonClick(ONE)) {
+      if(Wii.getButtonClick(ONE))
         Wii.IRinitialize(); // Run the initialisation sequence
-        //Wii.statusRequest(); // This function isn't working right now
-      }
-      if(Wii.getButtonClick(TWO)) // Check status request. Returns if IR is intialized or not (Serial Monitor only)
-        Wii.statusRequest(); // Isn't working proberly. It returns "extension disconnected", will fix soon
       if(Wii.getButtonClick(MINUS)) {
-        printIR1 = !printIR1; // Will track 1 bright point
+        if(!Wii.isIRCameraEnabled())
-        printIR2 = false;
+          Serial.print(F("\r\nEnable IR camera first"));
+        else {
+          if(printObjects > 0)
+            printObjects--;
+        }
       }
-      if(Wii.getButtonClick(PLUS)) { // Will track 2 brightest points 
+      if(Wii.getButtonClick(PLUS)) { // Will track 2 brightest points
-        printIR2 = !printIR2;
+        if(!Wii.isIRCameraEnabled())
-        printIR1 = false;
+          Serial.print(F("\r\nEnable IR camera first"));
+        else {
+          if(printObjects < 4)
+            printObjects++;
+        }
+      }
+      if(Wii.getButtonClick(A)) {
+        printAngle = !printAngle;
+        Serial.print(F("\r\nA"));
+      }
+      if(Wii.getButtonClick(B)) {
+        Serial.print(F("\r\nBattery level: "));
+        Serial.print(Wii.getBatteryLevel()); // You can get the battery level as well
       }
     }
-    if(printIR1) {
+    if(printObjects > 0) {
-      Serial.print(F("\r\n y1: "));
+      Serial.print(F("\r\ny1: "));
       Serial.print(Wii.getIRy1());  
-      Serial.print(F("\t x1: "));
+      Serial.print(F("\tx1: "));
       Serial.print(Wii.getIRx1()); 
-      Serial.print(F("\t s1:"));
+      Serial.print(F("\ts1:"));
-      Serial.print(Wii.getIRs1());     
-    }
-    if(printIR2) {
-      Serial.print(F("\r\n y1: "));
-      Serial.print(Wii.getIRy1());
-      Serial.print(F("\t y2: "));
-      Serial.print(Wii.getIRy2());
-      Serial.print(F("\t x1: "));
-      Serial.print(Wii.getIRx1());
-      Serial.print(F("\t x2: "));
-      Serial.print(Wii.getIRx2());              
-      Serial.print(F("\t s1:"));
       Serial.print(Wii.getIRs1());
-      Serial.print(F("\t s2:"));
+      if(printObjects > 1) {
-      Serial.print(Wii.getIRs2());   
+        Serial.print(F("\t\ty2: "));
+        Serial.print(Wii.getIRy2());  
+        Serial.print(F("\tx2: "));
+        Serial.print(Wii.getIRx2()); 
+        Serial.print(F("\ts2:"));
+        Serial.print(Wii.getIRs2());
+        if(printObjects > 2) {
+          Serial.print(F("\t\ty3: "));
+          Serial.print(Wii.getIRy3());  
+          Serial.print(F("\tx3: "));
+          Serial.print(Wii.getIRx3()); 
+          Serial.print(F("\ts3:"));
+          Serial.print(Wii.getIRs3());
+          if(printObjects > 3) {
+            Serial.print(F("\t\ty4: "));
+            Serial.print(Wii.getIRy4());  
+            Serial.print(F("\tx4: "));
+            Serial.print(Wii.getIRx4()); 
+            Serial.print(F("\ts4:"));
+            Serial.print(Wii.getIRs4());
+          }
+        }
+      }
+    } 
+    if(printAngle) { // There is no extension bytes avaliable, so the motionplus or nunchuck can't be read
+      Serial.print(F("\r\nPitch: "));
+      Serial.print(Wii.getPitch());
+      Serial.print(F("\tRoll: "));
+      Serial.print(Wii.getRoll());
     }
   }
 }