initial commit

1 files changed, 321 insertions, 0 deletions

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;
+			}
+		}
+	}
+}
+