diff --git a/Wii.cpp b/Wii.cpp index a629821a..b1f155f5 100755 --- a/Wii.cpp +++ b/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(motionPlusConnected) { + 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((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 - - Notify(PSTR("\r\nIR enabled and Initialized")); + delay(80); + + 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 \ No newline at end of file diff --git a/Wii.h b/Wii.h index 87d8a028..adb15289 100755 --- a/Wii.h +++ b/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) - 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) + uint16_t getIRx1() { return IR_object_x1; }; // IR object 1 x position (0-1023) + uint16_t getIRy1() { return IR_object_y1; }; // IR object 1 y position (0-767) + uint8_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; }; + uint16_t getIRx2() { return IR_object_x2; }; + uint16_t getIRy2() { return IR_object_y2; }; + 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) - 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; + + uint16_t IR_object_x1; // IR x position 10 bits + uint16_t IR_object_y1; // IR y position 10 bits + uint8_t IR_object_s1; // IR size value + uint16_t IR_object_x2; + uint16_t IR_object_y2; + uint8_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 \ No newline at end of file diff --git a/examples/Bluetooth/WiiIRCamera/WiiIRCamera.ino b/examples/Bluetooth/WiiIRCamera/WiiIRCamera.ino index 0598b9d3..77ce3818 100644 --- a/examples/Bluetooth/WiiIRCamera/WiiIRCamera.ino +++ b/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 printIR2; +bool printAngle; +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 - printIR2 = false; + if(!Wii.isIRCameraEnabled()) + Serial.print(F("\r\nEnable IR camera first")); + else { + if(printObjects > 0) + printObjects--; + } } - if(Wii.getButtonClick(PLUS)) { // Will track 2 brightest points - printIR2 = !printIR2; - printIR1 = false; + if(Wii.getButtonClick(PLUS)) { // Will track 2 brightest points + if(!Wii.isIRCameraEnabled()) + 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) { - Serial.print(F("\r\n y1: ")); + if(printObjects > 0) { + 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(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(F("\ts1:")); Serial.print(Wii.getIRs1()); - Serial.print(F("\t s2:")); - Serial.print(Wii.getIRs2()); + if(printObjects > 1) { + 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()); } } }