ErrorID_t error_mode(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
debug_print("@%u: An error has occurred\n", get_tick_count());
shutdown_application(-1);
return ERR_UNKN;
}
void vhalt(const char *message)
{
stop_recording();
logFatal("vhalt: %s", message);
GetCurrentLogger()->flush();
shutdown_application();
system_alert_user(message, fatalError);
abort();
}
void on_window_destroy(GtkObject *object, gpointer user_data)
{
shutdown_application(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;
#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);
}
/*!
@brief Driver for Wiimote operation menu
Handles parsing wiimote input to determine correct operating mode.
This function will never exit once entered.
*/
ErrorID_t main_menu(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
ErrorID_t error;
uint8_t menu_count;
uint16_t last_button_state = 0;
WII_OPERATE_STATE wii_operate_state = WII_OPERATE_INIT_MENU;
write_status_led(STATUS_LED_OFF, 0);
for (;;)
{
switch (wii_operate_state)
{
case WII_OPERATE_INIT_MENU:
menu_count = 0;
last_button_state = 0;
wii_operate_state = WII_OPERATE_WAIT_FOR_BUTTON_PRESS;
write_status_led(STATUS_LED_FLASH, 500);
if (0 > cwiid_set_led(wiimote, CWIID_LED1_ON))
return WII_ERROR_TRANSMIT;
if (0 > cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC))
return WII_ERROR_TRANSMIT;
if (0 > cwiid_set_rpt_mode(wiimote, CWIID_RPT_BTN))
return WII_ERROR_TRANSMIT;
/* Menu */
debug_print("%s", MENU);
set_lcd(0, "Connected.");
set_lcd(1, "Select Mode.");
break;
case WII_OPERATE_WAIT_FOR_BUTTON_PRESS:
error = wait_for_wiimotedata(wiimote_status, 2000);
if (error == ERR_WII_DATA_TIMEOUT)
{
set_lcd(0, "Select Mode");
set_lcd(1, (char *) lcd_menu_strings[menu_count]);
if (++menu_count >= MENU_ENTRIES)
menu_count = 0;
}
else if (0 < error)
{
shutdown_application(error);
}
else if (wiimote_status->button_data != last_button_state)
{
last_button_state = wiimote_status->button_data;
if (last_button_state & CWIID_BTN_A) // collect acceleration data
{
wii_operate_state = WII_OPERATE_DISPLAY_ACCELEROMETER_INFO;
}
else if (last_button_state & CWIID_BTN_B) // collect IR data
{
wii_operate_state = WII_OPERATE_DISPLAY_IR_STATUS;
}
else if (last_button_state & CWIID_BTN_HOME) // reset
{
shutdown_application(0);
}
else if (last_button_state & CWIID_BTN_PLUS)
{
sensor_cutoff_fwd += 5;
if (sensor_cutoff_fwd > 100)
sensor_cutoff_fwd = 100;
#if REVERSE_SENSOR_PRESENT
sensor_cutoff_rev += 5;
if (sensor_cutoff_rev > 100)
sensor_cutoff_rev = 100;
#endif
}
else if (last_button_state & CWIID_BTN_MINUS)
{
sensor_cutoff_fwd -= 5;
if (sensor_cutoff_fwd < 0)
sensor_cutoff_fwd = 0;
#if REVERSE_SENSOR_PRESENT
sensor_cutoff_rev -= 5;
if (sensor_cutoff_rev < 0)
sensor_cutoff_rev = 0;
#endif
}
else if (last_button_state & (CWIID_BTN_2 | CWIID_BTN_1))
{
wii_operate_state = WII_OPERATE_DISPLAY_BUTTON_STATUS;
}
}
break;
case WII_OPERATE_DISPLAY_ACCELEROMETER_INFO:
if (ERR_NONE != acceleration_mode(wiimote, wiimote_status))
wii_operate_state = WII_OPERATE_ERROR_STATE;
else
wii_operate_state = WII_OPERATE_INIT_MENU;
break;
case WII_OPERATE_DISPLAY_IR_STATUS:
if (ERR_NONE != infrared_mode(wiimote, wiimote_status))
wii_operate_state = WII_OPERATE_ERROR_STATE;
else
wii_operate_state = WII_OPERATE_INIT_MENU;
break;
case WII_OPERATE_DISPLAY_BUTTON_STATUS:
if (ERR_NONE != button_mode(wiimote, wiimote_status))
wii_operate_state = WII_OPERATE_ERROR_STATE;
else
wii_operate_state = WII_OPERATE_INIT_MENU;
break;
case WII_OPERATE_ERROR_STATE:
if (ERR_NONE == error_mode(wiimote, wiimote_status))
wii_operate_state = WII_OPERATE_INIT_MENU;
break;
}
}
return ERR_NONE;
}
/*!
@brief High level task for wiimote.
Handles initializing wiimote.
*/
void control_tasks(char *dev_name)
{
WiimoteState_t WiimoteState = WII_PROMPT;
bdaddr_t bdaddr = *BDADDR_ANY; /* bluetooth device address */
init_tick_count();
#if _MUTEX_ENABLE
pthread_mutex_lock(&mutex);
#endif
#if HAVE_GTK
int argc_dummy = 0;
char **argv_dummy = NULL;
init_gui(argc_dummy, argv_dummy);
set_comm_trace(true);
set_diagnostic_mode(true);
#else
if (0 >= comm_init(dev_name))
{
debug_print("@%u: Cannot initialize %s\n", get_tick_count(), dev_name);
exit(4);
}
else
{
debug_print("@%u: %s initialized.\n", get_tick_count(), dev_name);
}
do
{
debug_print("Connecting to controller board... ");
if (0 > send_password("WIFIBOT123"))
{
debug_print("Error\n");
}
else
{
debug_print("Done\n");
break;
}
}while (1);
#endif
set_lcd(0, "%s", PACKAGE_NAME);
set_lcd(1, "Rev: %s", PACKAGE_VERSION);
// display startup for 1 s
sleep(1);
cwiid_set_err(err);
for (;;)
{
switch (WiimoteState)
{
case WII_PROMPT:
/* Connect to the wiimote */
debug_print(
"Put Wiimote in discoverable mode now (press 1+2)...\n");
set_lcd(0, "Connecting...");
set_lcd(1, "Press 1+2 now");
WiimoteState = WII_WAIT_FOR_CONNECTION;
break;
case WII_WAIT_FOR_CONNECTION:
if (!(wiimote = cwiid_open(&bdaddr, 0)))
{
fprintf(stderr, "Unable to connect to wiimote, retrying...\n");
WiimoteState = WII_PROMPT;
}
else if (cwiid_set_mesg_callback(wiimote, cwiid_callback))
{
fprintf(stderr,
"Unable to set message callback, retrying...\n");
WiimoteState = WII_PROMPT;
}
else
{
WiimoteState = WII_OPERATE;
}
break;
case WII_OPERATE:
default:
main_menu(wiimote, &wiimote_status_data);
debug_print("exiting.\n");
shutdown_application(0);
break;
}
}
}
