From eb088895f592bf3c2b62de8954bd4cde1f6c25c7 Mon Sep 17 00:00:00 2001 From: Kristian Sloth Lauszus Date: Thu, 17 Jan 2013 00:07:56 +0100 Subject: [PATCH] Added support for the IR Camera inside the Wiimote Thanks to Allan Glover: https://github.com/Engr-AllanG --- Wii.cpp | 150 +++++++++++++++++- Wii.h | 37 ++++- WiiCameraReadme.md | 17 ++ .../Bluetooth/Wii/WiiCamera/WiiCamera.ino | 80 ++++++++++ 4 files changed, 281 insertions(+), 3 deletions(-) create mode 100644 WiiCameraReadme.md create mode 100644 examples/Bluetooth/Wii/WiiCamera/WiiCamera.ino diff --git a/Wii.cpp b/Wii.cpp index 922188fa..3ff7e2d3 100755 --- a/Wii.cpp +++ b/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; } -} \ No newline at end of file +} + +/************************************************************/ +/* 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(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 \ No newline at end of file diff --git a/Wii.h b/Wii.h index 1108ccb2..fd78ff7f 100755 --- a/Wii.h +++ b/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(); diff --git a/WiiCameraReadme.md b/WiiCameraReadme.md new file mode 100644 index 00000000..375a320c --- /dev/null +++ b/WiiCameraReadme.md @@ -0,0 +1,17 @@ +Please see 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 to get an understanding of the camera and its settings. \ No newline at end of file diff --git a/examples/Bluetooth/Wii/WiiCamera/WiiCamera.ino b/examples/Bluetooth/Wii/WiiCamera/WiiCamera.ino new file mode 100644 index 00000000..028ca8b1 --- /dev/null +++ b/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 +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()); + } + } +} +