From 7acf598a59d01113140022a0cf94637bea116307 Mon Sep 17 00:00:00 2001 From: Kristian Sloth Lauszus Date: Thu, 16 Apr 2015 16:55:35 +0200 Subject: [PATCH] Convert Wii Balance Board readings into kg --- Wii.cpp | 40 +++++++++++-- Wii.h | 57 ++++++++++++++++++- .../WiiBalanceBoard/WiiBalanceBoard.ino | 22 ++++--- keywords.txt | 7 +++ 4 files changed, 105 insertions(+), 21 deletions(-) diff --git a/Wii.cpp b/Wii.cpp index 902a4301..d7b0b8c0 100755 --- a/Wii.cpp +++ b/Wii.cpp @@ -344,6 +344,7 @@ void WII::ACLData(uint8_t* l2capinbuf) { break; case 0x21: // Read Memory Data if((l2capinbuf[12] & 0x0F) == 0) { // No error + uint8_t reportLength = (l2capinbuf[12] >> 4) + 1; // // Bit 4-7 is the length - 1 // See: http://wiibrew.org/wiki/Wiimote/Extension_Controllers if(l2capinbuf[16] == 0x00 && l2capinbuf[17] == 0xA4 && l2capinbuf[18] == 0x20 && l2capinbuf[19] == 0x00 && l2capinbuf[20] == 0x00) { #ifdef DEBUG_USB_HOST @@ -391,15 +392,30 @@ void WII::ACLData(uint8_t* l2capinbuf) { Notify(PSTR("\r\nWii Balance Board connected"), 0x80); #endif setReportMode(false, 0x32); // Read the Wii Balance Board extension + wiiBalanceBoardCalibrationComplete = false; wiiBalanceBoardConnected = true; } + // Wii Balance Board calibration reports (24 bits in total) + else if(l2capinbuf[13] == 0x00 && l2capinbuf[14] == 0x24 && reportLength == 16) { // First 16-bit + for(uint8_t i = 0; i < 2; i++) { + for(uint8_t j = 0; j < 4; j++) + balanceBoardCal[i][j] = l2capinbuf[16 + 8 * i + 2 * j] | l2capinbuf[15 + 8 * i + 2 * j] << 8; + } + } else if(l2capinbuf[13] == 0x00 && l2capinbuf[14] == 0x34 && reportLength == 8) { // Last 8-bit + for(uint8_t j = 0; j < 4; j++) + balanceBoardCal[2][j] = l2capinbuf[16 + 2 * j] | l2capinbuf[15 + 2 * j] << 8; +#ifdef DEBUG_USB_HOST + Notify(PSTR("\r\nWii Balance Board calibrated successfully"), 0x80); +#endif + wiiBalanceBoardCalibrationComplete = true; + } #ifdef DEBUG_USB_HOST else { Notify(PSTR("\r\nUnknown Device: "), 0x80); D_PrintHex (l2capinbuf[13], 0x80); D_PrintHex (l2capinbuf[14], 0x80); Notify(PSTR("\r\nData: "), 0x80); - for(uint8_t i = 0; i < ((l2capinbuf[12] >> 4) + 1); i++) { // bit 4-7 is the length-1 + for(uint8_t i = 0; i < reportLength; i++) { D_PrintHex (l2capinbuf[15 + i], 0x80); Notify(PSTR(" "), 0x80); } @@ -428,10 +444,10 @@ void WII::ACLData(uint8_t* l2capinbuf) { break; case 0x32: // Core Buttons with 8 Extension bytes - (a1) 32 BB BB EE EE EE EE EE EE EE EE // See: http://wiibrew.org/wiki/Wii_Balance_Board#Data_Format - topRight = l2capinbuf[13] | l2capinbuf[12] << 8; - botRight = l2capinbuf[15] | l2capinbuf[14] << 8; - topLeft = l2capinbuf[17] | l2capinbuf[16] << 8; - botleft = l2capinbuf[19] | l2capinbuf[18] << 8; + balanceBoardRaw[TopRight] = l2capinbuf[13] | l2capinbuf[12] << 8; // Top right + balanceBoardRaw[BotRight] = l2capinbuf[15] | l2capinbuf[14] << 8; // Bottom right + balanceBoardRaw[TopLeft] = l2capinbuf[17] | l2capinbuf[16] << 8; // Top left + balanceBoardRaw[BotLeft] = l2capinbuf[19] | l2capinbuf[18] << 8; // Bottom left break; 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 #ifdef WIICAMERA @@ -768,13 +784,21 @@ void WII::Run() { if(unknownExtensionConnected) // Check if there is a extension is connected to the port initExtension1(); else - stateCounter = 399; + stateCounter = 499; } else if(stateCounter == 200) initExtension2(); else if(stateCounter == 300) { readExtensionType(); unknownExtensionConnected = false; } else if(stateCounter == 400) { + if(wiiBalanceBoardConnected) { +#ifdef DEBUG_USB_HOST + Notify(PSTR("\r\nCalibrating Wii Balance Board"), 0x80); +#endif + calibrateWiiBalanceBoard(); + } else + stateCounter = 499; + } else if(stateCounter == 500) { stateCounter = 0; l2cap_state = TURN_ON_LED; } @@ -1047,6 +1071,10 @@ void WII::checkMotionPresent() { readData(0xA600FA, 6, false); } +void WII::calibrateWiiBalanceBoard() { + readData(0xA40024, 24, false); +} + /************************************************************/ /* WII Commands */ diff --git a/Wii.h b/Wii.h index f2e52947..91026469 100755 --- a/Wii.h +++ b/Wii.h @@ -39,6 +39,14 @@ enum HatEnum { HatY = 1, }; +/** Enum used to read the weight on Wii Balance Board. */ +enum BalanceBoardEnum { + TopRight = 0, + BotRight = 1, + TopLeft = 2, + BotLeft = 3, +}; + /** * This BluetoothService class implements support for the Wiimote including the Nunchuck and Motion Plus extension. * @@ -263,9 +271,48 @@ public: int16_t gyroPitchZero; /**@}*/ - /**@{*/ - /** Wii Balance Board raw values. */ - uint16_t topRight, botRight, topLeft, botleft; + /** @name Wii Balance Board functions */ + + /** + * Used to get the weight at the specific position on the Wii Balance Board. + * @param ::BalanceBoardEnum to read from. + * @return Returns the weight in kg. + */ + float getWeight(BalanceBoardEnum pos) { + // Based on: https://github.com/skorokithakis/gr8w8upd8m8/blob/master/gr8w8upd8m8.py + // calibration[pos][0] is calibration values for 0 kg + // calibration[pos][1] is calibration values for 17 kg + // calibration[pos][2] is calibration values for 34 kg + float val = 0; + if(balanceBoardRaw[pos] < balanceBoardCal[0][pos]) + return val; + else if(balanceBoardRaw[pos] < balanceBoardCal[1][pos]) + val = 17 * ((balanceBoardRaw[pos] - balanceBoardCal[0][pos]) / float((balanceBoardCal[1][pos] - balanceBoardCal[0][pos]))); + else if(balanceBoardRaw[pos] > balanceBoardCal[1][pos]) + val = 17 + 17 * ((balanceBoardRaw[pos] - balanceBoardCal[1][pos]) / float((balanceBoardCal[2][pos] - balanceBoardCal[1][pos]))); + + return val; + }; + + /** + * Used to get total weight on the Wii Balance Board. + * @returnReturns the weight in kg. + */ + float getTotalWeight() { + return getWeight(TopRight) + getWeight(BotRight) + getWeight(TopLeft) + getWeight(BotLeft); + }; + + /** + * Used to get the raw reading at the specific position on the Wii Balance Board. + * @param ::BalanceBoardEnum to read from. + * @return Returns the raw reading. + */ + uint16_t getWeightRaw(BalanceBoardEnum pos) { + return balanceBoardRaw[pos]; + }; + + /** Indicates when the calibration of the Wii Balance Board is done. */ + bool wiiBalanceBoardCalibrationComplete; /**@}*/ #ifdef WIICAMERA @@ -448,6 +495,10 @@ private: void checkMotionPresent(); // Used to see if a Motion Plus is connected to the Wiimote void initMotionPlus(); void activateMotionPlus(); + void calibrateWiiBalanceBoard(); + + uint16_t balanceBoardRaw[4]; // Wii Balance Board raw values + uint16_t balanceBoardCal[3][4]; // Wii Balance Board calibration values double compPitch; // Fusioned angle using a complimentary filter if the Motion Plus is connected double compRoll; // Fusioned angle using a complimentary filter if the Motion Plus is connected diff --git a/examples/Bluetooth/WiiBalanceBoard/WiiBalanceBoard.ino b/examples/Bluetooth/WiiBalanceBoard/WiiBalanceBoard.ino index 11459519..6186a179 100644 --- a/examples/Bluetooth/WiiBalanceBoard/WiiBalanceBoard.ino +++ b/examples/Bluetooth/WiiBalanceBoard/WiiBalanceBoard.ino @@ -34,20 +34,18 @@ void setup() { } void loop() { Usb.Task(); - if (Wii.wiiBalanceBoardConnected) { - Serial.print(F("\r\ntopRight: ")); - Serial.print(Wii.topRight); - Serial.print(F("\tbotRight: ")); - Serial.print(Wii.botRight); - Serial.print(F("\ttopLeft: ")); - Serial.print(Wii.topLeft); - Serial.print(F("\tbotleft: ")); - Serial.print(Wii.botleft); - + if (Wii.wiiBalanceBoardConnected && Wii.wiiBalanceBoardCalibrationComplete) { + Serial.print(F("\r\nWeight: ")); + for (uint8_t i = 0; i < 4; i++) { + Serial.print(Wii.getWeight((BalanceBoardEnum)i)); + Serial.print(F("\t")); + } + Serial.print(F("Total Weight: ")); + Serial.print(Wii.getTotalWeight()); if (Wii.getButtonClick(A)) { Serial.print(F("\r\nA")); - Wii.setLedToggle(LED1); - //Wii.disconnect(); + //Wii.setLedToggle(LED1); // The Wii Balance Board has one LED as well + Wii.disconnect(); } } } diff --git a/keywords.txt b/keywords.txt index ade71b28..f4e19cd1 100644 --- a/keywords.txt +++ b/keywords.txt @@ -282,6 +282,9 @@ PAIR KEYWORD2 statusRequest KEYWORD2 getBatteryLevel KEYWORD2 getWiiState KEYWORD2 +getWeight KEYWORD2 +getTotalWeight KEYWORD2 +getWeightRaw KEYWORD2 #################################################### # Constants and enums (LITERAL1) @@ -300,6 +303,10 @@ ZL LITERAL1 ZR LITERAL1 HatX LITERAL1 HatY LITERAL1 +TopRight LITERAL1 +BotRight LITERAL1 +TopLeft LITERAL1 +BotLeft LITERAL1 #################################################### # Methods and Functions for the IR Camera