Permalink
Browse files

Balance board patch from http://www.akhphd.au.dk/~bertho/wii/

  • Loading branch information...
1 parent fa8b165 commit cefbef2ee1d7247135441e1ca65925b5498a1c61 @paulburton committed Mar 1, 2010
Showing with 290 additions and 5 deletions.
  1. +33 −0 example/example.c
  2. +1 −0 src/Makefile
  3. +140 −0 src/balance_board.c
  4. +53 −0 src/balance_board.h
  5. +25 −0 src/events.c
  6. +3 −4 src/ir.c
  7. +34 −1 src/wiiuse.h
  8. +1 −0 src/wiiuse_internal.h
View
@@ -36,6 +36,7 @@
#include <stdio.h>
#include <stdlib.h>
+#include <math.h>
#ifndef WIN32
#include <unistd.h>
@@ -56,6 +57,7 @@
* event occurs on the specified wiimote.
*/
void handle_event(struct wiimote_t* wm) {
+ if (wm->exp.type != EXP_BALANCE_BOARD)
printf("\n\n--- EVENT [id %i] ---\n", wm->unid);
/* if a button is pressed, report it */
@@ -181,6 +183,32 @@ void handle_event(struct wiimote_t* wm) {
printf("Guitar whammy bar: %f\n", gh3->whammy_bar);
printf("Guitar joystick angle: %f\n", gh3->js.ang);
printf("Guitar joystick magnitude: %f\n", gh3->js.mag);
+ } else if (wm->exp.type == EXP_BALANCE_BOARD) {
+ static int first = 1;
+ const struct balance_board_t *bb = &wm->exp.bb;
+ float v = bb->tr + bb->tl + bb->br + bb->bl;
+ float x = ((bb->tr + bb->br) - (bb->tl + bb->bl));
+ float y = ((bb->tl + bb->tr) - (bb->bl + bb->br));
+ float a = atan2f(y, x) / M_PI * 180.0;
+
+ /*
+ * Board orientation:
+ * +---------------------+
+ * | tl tr |
+ * | y |
+ * | ^ |
+ * | | |
+ * | +----> x |
+ * | bl br |
+ * +---------vvv---------+
+ * Button-A (power)
+ */
+ if(first) {
+ printf(" tr br tl bl | x y |v| ang\n");
+ first = 0;
+ }
+ printf("% 5.1f % 5.1f % 5.1f % 5.1f | % 5.1f %5.1f % 5.1f % 6.1f\r", bb->tr, bb->br, bb->tl, bb->bl, x, y, v, a);
+ fflush(stdout);
}
}
@@ -407,6 +435,10 @@ int main(int argc, char** argv) {
printf("Classic controller inserted.\n");
break;
+ case WIIUSE_BALANCE_BOARD_CTRL_INSERTED:
+ printf("Balance board controller inserted.\n");
+ break;
+
case WIIUSE_GUITAR_HERO_3_CTRL_INSERTED:
/* some expansion was inserted */
handle_ctrl_status(wiimotes[i]);
@@ -416,6 +448,7 @@ int main(int argc, char** argv) {
case WIIUSE_NUNCHUK_REMOVED:
case WIIUSE_CLASSIC_CTRL_REMOVED:
case WIIUSE_GUITAR_HERO_3_CTRL_REMOVED:
+ case WIIUSE_BALANCE_BOARD_CTRL_REMOVED:
/* some expansion was removed */
handle_ctrl_status(wiimotes[i]);
printf("An expansion was removed.\n");
View
@@ -48,6 +48,7 @@ INCLUDES = -I.
# Generate a list of object files
#
OBJS = \
+ $(OBJ_DIR)/balance_board.o \
$(OBJ_DIR)/classic.o \
$(OBJ_DIR)/dynamics.o \
$(OBJ_DIR)/events.o \
View
@@ -0,0 +1,140 @@
+/*
+ * wiiuse
+ *
+ * Written By:
+ * Michael Laforest < para >
+ * Email: < thepara (--AT--) g m a i l [--DOT--] com >
+ * This file:
+ * Bertho Stultiens < para >
+ *
+ * Copyright 2009
+ *
+ * This file is part of wiiuse.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * $Header$
+ *
+ */
+
+/**
+ * @file
+ * @brief Balance Board expansion device.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#ifdef WIN32
+ #include <Winsock2.h>
+#endif
+
+#include "definitions.h"
+#include "wiiuse_internal.h"
+#include "dynamics.h"
+#include "events.h"
+#include "balance_board.h"
+
+/**
+ * @brief Handle the handshake data from the guitar.
+ *
+ * @param cc A pointer to a classic_ctrl_t structure.
+ * @param data The data read in from the device.
+ * @param len The length of the data block, in bytes.
+ *
+ * @return Returns 1 if handshake was successful, 0 if not.
+ */
+int balance_board_handshake(struct wiimote_t* wm, struct balance_board_t* bb, byte* data, unsigned short len) {
+ int i;
+
+ bb->tr = 0.0;
+ bb->br = 0.0;
+ bb->tl = 0.0;
+ bb->br = 0.0;
+
+ if (len < 0xe0)
+ return 0;
+
+ if (data[0xdc] != 0xa4) {
+ /* decrypt data */
+ for (i = 0; i < len; ++i)
+ data[i] = (data[i] ^ 0x17) + 0x17;
+ }
+
+ /* See: http://wiibrew.org/wiki/Wii_Balance_Board */
+ /* Unknown data at 0xa40020..23 (mine says: 0x44 0x69 0x00 0x00) */
+ bb->cal_0.tr = (data[0x04] << 8) + data[0x05];
+ bb->cal_0.br = (data[0x06] << 8) + data[0x07];
+ bb->cal_0.tl = (data[0x08] << 8) + data[0x09];
+ bb->cal_0.bl = (data[0x0a] << 8) + data[0x0b];
+ bb->cal_17.tr = (data[0x0c] << 8) + data[0x0d];
+ bb->cal_17.br = (data[0x0e] << 8) + data[0x0f];
+ bb->cal_17.tl = (data[0x10] << 8) + data[0x11];
+ bb->cal_17.bl = (data[0x12] << 8) + data[0x13];
+ bb->cal_34.tr = (data[0x14] << 8) + data[0x15];
+ bb->cal_34.br = (data[0x16] << 8) + data[0x17];
+ bb->cal_34.tl = (data[0x18] << 8) + data[0x19];
+ bb->cal_34.bl = (data[0x1a] << 8) + data[0x1b];
+ /* Unknown data at 0xa4003c..3f (mine says: 0x4c 0x81 0x59 0x95) */
+
+ WIIUSE_DEBUG(stderr, "calibration 0: %04x %04x %04x %04x\n", bb->cal_0.tr, bb->cal_0.br, bb->cal_0.tl, bb->cal_0.br);
+ WIIUSE_DEBUG(stderr, "calibration 17: %04x %04x %04x %04x\n", bb->cal_17.tr, bb->cal_17.br, bb->cal_17.tl, bb->cal_17.br);
+ WIIUSE_DEBUG(stderr, "calibration 34: %04x %04x %04x %04x\n", bb->cal_34.tr, bb->cal_34.br, bb->cal_34.tl, bb->cal_34.br);
+
+ /* handshake done */
+ wm->exp.type = EXP_BALANCE_BOARD;
+
+ #ifdef WIN32
+ wm->timeout = WIIMOTE_DEFAULT_TIMEOUT;
+ #endif
+
+ return 1;
+}
+
+
+/**
+ * @brief The balance board disconnected.
+ *
+ * @param cc A pointer to a balance_board_t structure.
+ */
+void balance_board_disconnected(struct balance_board_t* bb) {
+ memset(bb, 0, sizeof(*bb));
+}
+
+
+
+/**
+ * @brief Handle balance board event.
+ *
+ * @param cc A pointer to a balance board_t structure.
+ * @param msg The message specified in the event packet.
+ */
+#define set_bbval(x) do {\
+ if(bb->raw.x < bb->cal_17.x) \
+ bb->x = 17.0 * (float)(bb->raw.x - bb->cal_0.x) / (float)(bb->cal_17.x - bb->cal_0.x); \
+ else \
+ bb->x = 17.0 * (1.0 + (float)(bb->raw.x - bb->cal_17.x) / (float)(bb->cal_34.x - bb->cal_17.x)); \
+ } while(0)
+void balance_board_event(struct balance_board_t* bb, byte* msg) {
+ bb->raw.tr = (msg[0] << 8) + msg[1];
+ bb->raw.br = (msg[2] << 8) + msg[3];
+ bb->raw.tl = (msg[4] << 8) + msg[5];
+ bb->raw.bl = (msg[6] << 8) + msg[7];
+ set_bbval(tr);
+ set_bbval(br);
+ set_bbval(tl);
+ set_bbval(bl);
+}
+
View
@@ -0,0 +1,53 @@
+/*
+ * wiiuse
+ *
+ * Written By:
+ * Michael Laforest < para >
+ * Email: < thepara (--AT--) g m a i l [--DOT--] com >
+ * This file:
+ * Bertho Stultiens < para >
+ *
+ * Copyright 2009
+ *
+ * This file is part of wiiuse.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * $Header$
+ *
+ */
+
+/**
+ * @file
+ * @brief Balance Board expansion device.
+ */
+
+#ifndef BALANCE_BOARD_H_INCLUDED
+#define BALANCE_BOARD_H_INCLUDED
+
+#include "wiiuse_internal.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int balance_board_handshake(struct wiimote_t* wm, struct balance_board_t* bb, byte* data, unsigned short len);
+void balance_board_disconnected(struct balance_board_t* bb);
+void balance_board_event(struct balance_board_t* bb, byte* msg);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BALANCE_BOARD_H_INCLUDED
View
@@ -56,6 +56,7 @@
#include "nunchuk.h"
#include "classic.h"
#include "guitar_hero_3.h"
+#include "balance_board.h"
#include "events.h"
static void idle_cycle(struct wiimote_t* wm);
@@ -611,6 +612,9 @@ static void handle_expansion(struct wiimote_t* wm, byte* msg) {
case EXP_GUITAR_HERO_3:
guitar_hero_3_event(&wm->exp.gh3, msg);
break;
+ case EXP_BALANCE_BOARD:
+ balance_board_event(&wm->exp.bb, msg);
+ break;
default:
break;
}
@@ -680,6 +684,12 @@ void handshake_expansion(struct wiimote_t* wm, byte* data, unsigned short len) {
wm->event = WIIUSE_GUITAR_HERO_3_CTRL_INSERTED;
break;
}
+ case EXP_ID_CODE_BALANCE_BOARD:
+ {
+ if (balance_board_handshake(wm, &wm->exp.bb, data, len))
+ wm->event = WIIUSE_BALANCE_BOARD_CTRL_INSERTED;
+ break;
+ }
default:
{
WIIUSE_WARNING("Unknown expansion type. Code: 0x%x", id);
@@ -720,6 +730,10 @@ void disable_expansion(struct wiimote_t* wm) {
guitar_hero_3_disconnected(&wm->exp.gh3);
wm->event = WIIUSE_GUITAR_HERO_3_CTRL_REMOVED;
break;
+ case EXP_BALANCE_BOARD:
+ balance_board_disconnected(&wm->exp.bb);
+ wm->event = WIIUSE_BALANCE_BOARD_CTRL_REMOVED;
+ break;
default:
break;
}
@@ -771,6 +785,9 @@ static void save_state(struct wiimote_t* wm) {
wm->lstate.exp_btns = wm->exp.gh3.btns;
break;
+ case EXP_BALANCE_BOARD:
+ wm->lstate.exp_bb_raw = wm->exp.bb.raw;
+
case EXP_NONE:
break;
}
@@ -866,6 +883,14 @@ static int state_changed(struct wiimote_t* wm) {
STATE_CHANGED(wm->lstate.exp_btns, wm->exp.gh3.btns);
break;
}
+ case EXP_BALANCE_BOARD:
+ {
+ STATE_CHANGED(wm->lstate.exp_bb_raw.tr, wm->exp.bb.raw.tr);
+ STATE_CHANGED(wm->lstate.exp_bb_raw.br, wm->exp.bb.raw.br);
+ STATE_CHANGED(wm->lstate.exp_bb_raw.tl, wm->exp.bb.raw.tl);
+ STATE_CHANGED(wm->lstate.exp_bb_raw.bl, wm->exp.bb.raw.bl);
+ break;
+ }
case EXP_NONE:
{
break;
View
@@ -623,11 +623,10 @@ static void reorder_ir_dots(struct ir_dot_t* dot) {
dot[i].order = 0;
for (order = 1; order < 5; ++order) {
- i = 0;
-
- for (; !dot[i].visible || dot[i].order; ++i)
- if (i > 4)
+ for (i = 0; i < 4 && (!dot[i].visible || dot[i].order); ++i) {
+ if (i >= 4)
return;
+ }
for (j = 0; j < 4; ++j) {
if (dot[j].visible && !dot[j].order && (dot[j].x < dot[i].x))
Oops, something went wrong.

0 comments on commit cefbef2

Please sign in to comment.