Added support for the IR Camera inside the Wiimote

Thanks to Allan Glover: https://github.com/Engr-AllanG

Wii.cpp

@@ -13,6 +13,10 @@
  Kristian Lauszus, TKJ Electronics
  Web      :  http://www.tkjelectronics.com
  e-mail   :  kristianl@tkjelectronics.com
+
+ IR camera support added by:
+ Allan Glover
+ adglover9.81@gmail.com
  */
 
 #include "Wii.h"
@@ -184,7 +188,7 @@ 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) { // These reports include the buttons
+                    if((l2capinbuf[9] >= 0x30 && l2capinbuf[9] <= 0x37) || l2capinbuf[9] == 0x3e || l2capinbuf[9] == 0x3f) { // These reports include the buttons
                         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)));
@@ -318,8 +322,40 @@ 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
+#ifdef WIICAMERA
+                            // Read the IR data                            
+                            IR_object_x1 = (l2capinbuf[15] | ((uint16_t)(l2capinbuf[17] & 0x30) << 4)); // x position
+                            IR_object_y1 = (l2capinbuf[16] | ((uint16_t)(l2capinbuf[17] & 0xC0) << 2)); // y position
+                            IR_object_s1 = (l2capinbuf[17] & 0x0F); // size value, 0-15
+
+                            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);
+#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
                             break;
+                        /* 0x3e and 0x3f both give unknown report types when report mode is 0x3e or 0x3f with mode number 0x05 */
+                        case 0x3E: // Core Buttons with Accelerometer and 32 IR bytes
+                                   // (a1) 31 BB BB AA AA AA II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II II 
+                                   // corresponds to output report mode 0x3e
+
+                            /**** for reading in full mode: DOES NOT WORK YET ****/
+                            /* When it works it will also have intensity and bounding box data */
+                            /*
+                            IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
+                            IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
+                            IR_object_s1 = (l2capinbuf[15] & 0x0F);
+                            */         
+                            break;
+                        case 0x3F:
+                            /*
+                            IR_object_x1 = (l2capinbuf[13] | ((uint16_t)(l2capinbuf[15] & 0x30) << 4));
+                            IR_object_y1 = (l2capinbuf[14] | ((uint16_t)(l2capinbuf[15] & 0xC0) << 2));
+                            IR_object_s1 = (l2capinbuf[15] & 0x0F);
+                            */
+                            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(motionPlusConnected) {
@@ -870,4 +906,114 @@ uint8_t WII::getAnalogHat(AnalogHat a) {
         else
             return output;
     }
-}
+}
+
+/************************************************************/
+/*       The following functions are for the IR camera      */
+/************************************************************/
+
+#ifdef WIICAMERA
+
+void WII::IRinitialize(){ //Turns on and initialises the IR camera
+    
+        EnableIRCamera1();
+#ifdef DEBUG
+        Notify(PSTR("\r\nEnable IR Camera1 Complete"));
+#endif
+        delay(80);
+        
+        EnableIRCamera2();
+#ifdef DEBUG
+        Notify(PSTR("\r\nEnable IR Camera2 Complete"));
+#endif
+        delay(80);
+    
+        write0x08Value();
+#ifdef DEBUG
+        Notify(PSTR("\r\nWrote hex number 0x08"));
+#endif
+        delay(80);
+        
+        WriteSensitivityBlock1();
+#ifdef DEBUG
+        Notify(PSTR("\r\nWrote Sensitivity Block 1"));
+#endif
+        delay(80);
+        
+        WriteSensitivityBlock2();
+#ifdef DEBUG
+        Notify(PSTR("\r\nWrote Sensitivity Block 2"));
+#endif
+        delay(80);
+
+        uint8_t mode_num[] = {0x03};
+        setWIIModeNumber(mode_num); //change input for whatever mode you want i.e. 0x01, 0x03, or 0x05
+#ifdef DEBUG
+        Notify(PSTR("\r\nSet Wii Mode Number To 0x"));
+        PrintHex<uint8_t>(mode_num[0]);
+#endif
+        delay(80);
+    
+        write0x08Value();
+#ifdef DEBUG
+        Notify(PSTR("\r\nWrote Hex Number 0x08"));
+#endif
+        delay(80);
+
+        setReportMode(false, 0x33); //note wiiMotePitch won't return values anymore because it uses output report 0x31 or 0x35
+        //setReportMode(false, 0x3f); //for full reporting mode, doesn't work
+#ifdef DEBUG
+        Notify(PSTR("\r\nSet Report Mode to 0x33"));
+#endif
+
+        Notify(PSTR("\r\nIR enabled and Initialized"));
+}
+
+void WII::EnableIRCamera1(){
+    uint8_t cmd_buf[3];
+    cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
+    cmd_buf[1] = 0x13; //output report 13
+    cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01);  // Keep the rumble bit and sets bit 2 
+    HID_Command(cmd_buf, 3);
+}
+
+void WII::EnableIRCamera2(){
+    uint8_t cmd_buf[3];
+    cmd_buf[0] = 0xA2; // HID BT DATA_request (0xA0) | Report Type (Output 0x02)
+    cmd_buf[1] = 0x1A; //output report 1A
+    cmd_buf[2] = 0x04 | (HIDBuffer[2] & 0x01); // Keep the rumble bit and sets bit 2 
+    HID_Command(cmd_buf, 3);
+}
+
+void WII::WriteSensitivityBlock1(){
+    uint8_t buf[9];
+    buf[0] = 0x00;
+    buf[1] = 0x00;
+    buf[2] = 0x00;
+    buf[3] = 0x00;
+    buf[4] = 0x00;
+    buf[5] = 0x00;
+    buf[6] = 0x90;
+    buf[7] = 0x00;
+    buf[8] = 0x41;
+
+    writeData(0xB00000, 9, buf);
+}
+
+void WII::WriteSensitivityBlock2(){
+    uint8_t buf[2];
+    buf[0] = 0x40;
+    buf[1] = 0x00;
+
+    writeData(0xB0001A, 2, buf);
+}
+
+void WII::write0x08Value(){
+    uint8_t Value[]={0x08};
+    writeData(0xb00030, 1, Value);
+}
+
+void WII::setWIIModeNumber(uint8_t* mode_number){ //mode_number in hex i.e. 0x03 for mode extended mode
+    writeData(0xb00033,1,mode_number);
+}
+#endif

