int wii_open()
{
bdaddr = *BDADDR_ANY;
accmode = WII_ACCMODE_BTN;
as = 0;
speedacc = 0;
gettimeofday(&t1,NULL);
printf ( "N: wii: trying to find wiimote...\n" );
if (!(wiimote = cwiid_open(&bdaddr, 0))) {
fprintf(stderr, "E: wii: Unable to connect to wiimote\n");
return -1;
}
printf ("N: wii: open\n" );
if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) {
fprintf(stderr, "E: wii: Unable to set message callback\n");
cwiid_close(wiimote);
return -1;
}
printf ("N: wii: callback\n" );
if (cwiid_set_led(wiimote, 0x02)) {
fprintf(stderr, "Error setting LEDs \n");
cwiid_close(wiimote);
return -1;
}
printf ("N: wii: led\n" );
if (cwiid_set_rpt_mode(wiimote, CWIID_RPT_BTN | CWIID_RPT_ACC)) {
fprintf(stderr, "E: wii: could not set report mode\n");
cwiid_close(wiimote);
return -1;
}
printf ("N: wii: report mode set\n" );
if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC | CWIID_FLAG_CONTINUOUS)) {
fprintf(stderr, "E: wii: could not enable reporting\n");
cwiid_close(wiimote);
return -1;
}
printf ("N: wii: reporting enabled\n" );
wii_connected = 1;
return 0;
}
int32_t shutdown_all(cwiid_wiimote_t *wiimote)
{
stop_motors();
set_ir_led(false);
write_status_led(STATUS_LED_OFF, 0);
return cwiid_close(wiimote);
}
void Wiimote::StartReporting(uint8_t aReportMode)
{
ULOG_DEBUG_F();
if (cwiid_set_mesg_callback(mWiimote, &Wiimote::CWiidCallback)) {
ULOG_DEBUG_F("Error setting callback");
if (cwiid_close(mWiimote)) {
ULOG_DEBUG_F("Error on disconnect");
}
mWiimote = NULL;
NotifyConnectionStatus(IWiimoteObserver::EConnectionError);
NotifyConnectionStatus(IWiimoteObserver::ENotConnected);
} else {
ULOG_DEBUG_F("Connected");
if (cwiid_get_acc_cal(mWiimote, CWIID_EXT_NONE, &mWmCal)) {
ULOG_DEBUG_F("Unable to retrieve accelerometer calibration");
}
NotifyConnectionStatus(IWiimoteObserver::EConnected);
/*
set_gui_state();
set_report_mode();
cwiid_enable(wiimote, CWIID_FLAG_MOTIONPLUS);
*/
//uint8_t rpt_mode;
//rpt_mode = CWIID_RPT_STATUS | CWIID_RPT_BTN | CWIID_RPT_ACC;
if (cwiid_set_rpt_mode(mWiimote, aReportMode)) {
ULOG_DEBUG_F("Error setting report mode");
}
cwiid_request_status(mWiimote);
}
}
static void Wiimote_dealloc(Wiimote *self)
{
if (self->close_on_dealloc && self->wiimote) {
cwiid_close(self->wiimote);
}
Py_XDECREF(self->callback);
self->ob_type->tp_free((PyObject *)self);
}
int wii_close()
{
wii_connected = 0;
if (cwiid_close(wiimote)) {
fprintf(stderr, "E: wii: Error on wiimote disconnect\n");
return -1;
}
return 0;
}
void Wiimote::Disconnect()
{
if (mWiimote) {
if (cwiid_close(mWiimote)) {
ULOG_DEBUG_F("Error on disconnect");
}
NotifyConnectionStatus(IWiimoteObserver::ENotConnected);
}
}
void
WiimoteController::disconnect()
{
if (m_wiimote)
{
cwiid_close(m_wiimote);
m_wiimote = 0;
}
}
void menuDisconnect_activate(void)
{
if (cwiid_close(wiimote)) {
message(GTK_MESSAGE_ERROR, "Error on disconnect", GTK_WINDOW(winMain));
}
wiimote = NULL;
status("No connection");
clear_widgets();
set_gui_state();
}
static PyObject *Wiimote_close(Wiimote *self)
{
if (cwiid_close(self->wiimote)) {
PyErr_SetString(PyExc_RuntimeError,
"Error closing wiimote connection");
self->wiimote = NULL;
return NULL;
}
self->wiimote = NULL;
Py_RETURN_NONE;
}
static void wiimote_doDisconnect(t_wiimote *x)
{
if (x->connected)
{
if (cwiid_close(x->wiimote)) {
pd_error(x, "wiimote: unable to close connection.");
}
else {
post("disconnect successfull, resetting values");
removeWiimoteObject(x);
x->connected = 0;
}
} else {
post("device is not connected");
}
wiimote_out_status(x, x->connected);
}
void menuConnect_activate(void)
{
char reset_bdaddr = 0;
if (bacmp(&bdaddr, BDADDR_ANY) == 0) {
reset_bdaddr = 1;
}
message(GTK_MESSAGE_INFO,
"Put Wiimote in discoverable mode (press 1+2) and press OK",
GTK_WINDOW(winMain));
if ((wiimote = cwiid_open(&bdaddr, CWIID_FLAG_MESG_IFC)) == NULL) {
message(GTK_MESSAGE_ERROR, "Unable to connect", GTK_WINDOW(winMain));
status("No connection");
}
else if (cwiid_set_mesg_callback(wiimote, &cwiid_callback)) {
message(GTK_MESSAGE_ERROR, "Error setting callback",
GTK_WINDOW(winMain));
if (cwiid_close(wiimote)) {
message(GTK_MESSAGE_ERROR, "Error on disconnect",
GTK_WINDOW(winMain));
}
wiimote = NULL;
status("No connection");
}
else {
status("Connected");
if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &wm_cal)) {
message(GTK_MESSAGE_ERROR, "Unable to retrieve accelerometer "
"calibration", GTK_WINDOW(winMain));
}
set_gui_state();
set_report_mode();
cwiid_enable(wiimote, CWIID_FLAG_MOTIONPLUS);
cwiid_request_status(wiimote);
}
if (reset_bdaddr) {
bdaddr = *BDADDR_ANY;
}
}
// The following function attempts to connect to a wiimote at a
// specific address, provided as an argument. eg, 00:19:1D:70:CE:72
// This address can be discovered by running the following command
// in a console:
// hcitool scan | grep Nintendo
static void wiimote_doConnect(t_wiimote *x, t_symbol *addr, t_symbol *dongaddr)
{
bdaddr_t bdaddr;
unsigned int flags = CWIID_FLAG_MESG_IFC;
bdaddr_t dong_bdaddr;
bdaddr_t* dong_bdaddr_ptr=&dong_bdaddr;
if(x->connected) {
wiimote_doDisconnect(x);
}
// determine address:
if (NULL==addr || addr==gensym("")) {
verbose(1, "searching for wii...");
bdaddr = *BDADDR_ANY;
} else {
str2ba(addr->s_name, &bdaddr);
verbose(1, "Connecting to Wii '%s'", addr->s_name);
}
post("Press buttons 1 and 2 simultaneously.");
// determine dongleaddress:
if (NULL==dongaddr || dongaddr==gensym("")) {
verbose(1, "using default dongle");
dong_bdaddr_ptr = NULL;
} else {
verbose(1, "using dongle '%s'", dongaddr->s_name);
str2ba(dongaddr->s_name, &dong_bdaddr);
}
// connect:
#ifdef CWIID_OPEN_WITH_DONGLE
verbose(1,"wiimote: opening multidongle");
x->wiimote = cwiid_open(&bdaddr, dong_bdaddr_ptr, flags);
#else
verbose(1,"wiimote: opening");
x->wiimote = cwiid_open(&bdaddr, flags);
#endif
if(NULL==x->wiimote) {
pd_error(x, "wiimote: unable to connect");
wiimote_out_status(x, x->connected);
return;
}
if(!addWiimoteObject(x, cwiid_get_id(x->wiimote))) {
cwiid_close(x->wiimote);
x->wiimote=NULL;
wiimote_out_status(x, x->connected);
return;
}
x->wiimoteID= cwiid_get_id(x->wiimote);
post("wiimote %i is successfully connected", x->wiimoteID);
if(cwiid_get_acc_cal(x->wiimote, CWIID_EXT_NONE, &x->acc_cal)) {
post("Unable to retrieve accelerometer calibration");
} else {
post("Retrieved wiimote calibration: zero=(%02d,%02d,%02d) one=(%02d,%02d,%02d)",
x->acc_cal.zero[CWIID_X],
x->acc_cal.zero[CWIID_Y],
x->acc_cal.zero[CWIID_Z],
x->acc_cal.one [CWIID_X],
x->acc_cal.one [CWIID_Y],
x->acc_cal.one [CWIID_Z]);
}
x->connected = 1;
wiimote_out_status(x, x->connected);
x->reportMode |= CWIID_RPT_STATUS;
x->reportMode |= CWIID_RPT_BTN;
wiimote_resetReportMode(x);
if (cwiid_set_mesg_callback(x->wiimote, &cwiid_callback)) {
pd_error(x, "Unable to set message callback");
}
wiimote_setbasetime(x);
}
/* cwiid_mesg_callback_t has undergone a few changes lately, hopefully this
* will be the last. Some programs need to know which messages were received
* simultaneously (e.g. for correlating accelerometer and IR data), and the
* sequence number mechanism used previously proved cumbersome, so we just
* pass an array of messages, all of which were received at the same time.
* The id is to distinguish between multiple wiimotes using the same callback.
* */
void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
union cwiid_mesg mesg[], struct timespec *timestamp)
{
int i;
//int valid_source;
for (i=0; i < mesg_count; i++)
{
switch (mesg[i].type) {
case CWIID_MESG_STATUS:
printf("Status Report: battery=%d extension=",
mesg[i].status_mesg.battery);
switch (mesg[i].status_mesg.ext_type) {
case CWIID_EXT_NONE:
printf("none");
break;
case CWIID_EXT_NUNCHUK:
printf("Nunchuk");
break;
case CWIID_EXT_CLASSIC:
printf("Classic Controller");
break;
default:
printf("Unknown Extension");
break;
}
printf("\n");
break;
case CWIID_MESG_BTN:
printf("Button Report: %.4X\n", mesg[i].btn_mesg.buttons);
button_state = mesg[i].btn_mesg.buttons;
break;
case CWIID_MESG_ACC:
if (button_state & 0x0004) {
struct timeval now;
gettimeofday(&now, NULL);
printf("Acc Report: x=%d, y=%d, z=%d %ld %ld\n",
mesg[i].acc_mesg.acc[CWIID_X],
mesg[i].acc_mesg.acc[CWIID_Y],
mesg[i].acc_mesg.acc[CWIID_Z],
now.tv_sec, now.tv_usec);
}
break;
case CWIID_MESG_IR:
printf("IR Report: elided");
printf("\n");
break;
case CWIID_MESG_NUNCHUK:
printf("Nunchuk Report elided\n");
break;
case CWIID_MESG_CLASSIC:
printf("Classic Report: btns=%.4X l_stick=(%d,%d) r_stick=(%d,%d) "
"l=%d r=%d\n", mesg[i].classic_mesg.buttons,
mesg[i].classic_mesg.l_stick[CWIID_X],
mesg[i].classic_mesg.l_stick[CWIID_Y],
mesg[i].classic_mesg.r_stick[CWIID_X],
mesg[i].classic_mesg.r_stick[CWIID_Y],
mesg[i].classic_mesg.l, mesg[i].classic_mesg.r);
break;
case CWIID_MESG_ERROR:
if (cwiid_close(wiimote)) {
fprintf(stderr, "Error on wiimote disconnect\n");
exit(-1);
}
exit(0);
break;
default:
printf("Unknown Report");
break;
}
}
}
/* call to shutdown the wiimote device
* typically called once at close
* turns the IR leds off
* can be used to deactivate the wiimote;
* must call init to restart
* a return value < 0 indicates error */
int ltr_int_tracker_close() {
if (gWiimote) cwiid_close(gWiimote);
return 0;
}
int main(int argc, char** argv) {
cwiid_wiimote_t *wiimote;
struct cwiid_state state;
bdaddr_t bdaddr;
int exit = 0;
point3Df dimensions3PtsCap[3];
bdaddr = *BDADDR_ANY;
printf("connecting!\n");
if (!(wiimote = cwiid_open(&bdaddr, 0))) {
fprintf(stderr, "Unable to connect to wiimote\n");
return -1;
}
printf("connected\n");
cwiid_set_led(wiimote, CWIID_LED1_ON | CWIID_LED4_ON);
cwiid_set_rpt_mode(wiimote, CWIID_RPT_STATUS | CWIID_RPT_IR);
//*
if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) {
fprintf(stderr, "Unable to set message callback\n");
}
if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) {
fprintf(stderr, "Error enabling messages\n");
}
// */
dimensions3PtsCap[0].x = 70;
dimensions3PtsCap[0].y = 80;
dimensions3PtsCap[0].z = 100;
Initialize3PCapModel(dimensions3PtsCap);
while (!exit) {
switch(getchar()) {
case 'q':
exit = 1;
break;
default:
if (cwiid_get_state(wiimote, &state)) {
printf("Error getting state\n");
} else {
print_state(&state);
}
}
}
/*
if (getchar()) {
printf("Exiting\n");
}
**/
if (cwiid_close(wiimote)) {
printf("Error on wiimote disconnect\n");
return -1;
}
return (EXIT_SUCCESS);
}
int main(int argc, char *argv[])
{
char wait_forever = 0, quiet = 0, reconnect = 0, reconnect_wait = 0;
char *config_search_dirs[3], *plugin_search_dirs[3];
char *config_filename = DEFAULT_CONFIG_FILE;
char home_config_dir[HOME_DIR_LEN];
char home_plugin_dir[HOME_DIR_LEN];
char *tmp;
const char *dev_name = NULL;
int c, i;
char *str_addr;
bdaddr_t bdaddr, current_bdaddr;
sigset_t sigset;
int signum, ret=0;
struct uinput_listen_data uinput_listen_data;
pthread_t uinput_listen_thread;
init = 1;
/* Parse Options */
while (1) {
int option_index = 0;
static struct option long_options[] = {
{"help", 0, 0, 'h'},
{"version", 0, 0, 'v'},
{"config", 1, 0, 'c'},
{"daemon", 0, 0, 'd'},
{"quiet", 0, 0, 'q'},
{"reconnect", 2, 0, 'r'},
{"wait", 0, 0, 'w'},
{"name", 1, 0, 'n'},
{0, 0, 0, 0}
};
c = getopt_long (argc, argv, "hvc:dqr::wn:", long_options, &option_index);
if (c == -1) {
break;
}
switch (c) {
case 'h':
print_usage();
return 0;
break;
case 'v':
printf("CWiid Version %s\n", PACKAGE_VERSION);
return 0;
break;
case 'c':
config_filename = optarg;
break;
case 'd':
wait_forever = 1;
quiet = 1;
reconnect = 1;
break;
case 'q':
quiet = 1;
break;
case 'r':
reconnect = 1;
if (optarg) {
reconnect_wait = strtol(optarg, &tmp, 10);
if (*tmp != '\0') {
wminput_err("bad reconnect wait time");
return -1;
}
}
break;
case 'w':
wait_forever = 1;
break;
case 'n':
dev_name = optarg;
break;
case '?':
printf("Try `wminput --help` for more information\n");
return 1;
break;
default:
return -1;
break;
}
}
if (c_init()) {
return -1;
}
#ifdef HAVE_PYTHON
if (py_init()) {
return -1;
}
#endif
/* Load Config */
/* Setup search directory arrays */
if ((tmp = getenv("HOME")) == NULL) {
wminput_err("Unable to find home directory");
config_search_dirs[0] = WMINPUT_CONFIG_DIR;
plugin_search_dirs[0] = CWIID_PLUGINS_DIR;
config_search_dirs[1] = plugin_search_dirs[1] = NULL;
}
else {
snprintf(home_config_dir, HOME_DIR_LEN, "%s/.cwiid/wminput", tmp);
snprintf(home_plugin_dir, HOME_DIR_LEN, "%s/.cwiid/plugins", tmp);
config_search_dirs[0] = home_config_dir;
plugin_search_dirs[0] = home_plugin_dir;
config_search_dirs[1] = WMINPUT_CONFIG_DIR;
plugin_search_dirs[1] = CWIID_PLUGINS_DIR;
config_search_dirs[2] = plugin_search_dirs[2] = NULL;
}
if (conf_load(&conf, config_filename, config_search_dirs,
plugin_search_dirs, dev_name)) {
return -1;
}
/* Determine BDADDR */
/* priority: command-line option, environment variable, BDADDR_ANY */
if (optind < argc) {
if (str2ba(argv[optind], &bdaddr)) {
wminput_err("invalid bdaddr");
bdaddr = *BDADDR_ANY;
}
optind++;
if (optind < argc) {
wminput_err("invalid command-line");
print_usage();
conf_unload(&conf);
return -1;
}
}
else if ((str_addr = getenv(WIIMOTE_BDADDR)) != NULL) {
if (str2ba(str_addr, &bdaddr)) {
wminput_err("invalid address in %s", WIIMOTE_BDADDR);
bdaddr = *BDADDR_ANY;
}
}
else {
bdaddr = *BDADDR_ANY;
}
sigemptyset(&sigset);
sigaddset(&sigset, SIGTERM);
sigaddset(&sigset, SIGINT);
sigaddset(&sigset, SIGUSR1);
do {
bacpy(¤t_bdaddr, &bdaddr);
/* Wiimote Connect */
if (!quiet) {
printf("Put Wiimote in discoverable mode now (press 1+2)...\n");
}
if (wait_forever) {
if (!bacmp(¤t_bdaddr, BDADDR_ANY)) {
if (cwiid_find_wiimote(¤t_bdaddr, -1)) {
wminput_err("error finding wiimote");
conf_unload(&conf);
return -1;
}
}
/* TODO: avoid continuously calling cwiid_open */
cwiid_set_err(cwiid_err_connect);
while (!(wiimote = cwiid_open(¤t_bdaddr, CWIID_FLAG_MESG_IFC)));
cwiid_set_err(cwiid_err_default);
}
else {
if ((wiimote = cwiid_open(¤t_bdaddr, CWIID_FLAG_MESG_IFC)) == NULL) {
wminput_err("unable to connect");
conf_unload(&conf);
return -1;
}
}
if (cwiid_set_mesg_callback(wiimote, &cwiid_callback)) {
wminput_err("error setting callback");
conf_unload(&conf);
return -1;
}
if (c_wiimote(wiimote)) {
conf_unload(&conf);
return -1;
}
#ifdef HAVE_PYTHON
if (py_wiimote(wiimote)) {
conf_unload(&conf);
return -1;
}
#endif
/* init plugins */
for (i=0; (i < CONF_MAX_PLUGINS) && conf.plugins[i].name; i++) {
switch (conf.plugins[i].type) {
case PLUGIN_C:
if (c_plugin_init(&conf.plugins[i], i)) {
wminput_err("error on %s init", conf.plugins[i].name);
conf_unload(&conf);
cwiid_close(wiimote);
return -1;
}
break;
#ifdef HAVE_PYTHON
case PLUGIN_PYTHON:
if (py_plugin_init(&conf.plugins[i], i)) {
wminput_err("error %s init", conf.plugins[i].name);
conf_unload(&conf);
cwiid_close(wiimote);
return -1;
}
break;
#endif
}
}
if (wminput_set_report_mode()) {
conf_unload(&conf);
cwiid_close(wiimote);
return -1;
}
uinput_listen_data.wiimote = wiimote;
uinput_listen_data.conf = &conf;
if (pthread_create(&uinput_listen_thread, NULL,
(void *(*)(void *))uinput_listen,
&uinput_listen_data)) {
wminput_err("error starting uinput listen thread");
conf_unload(&conf);
cwiid_close(wiimote);
return -1;
}
if (!quiet) {
printf("Ready.\n");
}
init = 0;
/* wait */
sigprocmask(SIG_BLOCK, &sigset, NULL);
sigwait(&sigset, &signum);
sigprocmask(SIG_UNBLOCK, &sigset, NULL);
if ((signum == SIGTERM) || (signum == SIGINT)) {
reconnect = 0;
}
if (pthread_cancel(uinput_listen_thread)) {
wminput_err("Error canceling uinput listen thread");
ret = -1;
}
else if (pthread_join(uinput_listen_thread, NULL)) {
wminput_err("Error joining uinput listen thread");
ret = -1;
}
c_wiimote_deinit();
#ifdef HAVE_PYTHON
py_wiimote_deinit();
#endif
/* disconnect */
if (cwiid_close(wiimote)) {
wminput_err("Error on wiimote disconnect");
ret = -1;
}
if (reconnect && reconnect_wait) {
sleep(reconnect_wait);
}
} while (reconnect);
if (conf_unload(&conf)) {
ret = -1;
}
c_deinit();
#ifdef HAVE_PYTHON
py_deinit();
#endif
if (!quiet) {
printf("Exiting.\n");
}
return ret;
}
/* read the usb, and process it into frames
* a return value < 0 indicates error */
int ltr_int_tracker_get_frame(struct camera_control_block *ccb,
struct frame_type *f, bool *frame_acquired)
{
(void) ccb;
struct cwiid_state state;
unsigned int required_blobnum = 3;
unsigned int valid;
int i;
//Otherwise the polling takes too much processor
ltr_int_usleep(8334);
if (!gStateCheckIn--) {
gStateCheckIn = STATE_CHECK_INTERVAL;
if (cwiid_request_status(gWiimote)) {
ltr_int_log_message("Requesting status failed, disconnecting\n");
cwiid_close(gWiimote);
gWiimote = NULL;
return -1;
}
}
if (cwiid_get_state(gWiimote, &state)) {
// Treat connection as disconnected on error
ltr_int_log_message("Error reading wiimote state\n");
cwiid_close(gWiimote);
gWiimote = NULL;
return -1;
}
valid = 0;
for (i=0; i<CWIID_IR_SRC_COUNT; i++) {
if (state.ir_src[i].valid) {
valid++;
}
}
f->width = WIIMOTE_HORIZONTAL_RESOLUTION / 2;
f->height = WIIMOTE_VERTICAL_RESOLUTION / 2;
f->bloblist.num_blobs = valid < required_blobnum ? valid : required_blobnum;
f->bloblist.blobs = (struct blob_type *)
ltr_int_my_malloc(f->bloblist.num_blobs*sizeof(struct blob_type));
assert(f->bloblist.blobs);
bool draw;
if(f->bitmap != NULL){
draw = true;
}else{
draw = false;
}
image_t img = {
.w = WIIMOTE_HORIZONTAL_RESOLUTION / 2,
.h = WIIMOTE_VERTICAL_RESOLUTION / 2,
.bitmap = f->bitmap,
.ratio = 1.0
};
valid = 0;
for (i=0; i<CWIID_IR_SRC_COUNT; i++) {
if (state.ir_src[i].valid) {
if (valid<required_blobnum) {
f->bloblist.blobs[valid].x = -1 * state.ir_src[i].pos[CWIID_X] + WIIMOTE_HORIZONTAL_RESOLUTION/2;
f->bloblist.blobs[valid].y = state.ir_src[i].pos[CWIID_Y] - WIIMOTE_VERTICAL_RESOLUTION/2;
f->bloblist.blobs[valid].score = state.ir_src[i].size;
if(draw){
ltr_int_draw_square(&img, state.ir_src[i].pos[CWIID_X] / 2, (WIIMOTE_VERTICAL_RESOLUTION - state.ir_src[i].pos[CWIID_Y]) / 2, 2*state.ir_src[i].size);
ltr_int_draw_cross(&img, state.ir_src[i].pos[CWIID_X] / 2, (WIIMOTE_VERTICAL_RESOLUTION - state.ir_src[i].pos[CWIID_Y]) / 2, (int)WIIMOTE_HORIZONTAL_RESOLUTION/100.0);
}
}
valid++;
}
}
*frame_acquired = true;
return 0;
}
/* cwiid_mesg_callback_t has undergone a few changes lately, hopefully this
* will be the last. Some programs need to know which messages were received
* simultaneously (e.g. for correlating accelerometer and IR data), and the
* sequence number mechanism used previously proved cumbersome, so we just
* pass an array of messages, all of which were received at the same time.
* The id is to distinguish between multiple wiimotes using the same callback.
* */
void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
union cwiid_mesg mesg[], struct timespec *timestamp)
{
int i;
//int valid_source;
for (i=0; i < mesg_count; i++)
{
switch (mesg[i].type) {
case CWIID_MESG_STATUS:
printf("Status Report: battery=%d extension=",
mesg[i].status_mesg.battery);
switch (mesg[i].status_mesg.ext_type) {
case CWIID_EXT_NONE:
printf("none");
break;
case CWIID_EXT_NUNCHUK:
printf("Nunchuk");
break;
case CWIID_EXT_CLASSIC:
printf("Classic Controller");
break;
default:
printf("Unknown Extension");
break;
}
printf("\n");
break;
case CWIID_MESG_BTN:
printf("Button Report: %.4X\n", mesg[i].btn_mesg.buttons);
button_state = mesg[i].btn_mesg.buttons;
if (button_state == 0 && accs.next) {
dump_acc_stream(&accs);
printf("\n\nDOWNSAMPLED\n");
struct acc_report *ds = downsample_acc_stream(&accs);
dump_acc_stream(ds);
printf("\n\nQUANTIZED\n");
struct observation *obs = run_quantizer(ds);
StateSequenceRef sequence = createStateSequence(obs->sequence, obs->sequence_len);
observation_free(obs); // gets copied
int n_trained_index = n_trained / 3;
if (n_trained == -1) {
printf("GOOD, YOU'VE SYNCED. PROCEEDING TO TRAIN 0.");
n_trained++;
} else if (n_trained_index < n_gestures) {
hmm_train(hmms[n_trained_index], &sequence, 1);
printf("TRAINED %d\n", n_trained_index);
n_trained++;
n_trained_index = n_trained / 3;
if (n_trained_index < n_gestures)
printf("PROCEEDING TO TRAIN %d\n", n_trained_index);
else
printf("DONE TRAINING, WOO. On to classifying.\n");
} else {
printf("\nCLASSIFYING\n");
double max_p = 0.0;
int max_i = 0;
for(int i = 0; i < n_gestures; i++) {
double p = getProbability(hmms[i], sequence);
printf(" P(%d) = %f\n", i, p);
if (p > max_p) {
max_p = p;
max_i = i;
}
}
printf("\n CLASSIFICATION RESULTS: %d with P=%f\n", max_i, max_p);
}
reset_acc_stream(ds);
free(ds);
acc_cursor = reset_acc_stream(&accs);
}
break;
case CWIID_MESG_ACC:
if (button_state & 0x0004) {
struct timeval now;
gettimeofday(&now, NULL);
/*
printf("Acc Report: x=%d, y=%d, z=%d %ld %ld\n",
mesg[i].acc_mesg.acc[CWIID_X],
mesg[i].acc_mesg.acc[CWIID_Y],
mesg[i].acc_mesg.acc[CWIID_Z],
now.tv_sec, now.tv_usec);
*/
acc_cursor = add_acc_report(
acc_cursor,
(signed char)mesg[i].acc_mesg.acc[CWIID_X],
(signed char)mesg[i].acc_mesg.acc[CWIID_Y],
(signed char)mesg[i].acc_mesg.acc[CWIID_Z],
now.tv_sec, now.tv_usec);
}
break;
case CWIID_MESG_IR:
printf("IR Report: elided");
printf("\n");
break;
case CWIID_MESG_NUNCHUK:
printf("Nunchuk Report elided\n");
break;
case CWIID_MESG_CLASSIC:
printf("Classic Report: btns=%.4X l_stick=(%d,%d) r_stick=(%d,%d) "
"l=%d r=%d\n", mesg[i].classic_mesg.buttons,
mesg[i].classic_mesg.l_stick[CWIID_X],
mesg[i].classic_mesg.l_stick[CWIID_Y],
mesg[i].classic_mesg.r_stick[CWIID_X],
mesg[i].classic_mesg.r_stick[CWIID_Y],
mesg[i].classic_mesg.l, mesg[i].classic_mesg.r);
break;
case CWIID_MESG_ERROR:
if (cwiid_close(wiimote)) {
fprintf(stderr, "Error on wiimote disconnect\n");
exit(-1);
}
exit(0);
break;
default:
printf("Unknown Report");
break;
}
}
}
int main(int argc, char *argv[])
{
cwiid_wiimote_t *wiimote; /* wiimote handle */
struct cwiid_state state; /* wiimote state */
bdaddr_t bdaddr; /* bluetooth device address */
unsigned char mesg = 0;
unsigned char led_state = 0;
unsigned char rpt_mode = 0;
unsigned char rumble = 0;
int exit = 0;
deviceState.numButtons = NUM_BUTTONS;
deviceState.numAxis = NUM_AXIS;
deviceState.numSensors = NUM_SENSORS;
deviceState.axisValues = NULL;
deviceState.buttonValues = NULL;
deviceState.sensorValues = new SensorData[NUM_SENSORS];
cwiid_set_err(err);
if (argc < 2) {
fprintf(stderr, "Usage: %s HOSTNAME\n\n", argv[0]);
return 1;
} // if
// TODO: parse data from config file!!!
/* Connect to address given on command-line, if present */
if (argc > 2) {
str2ba(argv[2], &bdaddr);
}
else {
bdaddr = *BDADDR_ANY;
}
/* Connect to the wiimote */
printf("Put Wiimote in discoverable mode now (press 1+2)...\n");
if (!(wiimote = cwiid_open(&bdaddr, 0))) {
fprintf(stderr, "Unable to connect to wiimote\n");
return -1;
}
if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) {
fprintf(stderr, "Unable to set message callback\n");
}
toggle_bit(led_state, CWIID_LED1_ON);
set_led_state(wiimote, led_state);
if (cwiid_get_state(wiimote, &state)) {
fprintf(stderr, "Error getting state\n");
}
print_state(&state);
toggle_bit(rpt_mode, CWIID_RPT_IR);
toggle_bit(rpt_mode, CWIID_RPT_BTN);
// toggle_bit(rpt_mode, CWIID_RPT_ACC);
set_rpt_mode(wiimote, rpt_mode);
if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) {
fprintf(stderr, "Error enabling messages\n");
} else {
mesg = 1;
}
if (initNetwork(argv[1], CLIENTPORT)) {
fprintf(stderr, "Error at network initialization\n");
return -1;
} // if
bool running = true;
while (running) {sleep(1);}
if (cwiid_close(wiimote)) {
fprintf(stderr, "Error on wiimote disconnect\n");
return -1;
}
return 0;
}
void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
union cwiid_mesg mesg[], struct timespec *timestamp)
{
int i;
for (i=0; i < mesg_count; i++)
{
switch (mesg[i].type) {
case CWIID_MESG_STATUS:
printf("Status Report: battery=%d extension=",
mesg[i].status_mesg.battery);
switch (mesg[i].status_mesg.ext_type) {
case CWIID_EXT_NONE:
printf("none");
break;
case CWIID_EXT_NUNCHUK:
printf("Nunchuk");
break;
case CWIID_EXT_CLASSIC:
printf("Classic Controller");
break;
default:
printf("Unknown Extension");
break;
}
printf("\n");
break;
case CWIID_MESG_BTN:
printf("Button Report: %i\n", mesg[i].btn_mesg.buttons);
if(mesg[i].btn_mesg.buttons & CWIID_BTN_A) continuer = 0;
if(mesg[i].btn_mesg.buttons & CWIID_BTN_2) bouton_2 = 1; else bouton_2 = 0;
if(mesg[i].btn_mesg.buttons & CWIID_BTN_1) bouton_1 = 1; else bouton_1 = 0;
break;
case CWIID_MESG_ACC:
/* printf("Acc Report: x=%d, y=%d, z=%d\n",
mesg[i].acc_mesg.acc[CWIID_X],
mesg[i].acc_mesg.acc[CWIID_Y],
mesg[i].acc_mesg.acc[CWIID_Z]);*/
AccX = mesg[i].acc_mesg.acc[CWIID_X];
AccY = mesg[i].acc_mesg.acc[CWIID_Y];
AccZ = mesg[i].acc_mesg.acc[CWIID_Z];
break;
case CWIID_MESG_ERROR:
if (cwiid_close(wiimote)) {
fprintf(stderr, "Error on wiimote disconnect\n");
}
break;
default:
printf("Unknown Report");
break;
}
}
}
int main(int argc, char **argv)
{
cwiid_wiimote_t *wiimote;
struct cwiid_state state;
struct acc_cal acc_cal;
bdaddr_t bdaddr;
struct bebot bebot;
int buttons, speed = 150;
int ahead, turn, left, right;
int i;
struct timespec t;
led_init();
while (1) {
if (argc > 1) {
str2ba(argv[1], &bdaddr);
} else {
bdaddr = *BDADDR_ANY;
}
led_set_brightness(1);
printf("Put Wiimote in discoverable mode now (press 1+2)...\n");
while(!(wiimote = cwiid_open(&bdaddr, 0)));
led_set_brightness(0);
if (bebot_init(&bebot) < 0) {
printf("Unable to init bebot\n");
exit(1);
}
cwiid_set_rpt_mode(wiimote, CWIID_RPT_ACC | CWIID_RPT_BTN);
cwiid_set_led(wiimote, CWIID_LED1_ON);
cwiid_set_rumble(wiimote, 0);
cwiid_get_state(wiimote, &state);
cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &acc_cal);
while (!(state.buttons & CWIID_BTN_HOME) && bebot_poll(&bebot, -1) > 0) {
cwiid_get_state(wiimote, &state);
bebot_update(&bebot);
#if 0
rumble = 0;
for (i = 0; i < BEBOT_BRIGHTNESS_COUNT; i++) {
if (bebot_get_brightness(&bebot, i) > 200)
rumble = 1;
}
cwiid_set_rumble(wiimote, rumble);
#endif
if (state.buttons & ~buttons & CWIID_BTN_PLUS)
speed = min(speed + 50, 300);
if (state.buttons & ~buttons & CWIID_BTN_MINUS)
speed = max(speed - 50, 50);
buttons = state.buttons;
if (state.buttons & CWIID_BTN_B) {
ahead = limit(-10,state.acc[CWIID_Y] - acc_cal.zero[CWIID_Y], 10);
turn = limit(-10, state.acc[CWIID_X] - acc_cal.zero[CWIID_X], 10);
// printf("Acc: x=%d y=%d z=%d\n", state.acc[CWIID_X],
// state.acc[CWIID_Y], state.acc[CWIID_Z]);
} else {
if (state.buttons & CWIID_BTN_UP)
ahead = 5;
else if (state.buttons & CWIID_BTN_DOWN)
ahead = -5;
else
ahead = 0;
if (state.buttons & CWIID_BTN_RIGHT)
turn = 5;
else if (state.buttons & CWIID_BTN_LEFT)
turn = -5;
else
turn = 0;
}
// printf("ahead: %d - turn: %d\n", ahead, turn);
left = limit(-300, ahead * speed / 10 + turn * speed / 15, 300);
right = limit(-300, ahead * speed / 10 - turn * speed / 15, 300);
// printf("left: %d - right: %d\n", left, right);
bebot_set_speed(&bebot, left, right);
t.tv_sec = 0;
t.tv_nsec = 50000000;
nanosleep(&t, NULL);
}
bebot_release(&bebot);
cwiid_close(wiimote);
}
return 0;
}
int main(int argc, char *argv[])
{
cwiid_wiimote_t *wiimote; /* wiimote handle */
struct cwiid_state state; /* wiimote state */
bdaddr_t bdaddr; /* bluetooth device address */
unsigned char mesg = 0;
unsigned char led_state = 0;
unsigned char rpt_mode = 0;
unsigned char rumble = 0;
int exit = 0;
cwiid_set_err(err);
/* Connect to address given on command-line, if present */
if (argc > 1) {
str2ba(argv[1], &bdaddr);
}
else {
bdaddr = *BDADDR_ANY;
}
/* Connect to the wiimote */
printf("Put Wiimote in discoverable mode now (press 1+2)...\n");
if (!(wiimote = cwiid_open(&bdaddr, 0))) {
fprintf(stderr, "Unable to connect to wiimote\n");
return -1;
}
if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) {
fprintf(stderr, "Unable to set message callback\n");
}
printf("Note: To demonstrate the new API interfaces, wmdemo no longer "
"enables messages by default.\n"
"Output can be gathered through the new state-based interface (s), "
"or by enabling the messages interface (c).\n");
/* Menu */
printf("%s", MENU);
while (!exit) {
switch (getchar()) {
case '1':
toggle_bit(led_state, CWIID_LED1_ON);
set_led_state(wiimote, led_state);
break;
case '2':
toggle_bit(led_state, CWIID_LED2_ON);
set_led_state(wiimote, led_state);
break;
case '3':
toggle_bit(led_state, CWIID_LED3_ON);
set_led_state(wiimote, led_state);
break;
case '4':
toggle_bit(led_state, CWIID_LED4_ON);
set_led_state(wiimote, led_state);
break;
case '5':
toggle_bit(rumble, 1);
if (cwiid_set_rumble(wiimote, rumble)) {
fprintf(stderr, "Error setting rumble\n");
}
break;
case 'a':
toggle_bit(rpt_mode, CWIID_RPT_ACC);
set_rpt_mode(wiimote, rpt_mode);
break;
case 'b':
toggle_bit(rpt_mode, CWIID_RPT_BTN);
set_rpt_mode(wiimote, rpt_mode);
break;
case 'e':
/* CWIID_RPT_EXT is actually
* CWIID_RPT_NUNCHUK | CWIID_RPT_CLASSIC */
toggle_bit(rpt_mode, CWIID_RPT_EXT);
set_rpt_mode(wiimote, rpt_mode);
break;
case 'i':
/* libwiimote picks the highest quality IR mode available with the
* other options selected (not including as-yet-undeciphered
* interleaved mode */
toggle_bit(rpt_mode, CWIID_RPT_IR);
set_rpt_mode(wiimote, rpt_mode);
break;
case 'm':
if (!mesg) {
if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) {
fprintf(stderr, "Error enabling messages\n");
}
else {
mesg = 1;
}
}
else {
if (cwiid_disable(wiimote, CWIID_FLAG_MESG_IFC)) {
fprintf(stderr, "Error disabling message\n");
}
else {
mesg = 0;
}
}
break;
case 'p':
printf("%s", MENU);
break;
case 'r':
if (cwiid_request_status(wiimote)) {
fprintf(stderr, "Error requesting status message\n");
}
break;
case 's':
if (cwiid_get_state(wiimote, &state)) {
fprintf(stderr, "Error getting state\n");
}
print_state(&state);
break;
case 't':
toggle_bit(rpt_mode, CWIID_RPT_STATUS);
set_rpt_mode(wiimote, rpt_mode);
break;
case 'x':
exit = -1;
break;
case '\n':
break;
default:
fprintf(stderr, "invalid option\n");
}
}
if (cwiid_close(wiimote)) {
fprintf(stderr, "Error on wiimote disconnect\n");
return -1;
}
return 0;
}
/* cwiid_mesg_callback_t has undergone a few changes lately, hopefully this
* will be the last. Some programs need to know which messages were received
* simultaneously (e.g. for correlating accelerometer and IR data), and the
* sequence number mechanism used previously proved cumbersome, so we just
* pass an array of messages, all of which were received at the same time.
* The id is to distinguish between multiple wiimotes using the same callback.
* */
void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
union cwiid_mesg mesg[], struct timespec *timestamp)
{
int i, j;
int valid_source;
for (i=0; i < mesg_count; i++)
{
switch (mesg[i].type) {
case CWIID_MESG_STATUS:
printf("Status Report: battery=%d extension=",
mesg[i].status_mesg.battery);
switch (mesg[i].status_mesg.ext_type) {
case CWIID_EXT_NONE:
printf("none");
break;
case CWIID_EXT_NUNCHUK:
printf("Nunchuk");
break;
case CWIID_EXT_CLASSIC:
printf("Classic Controller");
break;
case CWIID_EXT_BALANCE:
printf("Balance Board");
break;
case CWIID_EXT_MOTIONPLUS:
printf("MotionPlus");
break;
default:
printf("Unknown Extension");
break;
}
printf("\n");
break;
case CWIID_MESG_BTN:
printf("Button Report: %.4X\n", mesg[i].btn_mesg.buttons);
break;
case CWIID_MESG_ACC:
printf("Acc Report: x=%d, y=%d, z=%d\n",
mesg[i].acc_mesg.acc[CWIID_X],
mesg[i].acc_mesg.acc[CWIID_Y],
mesg[i].acc_mesg.acc[CWIID_Z]);
break;
case CWIID_MESG_IR:
//printf("IR Report: ");
valid_source = 0;
bool f;
int record_x;
int record_y;
for (j = 0; j < CWIID_IR_SRC_COUNT; j++) {
if (mesg[i].ir_mesg.src[j].valid) {
valid_source = 1;
/* printf("(%d,%d) ", mesg[i].ir_mesg.src[j].pos[CWIID_X],
mesg[i].ir_mesg.src[j].pos[CWIID_Y]);
*/
//get the data and process
if ( record_x != mesg[i].ir_mesg.src[j].pos[CWIID_X] && record_y != mesg[i].ir_mesg.src[j].pos[CWIID_Y] )
{
printf("(%d,%d)\n", mesg[i].ir_mesg.src[j].pos[CWIID_X],mesg[i].ir_mesg.src[j].pos[CWIID_Y]);
record_x = mesg[i].ir_mesg.src[j].pos[CWIID_X];
record_y = mesg[i].ir_mesg.src[j].pos[CWIID_Y];
//emit BluetoothProcess::sendData( record_x, record_y );
f = true;
break;
}
}
}
if (!valid_source) {
//printf("no sources detected");
f = false;
}
//printf("\n");
break;
case CWIID_MESG_NUNCHUK:
printf("Nunchuk Report: btns=%.2X stick=(%d,%d) acc.x=%d acc.y=%d "
"acc.z=%d\n", mesg[i].nunchuk_mesg.buttons,
mesg[i].nunchuk_mesg.stick[CWIID_X],
mesg[i].nunchuk_mesg.stick[CWIID_Y],
mesg[i].nunchuk_mesg.acc[CWIID_X],
mesg[i].nunchuk_mesg.acc[CWIID_Y],
mesg[i].nunchuk_mesg.acc[CWIID_Z]);
break;
case CWIID_MESG_CLASSIC:
printf("Classic Report: btns=%.4X l_stick=(%d,%d) r_stick=(%d,%d) "
"l=%d r=%d\n", mesg[i].classic_mesg.buttons,
mesg[i].classic_mesg.l_stick[CWIID_X],
mesg[i].classic_mesg.l_stick[CWIID_Y],
mesg[i].classic_mesg.r_stick[CWIID_X],
mesg[i].classic_mesg.r_stick[CWIID_Y],
mesg[i].classic_mesg.l, mesg[i].classic_mesg.r);
break;
case CWIID_MESG_BALANCE:
printf("Balance Report: right_top=%d right_bottom=%d "
"left_top=%d left_bottom=%d\n",
mesg[i].balance_mesg.right_top,
mesg[i].balance_mesg.right_bottom,
mesg[i].balance_mesg.left_top,
mesg[i].balance_mesg.left_bottom);
break;
case CWIID_MESG_MOTIONPLUS:
printf("MotionPlus Report: angle_rate=(%d,%d,%d) low_speed=(%d,%d,%d)\n",
mesg[i].motionplus_mesg.angle_rate[0],
mesg[i].motionplus_mesg.angle_rate[1],
mesg[i].motionplus_mesg.angle_rate[2],
mesg[i].motionplus_mesg.low_speed[0],
mesg[i].motionplus_mesg.low_speed[1],
mesg[i].motionplus_mesg.low_speed[2]);
break;
case CWIID_MESG_ERROR:
if (cwiid_close(wiimote)) {
fprintf(stderr, "Error on wiimote disconnect\n");
exit(-1);
}
exit(0);
break;
default:
printf("Unknown Report");
break;
}
}
}
/* cwiid_mesg_callback_t has undergone a few changes lately, hopefully this
* will be the last. Some programs need to know which messages were received
* simultaneously (e.g. for correlating accelerometer and IR data), and the
* sequence number mechanism used previously proved cumbersome, so we just
* pass an array of messages, all of which were received at the same time.
* The id is to distinguish between multiple wiimotes using the same callback.
* */
void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
union cwiid_mesg mesg[], struct timespec *timestamp)
{
int i, j;
int valid_source;
#if _DEBUG >= 2
debug_print("@%u: msg received\n",get_tick_count());
#endif
for (i = 0; i < mesg_count; i++)
{
switch (mesg[i].type)
{
case CWIID_MESG_STATUS:
#if _DEBUG >= 2
debug_print("@%u: Status Report: battery=%d extension=", get_tick_count(),
mesg[i].status_mesg.battery);
#endif
wiimote_status_data.battery_level = mesg[i].status_mesg.battery;
switch (mesg[i].status_mesg.ext_type)
{
case CWIID_EXT_NONE:
#if _DEBUG >= 2
debug_print("none\n");
#endif
break;
case CWIID_EXT_NUNCHUK:
#if _DEBUG >= 2
debug_print("Nunchuk\n");
#endif
break;
case CWIID_EXT_CLASSIC:
#if _DEBUG >= 2
debug_print("Classic Controller\n");
#endif
break;
default:
#if _DEBUG >= 2
debug_print("Unknown Extension\n");
#endif
break;
}
break;
case CWIID_MESG_BTN:
#if _DEBUG >= 2
debug_print("@%u: Button Report: %.4X\n", get_tick_count(), mesg[i].btn_mesg.buttons);
#endif
wiimote_status_data.button_data = mesg[i].btn_mesg.buttons;
break;
case CWIID_MESG_ACC:
#if _DEBUG >= 2
debug_print("@%u: Acc Report: x=%d, y=%d, z=%d\n", get_tick_count(),
mesg[i].acc_mesg.acc[CWIID_X],
mesg[i].acc_mesg.acc[CWIID_Y],
mesg[i].acc_mesg.acc[CWIID_Z]);
#endif
for (j = 0; j < 3; j++)
{
wiimote_status_data.accel_raw_data[j] = mesg[i].acc_mesg.acc[j];
}
break;
case CWIID_MESG_IR:
#if _DEBUG >= 2
debug_print("@%u: IR Report: ", get_tick_count());
#endif
valid_source = 0;
for (j = 0; j < CWIID_IR_SRC_COUNT; j++)
{
#if _DEBUG >= 2
if (mesg[i].ir_mesg.src[j].valid)
{
valid_source = 1;
debug_print("(%d,%d) ", mesg[i].ir_mesg.src[j].pos[CWIID_X],
mesg[i].ir_mesg.src[j].pos[CWIID_Y]);
}
#endif
wiimote_status_data.ir_raw_data.WiimoteIRPoint[j].valid =
mesg[i].ir_mesg.src[j].valid;
wiimote_status_data.ir_raw_data.WiimoteIRPoint[j].size =
mesg[i].ir_mesg.src[j].size;
wiimote_status_data.ir_raw_data.WiimoteIRPoint[j].pos[0] =
mesg[i].ir_mesg.src[j].pos[CWIID_X];
wiimote_status_data.ir_raw_data.WiimoteIRPoint[j].pos[1] =
mesg[i].ir_mesg.src[j].pos[CWIID_Y];
}
#if _DEBUG >= 2
if (!valid_source)
{
debug_print("@%u: no sources detected", get_tick_count());
}
printf("\n");
#endif
break;
case CWIID_MESG_NUNCHUK:
#if _DEBUG >= 2
debug_print("@%u: Nunchuk Report: btns=%.2X stick=(%d,%d) acc.x=%d acc.y=%d "
"acc.z=%d\n", get_tick_count(), mesg[i].nunchuk_mesg.buttons,
mesg[i].nunchuk_mesg.stick[CWIID_X],
mesg[i].nunchuk_mesg.stick[CWIID_Y],
mesg[i].nunchuk_mesg.acc[CWIID_X],
mesg[i].nunchuk_mesg.acc[CWIID_Y],
mesg[i].nunchuk_mesg.acc[CWIID_Z]);
#endif
break;
case CWIID_MESG_CLASSIC:
#if _DEBUG >= 2
debug_print("@%u: Classic Report: btns=%.4X l_stick=(%d,%d) r_stick=(%d,%d) "
"l=%d r=%d\n", get_tick_count(), mesg[i].classic_mesg.buttons,
mesg[i].classic_mesg.l_stick[CWIID_X],
mesg[i].classic_mesg.l_stick[CWIID_Y],
mesg[i].classic_mesg.r_stick[CWIID_X],
mesg[i].classic_mesg.r_stick[CWIID_Y],
mesg[i].classic_mesg.l, mesg[i].classic_mesg.r);
#endif
break;
case CWIID_MESG_ERROR:
if (cwiid_close(wiimote))
{
fprintf(stderr, "@%u: Error on wiimote disconnect\n",
get_tick_count());
shutdown_application(-1);
}
shutdown_application(0);
break;
default:
#if _DEBUG > 2
debug_print("@%u: Unknown Report", get_tick_count());
#endif
break;
}
}
signal_wiimote_data_ready(&wiimote_status_data, 100);
}
void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
union cwiid_mesg mesg[], struct timespec *timestamp) {
int i, j;
int valid_source;
double a_x, a_y, a_z;
for (i = 0; i < mesg_count; i++) {
switch (mesg[i].type) {
case CWIID_MESG_BTN:
api->buttonApi = mesg[i].btn_mesg.buttons;
break;
case CWIID_MESG_ACC:
api->accApi[0] = mesg->acc_mesg.acc[CWIID_X];
api->accApi[1] = mesg->acc_mesg.acc[CWIID_Y];
api->accApi[2] = mesg->acc_mesg.acc[CWIID_Z];
// cout << "x" << mesg->acc_mesg.acc[CWIID_X] << endl;
// cout << "y" << mesg->acc_mesg.acc[CWIID_Y] << endl;
// cout << "z" << mesg->acc_mesg.acc[CWIID_Y] << endl;
break;
case CWIID_MESG_IR:
valid_source = 0;
for (j = 0; j < CWIID_IR_SRC_COUNT; j++) {
if (mesg[i].ir_mesg.src[j].valid) {
valid_source = 1;
if (j == 0) {
api->irApi1[0] = mesg[i].ir_mesg.src[j].pos[CWIID_X];
api->irApi1[1] = mesg[i].ir_mesg.src[j].pos[CWIID_Y];
api->sourceDetected = 1;
}
if (j == 1) {
api->irApi2[0] = mesg[i].ir_mesg.src[j].pos[CWIID_X];
api->irApi2[1] = mesg[i].ir_mesg.src[j].pos[CWIID_Y];
api->sourceDetected = 1;
}
if (j == 2) {
api->irApi3[0] = mesg[i].ir_mesg.src[j].pos[CWIID_X];
api->irApi3[1] = mesg[i].ir_mesg.src[j].pos[CWIID_Y];
api->sourceDetected = 1;
}
if (j == 3){
api->irApi4[0] = mesg[i].ir_mesg.src[j].pos[CWIID_X];
api->irApi4[1] = mesg[i].ir_mesg.src[j].pos[CWIID_Y];
api->sourceDetected = 1;
}
}
}
if (!valid_source) {
api->sourceDetected = 0;
}
break;
case CWIID_MESG_NUNCHUK:
api->nunchukButtonApi = mesg[i].nunchuk_mesg.buttons;
api->nunchukStickApi[0] = mesg[i].nunchuk_mesg.stick[CWIID_X];
api->nunchukStickApi[1] = mesg[i].nunchuk_mesg.stick[CWIID_Y];
api->nunchukAccApi[0] = mesg[i].nunchuk_mesg.acc[CWIID_X];
api->nunchukAccApi[1] = mesg[i].nunchuk_mesg.acc[CWIID_Y];
api->nunchukAccApi[2] = mesg[i].nunchuk_mesg.acc[CWIID_Z];
//cout << api->nunchukStickApi[0] << endl;
// cout << api->nunchukStickApi[1] << endl;
break;
case CWIID_MESG_MOTIONPLUS:
printf("MotionPlus Report: angle_rate=(%d,%d,%d) low_speed=(%d,%d,%d)\n",
mesg[i].motionplus_mesg.angle_rate[0],
mesg[i].motionplus_mesg.angle_rate[1],
mesg[i].motionplus_mesg.angle_rate[2]);
break;
case CWIID_MESG_ERROR:
if (cwiid_close(wiimote)) {
fprintf(stderr, "Error on wiimote disconnect\n");
exit(-1);
}
exit(0);
break;
default:
printf("Unknown Report");
break;
}
}
}
C_RESULT close_wiimote(void)
{
cwiid_close(wiimote);
return C_OK;
}
int main( int argc, char** argv )
{
// Variables definition
int wiimote_paired = 0;
int x = -1;
int y = -1;
int batt = 0;
char big_msg[256];
char small_msg[256];
unsigned char rumble = 0;
unsigned char rpt_mode = 0;
SDL_Surface* screen = NULL;
SDL_Event event;
SDL_Rect big_position, small_position;
SDL_Surface* big_text = NULL;
SDL_Surface* small_text = NULL;
TTF_Font* big_font = NULL;
TTF_Font* small_font = NULL;
AppState appstate = PAIRING;
cwiid_wiimote_t* wiimote;
bdaddr_t bdaddr = *BDADDR_ANY;
struct cwiid_state state;
SDL_Color white = {0,0,0};
SDL_Rect dot;
// Variables initialisation
big_position.x = 180;
big_position.y = 250;
small_position.x = 300;
small_position.y = 50;
// SDL and SDL_TTF initialization
if( SDL_Init( SDL_INIT_VIDEO | SDL_INIT_TIMER ) )
{
fprintf( stderr, "Error while initializing SDL: %s\n", SDL_GetError() );
exit( EXIT_FAILURE );
}
if( TTF_Init() )
{
fprintf( stderr, "Error while initializing SDL_TTF: %s\n", TTF_GetError() );
exit( EXIT_FAILURE );
}
// Resources loading
big_font = TTF_OpenFont( "gratis.ttf", 35 );
if( big_font == NULL )
{
fprintf( stderr, "Error while loading font: %s\n", TTF_GetError() );
exit( EXIT_FAILURE );
}
small_font = TTF_OpenFont( "gratis.ttf", 15 );
if( small_font == NULL )
{
fprintf( stderr, "Error while loading font: %s\n", TTF_GetError() );
exit( EXIT_FAILURE );
}
// Main window creation
screen = SDL_SetVideoMode( S_WIDTH, S_HEIGHT, S_BPP, S_FLAGS );
SDL_WM_SetCaption( "Wiimote pointer display", NULL );
// Pair with the wiimote and rumble to notify
printf( "Trying to pair with the wiimote now.\nPut the wiimote in search mode (press 1+2) ...\n" );
if( wiimote = cwiid_open( &bdaddr, 0 ) )
{
wiimote_paired = 1;
appstate = DISPLAY;
toggle_bit( rumble, 0x01 );
cwiid_set_rumble( wiimote, rumble );
usleep( 300000 );
toggle_bit( rumble, 0x01 );
cwiid_set_rumble( wiimote, rumble );
// toggle IR reporting ON
toggle_bit( rpt_mode, 0x08 );
cwiid_set_rpt_mode( wiimote, rpt_mode );
}
// Main loop
while( appstate != EXIT )
{
// Take care of events if there is any
if( SDL_PollEvent( &event ) )
{
switch( event.type )
{
case SDL_QUIT:
appstate = EXIT;
break;
case SDL_KEYDOWN:
if( event.key.keysym.sym == SDLK_ESCAPE )
appstate = EXIT;
break;
}
}
// Query the wiimote
if( appstate == DISPLAY )
{
cwiid_get_state( wiimote, &state );
if( state.ir_src[0].valid )
{
SDL_FillRect( screen, NULL, SDL_MapRGB( screen->format, 0, 255, 0 ) );
x = state.ir_src[0].pos[0];
y = state.ir_src[0].pos[1];
}
else
{
SDL_FillRect( screen, NULL, SDL_MapRGB( screen->format, 255, 0, 0 ) );
}
batt = (int)(100.0 * state.battery / 0xD0);
// sprintf( big_msg, "IR source position: (%d,%d)", x, y );
dot.x = (int) (x * S_WIDTH) / X_MAX;
dot.y = (int) (y * S_HEIGHT) / Y_MAX;
dot.w = 10;
dot.h = 10;
SDL_FillRect( screen, &dot, SDL_MapRGB( screen->format, 255, 255, 255) );
sprintf( small_msg, "Battery remaining: %d%%", batt );
//big_text = TTF_RenderText_Solid( big_font, big_msg, white );
small_text = TTF_RenderText_Solid( small_font, small_msg, white );
//SDL_BlitSurface( big_text, NULL, screen, &big_position );
SDL_BlitSurface( small_text, NULL, screen, &small_position );
}
// Render the screen
SDL_Flip( screen );
}
// Free resources and exits sub-systems
TTF_CloseFont( big_font );
TTF_CloseFont( small_font );
TTF_Quit();
if( wiimote_paired )
cwiid_close( wiimote );
else
fprintf( stderr, "No wiimotes could be found\n" );
SDL_Quit();
return EXIT_SUCCESS;
}
/* cwiid_mesg_callback_t has undergone a few changes lately, hopefully this
* will be the last. Some programs need to know which messages were received
* simultaneously (e.g. for correlating accelerometer and IR data), and the
* sequence number mechanism used previously proved cumbersome, so we just
* pass an array of messages, all of which were received at the same time.
* The id is to distinguish between multiple wiimotes using the same callback.
* */
void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
union cwiid_mesg mesg[], struct timespec *timestamp)
{
int i, j;
int valid_source;
for (i=0; i < mesg_count; i++)
{
switch (mesg[i].type) {
case CWIID_MESG_STATUS:
printf("Status Report: battery=%d extension=",
mesg[i].status_mesg.battery);
switch (mesg[i].status_mesg.ext_type) {
case CWIID_EXT_NONE:
printf("none");
break;
case CWIID_EXT_NUNCHUK:
printf("Nunchuk");
break;
case CWIID_EXT_CLASSIC:
printf("Classic Controller");
break;
default:
printf("Unknown Extension");
break;
}
printf("\n");
break;
case CWIID_MESG_BTN:
printf("Button Report: %.4X\n", mesg[i].btn_mesg.buttons);
break;
case CWIID_MESG_ACC:
printf("Acc Report: x=%d, y=%d, z=%d\n",
mesg[i].acc_mesg.acc[CWIID_X],
mesg[i].acc_mesg.acc[CWIID_Y],
mesg[i].acc_mesg.acc[CWIID_Z]);
break;
case CWIID_MESG_IR:
printf("IR Report: ");
valid_source = 0;
for (j = 0; j < CWIID_IR_SRC_COUNT; j++) {
if (mesg[i].ir_mesg.src[j].valid) {
valid_source = 1;
printf("(%d,%d) ", mesg[i].ir_mesg.src[j].pos[CWIID_X],
mesg[i].ir_mesg.src[j].pos[CWIID_Y]);
}
}
if (!valid_source) {
printf("no sources detected");
}
printf("\n");
break;
case CWIID_MESG_NUNCHUK:
printf("Nunchuk Report: btns=%.2X stick=(%d,%d) acc.x=%d acc.y=%d "
"acc.z=%d\n", mesg[i].nunchuk_mesg.buttons,
mesg[i].nunchuk_mesg.stick[CWIID_X],
mesg[i].nunchuk_mesg.stick[CWIID_Y],
mesg[i].nunchuk_mesg.acc[CWIID_X],
mesg[i].nunchuk_mesg.acc[CWIID_Y],
mesg[i].nunchuk_mesg.acc[CWIID_Z]);
break;
case CWIID_MESG_CLASSIC:
printf("Classic Report: btns=%.4X l_stick=(%d,%d) r_stick=(%d,%d) "
"l=%d r=%d\n", mesg[i].classic_mesg.buttons,
mesg[i].classic_mesg.l_stick[CWIID_X],
mesg[i].classic_mesg.l_stick[CWIID_Y],
mesg[i].classic_mesg.r_stick[CWIID_X],
mesg[i].classic_mesg.r_stick[CWIID_Y],
mesg[i].classic_mesg.l, mesg[i].classic_mesg.r);
break;
case CWIID_MESG_ERROR:
if (cwiid_close(wiimote)) {
fprintf(stderr, "Error on wiimote disconnect\n");
exit(-1);
}
exit(0);
break;
default:
printf("Unknown Report");
break;
}
}
}
