Skip to content
Browse files

initial commit

  • Loading branch information...
0 parents commit dfde95d0fc545ae938ae311c073334ade0707ecb @derf committed Dec 15, 2011
Showing with 475 additions and 0 deletions.
  1. +3 −0 .gitignore
  2. +18 −0 Makefile
  3. +133 −0 bal.c
  4. +321 −0 wibble.c
3 .gitignore
@@ -0,0 +1,3 @@
+/bal
+/mplog
+/wibble
18 Makefile
@@ -0,0 +1,18 @@
+CFLAGS = -Wall -Wextra -pedantic
+LDFLAGS = -lbluetooth -lcwiid
+
+all: bal mplog wibble
+
+bal: bal.c
+ ${CC} ${CFLAGS} ${LDFLAGS} -o $@ $<
+
+mplog: mplog.c
+ $(CC) ${CFLAGS} ${LDFLAGS} -lm -o $@ $<
+
+wibble: wibble.c
+ $(CC) ${CFLAGS} ${LDFLAGS} -lm -o $@ $<
+
+clean:
+ rm -f bal mplog wibble
+
+.PHONY: all clean
133 bal.c
@@ -0,0 +1,133 @@
+/*
+ * Copyright (C) 2011 by Daniel Friesel <derf@finalrewind.org>
+ * License: WTFPL <http://sam.zoy.org/wtfpl>
+ * 0. You just DO WHAT THE FUCK YOU WANT TO.
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <time.h>
+#include <math.h>
+#include <bluetooth/bluetooth.h>
+#include <cwiid.h>
+cwiid_mesg_callback_t cwiid_callback;
+
+
+struct balance_cal balance_cal;
+
+/* reported calibration:
+ * left top 16321 18090 19865
+ * right top 6642 8409 10177
+ * left bot 3113 4888 6668
+ * right bot 8494 10223 11951
+ */
+double weight(uint16_t reading, uint16_t cal[3])
+{
+/* if (reading < cal[0])
+ return 0.0;
+*/ if (reading < cal[1])
+ return ((double)reading - cal[0]) / (cal[1] - cal[0]) * 17.0;
+ else
+ return (((double)reading - cal[1]) / (cal[2] - cal[1]) * 17.0) + 17.0;
+}
+
+int main()
+{
+ cwiid_wiimote_t *wiimote = NULL;
+
+ struct cwiid_state state;
+
+ double wlt, wrt, wlb, wrb;
+ double bal_x, bal_y;
+
+ if ((wiimote = cwiid_open(BDADDR_ANY, 0)) == NULL) {
+ fputs("Unable to connect\n", stderr);
+ return EXIT_FAILURE;
+ }
+ fputs("connected\n", stdout);
+
+ sleep(2);
+
+ if (cwiid_set_led(wiimote, 1))
+ fputs("Unable to set LED state\n", stderr);
+
+ if (cwiid_get_balance_cal(wiimote, &balance_cal))
+ fputs("unable to retrieve balance calibration\n", stderr);
+
+ printf("bcal %d/%d/%d %d/%d/%d\n %d/%d/%d %d/%d/%d\n",
+ balance_cal.left_top[0],
+ balance_cal.left_top[1],
+ balance_cal.left_top[2],
+ balance_cal.right_top[0],
+ balance_cal.right_top[1],
+ balance_cal.right_top[2],
+ balance_cal.left_bottom[0],
+ balance_cal.left_bottom[1],
+ balance_cal.left_bottom[2],
+ balance_cal.right_bottom[0],
+ balance_cal.right_bottom[1],
+ balance_cal.right_bottom[2]
+ );
+
+ if (!cwiid_get_state(wiimote, &state))
+ printf("battery at %d%%\n",
+ (int)(100.0 * state.battery / CWIID_BATTERY_MAX));
+
+ if (cwiid_set_mesg_callback(wiimote, cwiid_callback))
+ fputs("cannot set callback. buttons won't work.\n", stderr);
+
+ if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC))
+ fputs("cannot enable callback. buttons won't work.\n", stderr);
+
+ if (cwiid_set_rpt_mode(wiimote,
+ CWIID_RPT_ACC | CWIID_RPT_STATUS | CWIID_RPT_EXT))
+ fputs("cannot set report mode. buttons won't work.\n", stderr);
+
+ while (1) {
+ cwiid_get_state(wiimote, &state);
+
+ wlt = weight(state.ext.balance.left_top, balance_cal.left_top);
+ wrt = weight(state.ext.balance.right_top, balance_cal.right_top);
+ wlb = weight(state.ext.balance.left_bottom, balance_cal.left_bottom);
+ wrb = weight(state.ext.balance.right_bottom, balance_cal.right_bottom);
+
+ bal_x = (wrt + wrb) / (wlt + wlb);
+ if (bal_x > 1)
+ bal_x = ((wlt + wlb) / (wrt + wrb) * (-1.0)) + 1.0;
+ else
+ bal_x -= 1;
+
+ bal_y = (wlt + wrt) / (wlb + wrb);
+ if (bal_y > 1)
+ bal_y = ((wlb + wrb) / (wlt + wrt) * (-1.0)) + 1.0;
+ else
+ bal_y -= 1;
+
+ printf("%6.1f kg %6.1f kg %04x %04x (%5.1f kg)\n%6.1f kg %6.1f kg %04x %04x\n\n",
+ wlt, wrt,
+ state.ext.balance.left_top, state.ext.balance.right_top,
+ wlt + wrt + wlb + wrb,
+ wlb, wrb,
+ state.ext.balance.left_bottom, state.ext.balance.right_bottom
+ );
+ printf("balance %6f %6f\n\n", bal_x, bal_y);
+ sleep(1);
+ }
+
+ return EXIT_SUCCESS;
+}
+
+/* 97 .. 122 .. 150 */
+/* mp: normal 8200 .. 8250? */
+
+void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
+ union cwiid_mesg mesg[], struct timespec *ts)
+{
+ for (int i = 0; i < mesg_count; i++) {
+ if (mesg[i].type == CWIID_MESG_BALANCE) {
+ }
+
+ }
+}
+
321 wibble.c
@@ -0,0 +1,321 @@
+/*
+ * Copyright (C) 2011 by Daniel Friesel <derf@finalrewind.org>
+ * License: WTFPL <http://sam.zoy.org/wtfpl>
+ * 0. You just DO WHAT THE FUCK YOU WANT TO.
+ */
+
+/*
+ * wibble: interactive wiimote blinkenlights with wobble.
+ *
+ * Usage: start wibble, put wiimote in discoverable mode, see blinkenlights
+ *
+ * Controls:
+ * D-Pad Up toggle auto / manual mode
+ * D-Pad Left previous animation
+ * D-Pad Right next animation
+ * D-Pad Down toggle rumble
+ *
+ * - decrease animation speed
+ * + increase animation speed
+ *
+ * Home quit
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <time.h>
+#include <math.h>
+#include <bluetooth/bluetooth.h>
+#include <cwiid.h>
+
+#define MAX_BRIGHTNESS 10
+#define X_MAXP 19
+#define X_MAXS 20
+#define X_MAXB 36
+#define X_MAX 36
+
+cwiid_mesg_callback_t cwiid_callback;
+
+volatile char rumble = 0;
+volatile char auto_rumble = 1;
+
+volatile char auto_mode = 1;
+volatile int cur_mode = 0;
+
+volatile int8_t cnt_max = 7;
+volatile uint8_t f_led[4][X_MAX];
+volatile uint8_t x_max = X_MAXP;
+
+struct acc_cal wm_cal;
+
+const uint8_t stevens_io[X_MAX] = {
+ 0, 0, 1, 1, 1, 3, 5, 7, 9, 10, 9, 7, 5, 3, 1, 1, 1, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+};
+
+const uint8_t invb[X_MAX] = {
+ 0, 0, 0, 0, 0, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+};
+
+void set_led_fun(int new_mode)
+{
+ const int max_current = 5;
+ int i;
+
+ cur_mode = new_mode;
+ if (cur_mode < 0)
+ cur_mode = max_current;
+ else if (cur_mode > max_current)
+ cur_mode = 0;
+
+ switch (cur_mode) {
+ case 0:
+ x_max = X_MAXP;
+ for (i = 0; i < x_max; i++) {
+ f_led[0][i] = floor( 5 * ( cos( (float) (i + 0) / 3 ) + 1 ) );
+ f_led[1][i] = floor( 5 * ( cos( (float) (i + 3) / 3 ) + 1 ) );
+ f_led[2][i] = floor( 5 * ( cos( (float) (i + 6) / 3 ) + 1 ) );
+ f_led[3][i] = floor( 5 * ( cos( (float) (i + 9) / 3 ) + 1 ) );
+ }
+ break;
+ case 1:
+ x_max = X_MAXP;
+ for (i = 0; i < x_max; i++) {
+ f_led[0][i] = floor( 5 * ( cos( (float) (i + 9) / 3 ) + 1 ) );
+ f_led[1][i] = floor( 5 * ( cos( (float) (i + 6) / 3 ) + 1 ) );
+ f_led[2][i] = floor( 5 * ( cos( (float) (i + 3) / 3 ) + 1 ) );
+ f_led[3][i] = floor( 5 * ( cos( (float) (i + 0) / 3 ) + 1 ) );
+ }
+ break;
+ case 2:
+ x_max = X_MAXP;
+ for (i = 0; i < x_max; i++)
+ f_led[0][i] = f_led[1][i] = f_led[2][i] = f_led[3][i]
+ = stevens_io[i];
+ break;
+ case 3:
+ x_max = X_MAXB;
+ for (i = 0; i < x_max; i++) {
+ f_led[0][i] = f_led[1][i] = f_led[2][i] = f_led[3][i] = 0;
+ if ((i%2) && (i<6))
+ f_led[0][i] = MAX_BRIGHTNESS;
+ if ((i%2) && (((i>=6) && (i<12)) || (i>=30)))
+ f_led[1][i] = MAX_BRIGHTNESS;
+ if ((i%2) && (((i>=12) && (i<18)) || ((i>=24) && (i<30))))
+ f_led[2][i] = MAX_BRIGHTNESS;
+ if ((i%2) && (i>=18) && (i<24))
+ f_led[3][i] = MAX_BRIGHTNESS;
+ }
+ break;
+ case 4:
+ x_max = X_MAXS;
+ for (i = 0; i < x_max; i++) {
+ f_led[0][i] = invb[(i + 15) % x_max];
+ f_led[1][i] = invb[(i + 10) % x_max];
+ f_led[2][i] = invb[(i + 5) % x_max];
+ f_led[3][i] = invb[i];
+ }
+ break;
+ case 5:
+ x_max = X_MAXS;
+ for (i = 0; i < x_max; i++)
+ f_led[0][i] = f_led[1][i] = f_led[2][i] = f_led[3][i]
+ = (i % 2) * 10;
+ break;
+ }
+}
+
+int main()
+{
+ cwiid_wiimote_t *wiimote = NULL;
+
+ struct cwiid_state state;
+
+ uint8_t ledstate = 0x0f;
+
+ uint8_t cnt = 0;
+ uint8_t led[4] = {0, 0, 0, 0};
+
+ uint8_t step = 0;
+ uint8_t x = 0;
+
+ uint8_t i;
+
+ uint8_t next_mode = 0;
+
+ if ((wiimote = cwiid_open(BDADDR_ANY, 0)) == NULL) {
+ fputs("Unable to connect\n", stderr);
+ return EXIT_FAILURE;
+ }
+ fputs("connected\n", stdout);
+
+ sleep(2);
+
+ set_led_fun(0);
+
+ if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &wm_cal))
+ fputs("unable to retrieve accelerometer calibration\n", stderr);
+
+ if (!cwiid_get_state(wiimote, &state))
+ printf("battery at %d%%\n",
+ (int)(100.0 * state.battery / CWIID_BATTERY_MAX));
+
+ if (cwiid_set_mesg_callback(wiimote, cwiid_callback))
+ fputs("cannot set callback. buttons won't work.\n", stderr);
+
+/* cwiid_enable(wiimote, CWIID_FLAG_MOTIONPLUS);
+*/
+ if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC))
+ fputs("cannot enable callback. buttons won't work.\n", stderr);
+
+ if (cwiid_set_rpt_mode(wiimote,
+ CWIID_RPT_BTN | CWIID_RPT_ACC | CWIID_RPT_STATUS | CWIID_RPT_EXT))
+ fputs("cannot set report mode. buttons won't work.\n", stderr);
+
+ while (1) {
+
+ if (++cnt >= cnt_max) {
+ cnt = 0;
+
+ if (++x == x_max) {
+ x = 0;
+
+ if (!auto_mode && (++next_mode == 42)) {
+ set_led_fun(cur_mode + 1);
+ next_mode = 0;
+ }
+ }
+
+ for (i = 0; i < 4; i++)
+ led[i] = f_led[i][x];
+ }
+
+ step = cnt % MAX_BRIGHTNESS;
+
+ if (step == 0)
+ ledstate = 0x0f;
+
+ for (i = 0; i < 4; i++)
+ if (step == led[i])
+ ledstate &= ~(1 << i);
+
+ if (cwiid_set_led(wiimote, ledstate))
+ fputs("Error setting LED state\n", stderr);
+ }
+
+ return EXIT_SUCCESS;
+}
+
+/* 97 .. 122 .. 150 */
+/* mp: normal 8200 .. 8250? */
+
+void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
+ union cwiid_mesg mesg[], struct timespec *ts)
+{
+ static double ff_start, ff_diff;
+ static double fp_start, fp_diff;
+ double a_x, a_y, a_z, accel;
+ struct cwiid_acc_mesg *am;
+
+ for (int i = 0; i < mesg_count; i++) {
+ if (mesg[i].type == CWIID_MESG_BTN) {
+
+ if (mesg[i].btn_mesg.buttons & CWIID_BTN_LEFT)
+ set_led_fun(cur_mode - 1);
+ if (mesg[i].btn_mesg.buttons & CWIID_BTN_RIGHT)
+ set_led_fun(cur_mode + 1);
+ if (mesg[i].btn_mesg.buttons & CWIID_BTN_PLUS)
+ cnt_max -= (cnt_max > 1 ? 1 : 0);
+ if (mesg[i].btn_mesg.buttons & CWIID_BTN_MINUS)
+ cnt_max += 1;
+ if (mesg[i].btn_mesg.buttons & CWIID_BTN_UP) {
+ auto_mode = !auto_mode;
+ if (auto_mode)
+ x_max = X_MAXP;
+ }
+
+ if (mesg[i].btn_mesg.buttons & CWIID_BTN_DOWN) {
+ if (auto_mode)
+ auto_rumble = !auto_rumble;
+ else if (!rumble)
+ cwiid_set_rumble(wiimote, (rumble = 1));
+ }
+ else if (rumble && !auto_mode)
+ cwiid_set_rumble(wiimote, (rumble = 0));
+
+ if (mesg[i].btn_mesg.buttons & CWIID_BTN_HOME) {
+ exit(0);
+ }
+
+ }
+/* else if ((mesg[i].type == CWIID_MESG_MOTIONPLUS) && auto_mode ) {
+
+ if ((mesg[i].motionplus_mesg.angle_rate[2] < 8000) && !fp_start)
+ fp_start = ((uint64_t)ts->tv_sec * 1000000000) + ts->tv_nsec;
+ else if ((mesg[i].motionplus_mesg.angle_rate[2] > 8200) && fp_start) {
+ fp_diff = ((((uint64_t)ts->tv_sec * 1000000000)
+ + ts->tv_nsec
+ - ff_start) / 1000000000);
+
+ printf("mpdel_t %.3fs - Fell approx. %.2fm\n", ff_diff,
+ (double)((9.81 * (double)(fp_diff) * (double)(fp_diff)) / (double)2));
+ fp_start = 0;
+ }
+ }
+*/ else if ((mesg[i].type == CWIID_MESG_ACC) && auto_mode) {
+
+ am = &mesg[i].acc_mesg;
+
+ a_x = ((double)am->acc[CWIID_X] - wm_cal.zero[CWIID_X]) /
+ (wm_cal.one[CWIID_X] - wm_cal.zero[CWIID_X]);
+ a_y = ((double)am->acc[CWIID_Y] - wm_cal.zero[CWIID_Y]) /
+ (wm_cal.one[CWIID_Y] - wm_cal.zero[CWIID_Y]);
+ a_z = ((double)am->acc[CWIID_Z] - wm_cal.zero[CWIID_Z]) /
+ (wm_cal.one[CWIID_Z] - wm_cal.zero[CWIID_Z]);
+ accel = sqrt(pow(a_x,2)+pow(a_y,2)+pow(a_z,2));
+
+ if ((accel < 0.07) && !ff_start)
+ ff_start = ((uint64_t)ts->tv_sec * 1000000000) + ts->tv_nsec;
+ else if ((accel > 1.0) && ff_start) {
+ ff_diff = ((((uint64_t)ts->tv_sec * 1000000000)
+ + ts->tv_nsec
+ - ff_start) / 1000000000);
+
+ printf("delta_t %.3fs - Fell approx. %.2fm\n", ff_diff,
+ (double)((9.81 * (double)(ff_diff) * (double)(ff_diff)) / (double)2));
+ ff_start = 0;
+ }
+
+ if (auto_rumble && (accel > 1.5)) {
+ if (!rumble)
+ cwiid_set_rumble(wiimote, (rumble = 1));
+ }
+ else {
+ if (rumble)
+ cwiid_set_rumble(wiimote, (rumble = 0));
+ }
+
+ if (mesg[i].acc_mesg.acc[CWIID_X] < 123) {
+
+ if (cur_mode != 0)
+ set_led_fun(0);
+
+ cnt_max = (mesg[i].acc_mesg.acc[CWIID_X] - 95) / 2;
+ if (cnt_max < 2)
+ cnt_max = 2;
+ }
+ else {
+
+ if (cur_mode != 1)
+ set_led_fun(1);
+
+ cnt_max = (150 - mesg[i].acc_mesg.acc[CWIID_X]) / 2;
+ if (cnt_max < 2)
+ cnt_max = 2;
+ }
+ }
+ }
+}
+

0 comments on commit dfde95d

Please sign in to comment.
Something went wrong with that request. Please try again.