Wii.h

@@ -13,6 +13,10 @@
  Kristian Lauszus, TKJ Electronics
  Web      :  http://www.tkjelectronics.com
  e-mail   :  kristianl@tkjelectronics.com
+
+ IR camera support added by:
+ Allan Glover
+ adglover9.81@gmail.com
  */
 
 #ifndef _wii_h_
@@ -20,6 +24,8 @@
 
 #include "BTD.h"
 
+//#define WIICAMERA //uncomment to enable IR camera
+
 /* Bluetooth L2CAP states for L2CAP_task() */
 #define L2CAP_WAIT                      0
 
@@ -177,6 +183,36 @@ public:
     int16_t gyroYawZero; // These values are set when the controller is first initialized
     int16_t gyroRollZero;
     int16_t gyroPitchZero;
+
+    void statusRequest();
+
+    /* These are functions for the IR camera */
+#ifdef WIICAMERA
+    void IRinitialize(); //Initialises the camera as per the steps from http://wiibrew.org/wiki/Wiimote#IR_Camera
+    void EnableIRCamera1(); //Sets bit 2 of output report 13 
+    void EnableIRCamera2(); //Sets bit 2 of output report 1A
+    void WriteSensitivityBlock1();
+    void WriteSensitivityBlock2();
+    void write0x08Value();
+    void setWIIModeNumber(uint8_t* mode_number);
+    
+    
+    int8_t IR_state; //stores the value in l2capinbuf[12] (0x08 means IR enabled)
+    int16_t IR_object_x1; // IR x position data 10 bits
+    int16_t IR_object_y1;  //IR y position data 10 bits
+    int8_t IR_object_s1; // IR size value
+    int16_t IR_object_x2;
+    int16_t IR_object_y2;
+    int8_t IR_object_s2;
+
+    int16_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)
+    int8_t getIRs1() { return IR_object_s1; }; //IR object 1 size (0-15)
+
+    int16_t getIRx2() { return IR_object_x2; };
+    int16_t getIRy2() { return IR_object_y2; };
+    int8_t getIRs2() { return IR_object_s2; };
+#endif
     
 private:
     /* Mandatory members */
