Convert Wii Balance Board readings into kg

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<uint8_t > (l2capinbuf[13], 0x80);
                                                                 D_PrintHex<uint8_t > (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<uint8_t > (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;
+                                                balanceBoardRaw[TopRight] = l2capinbuf[13] | l2capinbuf[12] << 8; // Top right
-                                                botRight = l2capinbuf[15] | l2capinbuf[14] << 8;
+                                                balanceBoardRaw[BotRight] = l2capinbuf[15] | l2capinbuf[14] << 8; // Bottom right
-                                                topLeft = l2capinbuf[17] | l2capinbuf[16] << 8;
+                                                balanceBoardRaw[TopLeft] = l2capinbuf[17] | l2capinbuf[16] << 8; // Top left
-                                                botleft = l2capinbuf[19] | l2capinbuf[18] << 8;
+                                                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                          */
 

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;
         /**@}*/
 
-        /**@{*/
+        /** @name Wii Balance Board functions */
-        /** Wii Balance Board raw values. */
+
-        uint16_t topRight, botRight, topLeft, botleft;
+        /**
+         * 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

examples/Bluetooth/WiiBalanceBoard/WiiBalanceBoard.ino

@@ -34,20 +34,18 @@ void setup() {
 }
 void loop() {
   Usb.Task();
-  if (Wii.wiiBalanceBoardConnected) {
+  if (Wii.wiiBalanceBoardConnected && Wii.wiiBalanceBoardCalibrationComplete) {
-    Serial.print(F("\r\ntopRight: "));
+    Serial.print(F("\r\nWeight: "));
-    Serial.print(Wii.topRight);
+    for (uint8_t i = 0; i < 4; i++) {
-    Serial.print(F("\tbotRight: "));
+      Serial.print(Wii.getWeight((BalanceBoardEnum)i));
-    Serial.print(Wii.botRight);
+      Serial.print(F("\t"));
-    Serial.print(F("\ttopLeft: "));
+    }
-    Serial.print(Wii.topLeft);
+    Serial.print(F("Total Weight: "));
-    Serial.print(F("\tbotleft: "));
+    Serial.print(Wii.getTotalWeight());
-    Serial.print(Wii.botleft);
-
     if (Wii.getButtonClick(A)) {
       Serial.print(F("\r\nA"));
-      Wii.setLedToggle(LED1);
+      //Wii.setLedToggle(LED1); // The Wii Balance Board has one LED as well
-      //Wii.disconnect();
+      Wii.disconnect();
     }
   }
 }

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