initial commit

4 files changed, 475 insertions, 0 deletions

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..f3ab4a5
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+/bal
+/mplog
+/wibble
diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..1f10491
--- /dev/null
+++ b/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
diff --git a/bal.c b/bal.c
new file mode 100644
index 0000000..f7559ad
--- /dev/null
+++ b/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) {
+		}
+
+	}
+}
+
diff --git a/wibble.c b/wibble.c
new file mode 100644
index 0000000..f1cdc10
--- /dev/null
+++ b/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;
+			}
+		}
+	}
+}
+