specchioriflesso/USB_Host_Shield_2.0
1
Fork 0
mirror of https://github.com/felis/USB_Host_Shield_2.0.git synced 2024-03-22 11:31:26 +01:00
Code Issues Projects Releases Packages Wiki Activity

Convert Wii Balance Board readings into kg

Browse source
This commit is contained in:
Kristian Sloth Lauszus 2015-04-16 16:55:35 +02:00
parent 9cb31799a8
commit 7acf598a59
4 changed files with 105 additions and 21 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

40
Wii.cpp
View file

@ -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;
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 */

57
Wii.h
View file

@ -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

22
examples/Bluetooth/WiiBalanceBoard/WiiBalanceBoard.ino
View file

@ -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();
}
}
}

7
keywords.txt
View file

@ -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