@@ -212,7 +248,6 @@ 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();

WiiCameraReadme.md

@@ -0,0 +1,17 @@
+Please see <http://wiibrew.org/wiki/Wiimote#IR_Camera> for the complete capabilities of the Wii camera. The IR camera code was written based on the above website and with support from Kristian Lauszus.
+
+Must omit the "." in the name of the USB_Host_Shiled_2.0 library folder when inserting into the Arudino library folder.
+
+This library is large, if you run into memory problems when uploading to the arduino, comment out the #define DEBUG in the BTD.cpp and Wii.cpp files.
+
+To enable the IR camera code, uncomment #define WIICAMERA in Wii.h.
+
+This library impliments the following settings:
+
+* Report sensitivity mode: 00 00 00 00 00 00 90 00 41	 40 00	 Suggested by inio (high sensitivity)
+* Data Format: Extended mode (0x03).  Full mode is not working yet. The output reports 0x3e and 0x3f need tampering with
+	* In this mode the camera outputs x and y corridinates and a size dimension for the 4 brightest points. 
+
+__CURRENTLY, CODE IS ONLY WRITTEN FOR THE 2 BRIGHTEST POINTS__
+
+Again, read through <http://wiibrew.org/wiki/Wiimote#IR_Camera> to get an understanding of the camera and its settings.

examples/Bluetooth/Wii/WiiCamera/WiiCamera.ino

@@ -0,0 +1,80 @@
+/*
+Example sketch for the Wii libary showing the IR camera functionality. This example
+is for the Bluetooth Wii library developed for the USB shield from Circuits@Home
+ 
+Created by Allan Glover and includes much from what Kristian Lauszus wrote in the existing 
+Wii example. Contact Kristian:  http://blog.tkjelectronics.dk/ or send email at kristianl@tkjelectronics.com.
+Contact Allan at adglover9.81@gmail.com
+ 
+To test the Wiimote IR camera, you will need access to an IR source. Sunlight will work but is not ideal. 
+The simpleist solution is to use the Wii sensor bar, i.e. emitter bar, supplied by the Wii system. Otherwise,
+wire up a IR LED yourself.
+*/
+
+#include <Wii.h>
+USB Usb;
+BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
+/* You can create the instance of the class in two ways */
+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 printIR2;
+
+void setup() {
+  Serial.begin(115200);
+  if (Usb.Init() == -1) {
+    Serial.print(F("\r\nOSC did not start"));
+    while(1); //halt
+  }
+  Serial.print(F("\r\nWiimote Bluetooth Library Started"));
+}
+
+void loop() {
+  Usb.Task();
+  if(Wii.wiimoteConnected) {
+    if(Wii.getButtonClick(HOME)) { // You can use getButtonPress to see if the button is held down
+      Serial.print(F("\r\nHOME"));
+      Wii.disconnect(); // Disconnect the Wiimote - it will establish the connection again since the Wiimote automatically reconnects
+    }    
+    else {
+      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
+        printIR2 = false;
+      }
+      if(Wii.getButtonClick(PLUS)) { // Will track 2 brightest points 
+        printIR2 = !printIR2;
+        printIR1 = false;
+      }
+    }
+    if(printIR1) {
+      Serial.print(F("\r\n y1: "));
+      Serial.print(Wii.getIRy1());  
+      Serial.print(F("\t x1: "));
+      Serial.print(Wii.getIRx1()); 
+      Serial.print(F("\t s1:"));
+      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:"));
+      Serial.print(Wii.getIRs2());   
+    }
+  }
+}
+