initial commit

1 files changed, 133 insertions, 0 deletions

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) {
+		}
+
+	}
+}
+