1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
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) {
}
}
}
|
