static void show_battery()
{
struct cwiid_state state;
float battery_percent;
int battery_led;
cwiid_get_state(wiimote, &state);
// calculate battery as a percent, then decide how many leds to light up
battery_percent = (100*state.battery / CWIID_BATTERY_MAX);
battery_led = ceil(battery_percent / 25);
switch(battery_led) {
case 1:
cwiid_set_led(wiimote, CWIID_LED1_ON);
break;
case 2:
cwiid_set_led(wiimote, CWIID_LED1_ON | CWIID_LED2_ON );
break;
case 3:
cwiid_set_led(wiimote, CWIID_LED1_ON | CWIID_LED2_ON | CWIID_LED3_ON );
break;
case 4:
cwiid_set_led(wiimote, CWIID_LED1_ON | CWIID_LED2_ON | CWIID_LED3_ON | CWIID_LED4_ON );
break;
default:
break;
}
}
void
WiimoteController::set_led_real(uint8_t status)
{
switch(status)
{
case 2:
case 6:
cwiid_set_led(m_wiimote, CWIID_LED1_ON);
break;
case 3:
case 7:
cwiid_set_led(m_wiimote, CWIID_LED2_ON);
break;
case 4:
case 8:
cwiid_set_led(m_wiimote, CWIID_LED3_ON);
break;
case 5:
case 9:
cwiid_set_led(m_wiimote, CWIID_LED4_ON);
break;
default:
cwiid_set_led(m_wiimote, 0);
break;
}
}
C_RESULT open_wiimote(void)
{
C_RESULT res = C_OK;
printf("\nSearching for Wiimote - press both '1' and '2' on your Wiimote !!\n");
if(cwiid_find_wiimote(&bdaddr, WIIMOTE_FIND_TIMEOUT) == 0)
{
if( ! (wiimote = cwiid_open(&bdaddr, 0)) )
PRINT("Unable to connect to wiimote\n");
else
res = C_OK;
}
if(SUCCEED(res))
if(cwiid_set_rpt_mode(wiimote, CWIID_RPT_BTN|CWIID_RPT_ACC))
res = C_FAIL;
if(SUCCEED(res))
if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &wm_cal))
res = C_FAIL;
cwiid_set_led(wiimote,1);
printf(" ... Wiimote found.\n");
return res;
}
static void leds(int flags)
{
static int localflags=0;
if (flags!=localflags){
localflags=flags;
cwiid_set_led(wiimote,flags);
}
}
static void set_leds_running()
{
uint8_t running = 0;
bool d1, d2, d3, d4;
ltr_int_get_run_indication(&d1, &d2, &d3, &d4);
if(d1) running |= CWIID_LED1_ON;
if(d2) running |= CWIID_LED2_ON;
if(d3) running |= CWIID_LED3_ON;
if(d4) running |= CWIID_LED4_ON;
cwiid_set_led(gWiimote, running);
}
static void set_leds_paused()
{
uint8_t paused = 0;
bool d1, d2, d3, d4;
ltr_int_get_pause_indication(&d1, &d2, &d3, &d4);
if(d1) paused |= CWIID_LED1_ON;
if(d2) paused |= CWIID_LED2_ON;
if(d3) paused |= CWIID_LED3_ON;
if(d4) paused |= CWIID_LED4_ON;
cwiid_set_led(gWiimote, paused);
}
/* Tell cwiid the LED states */
void CWiiRemote::SetLedState()
{ //Sets our leds on the wiiremote
#ifdef CWIID_OLD
if (cwiid_command(m_wiiremoteHandle, CWIID_CMD_LED, m_ledState))
#else
if (cwiid_set_led(m_wiiremoteHandle, m_ledState))
#endif
{
CPacketLOG log(LOGERROR, "Error setting WiiRemote LED state");
log.Send(m_Socket, m_MyAddr);
}
}
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;
}
static int Wiimote_set_led(Wiimote *self, PyObject *PyLed, void *closure)
{
long led;
if (((led = PyInt_AsLong(PyLed)) == -1) && PyErr_Occurred()) {
return -1;
}
if (cwiid_set_led(self->wiimote, (uint8_t)led)) {
PyErr_SetString(PyExc_AttributeError,
"Error setting wiimote led state");
return -1;
}
return 0;
}
void chkLED_toggled(void)
{
uint8_t LED_state;
if (wiimote) {
LED_state =
(gtk_check_menu_item_get_active(GTK_CHECK_MENU_ITEM(chkLED1))
? CWIID_LED1_ON : 0) |
(gtk_check_menu_item_get_active(GTK_CHECK_MENU_ITEM(chkLED2))
? CWIID_LED2_ON : 0) |
(gtk_check_menu_item_get_active(GTK_CHECK_MENU_ITEM(chkLED3))
? CWIID_LED3_ON : 0) |
(gtk_check_menu_item_get_active(GTK_CHECK_MENU_ITEM(chkLED4))
? CWIID_LED4_ON : 0);
if (cwiid_set_led(wiimote, LED_state)) {
message(GTK_MESSAGE_ERROR, "error setting LEDs",
GTK_WINDOW(winMain));
}
}
}
int cwiid_command(cwiid_wiimote_t *wiimote, enum cwiid_command command,
int flags) {
int ret;
switch (command) {
case CWIID_CMD_STATUS:
ret = cwiid_request_status(wiimote);
break;
case CWIID_CMD_LED:
ret = cwiid_set_led(wiimote, flags);
break;
case CWIID_CMD_RUMBLE:
ret = cwiid_set_rumble(wiimote, flags);
break;
case CWIID_CMD_RPT_MODE:
ret = cwiid_set_rpt_mode(wiimote, flags);
break;
default:
ret = -1;
break;
}
return ret;
}
void set_led_state(cwiid_wiimote_t *wiimote, unsigned char led_state)
{
if (cwiid_set_led(wiimote, led_state)) {
fprintf(stderr, "Error setting LEDs \n");
}
}
/*!
@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;
}
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()
{
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;
puts("Press 1+2 to connect wiimote.");
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;
}
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;
}
cwiid_wiimote_t *cwiid_new(int ctl_socket, int int_socket, int flags)
{
struct wiimote *wiimote = NULL;
char mesg_pipe_init = 0, status_pipe_init = 0, rw_pipe_init = 0,
state_mutex_init = 0, rw_mutex_init = 0, rpt_mutex_init = 0,
router_thread_init = 0, status_thread_init = 0;
void *pthread_ret;
int err;
/* Allocate wiimote */
if ((wiimote = malloc(sizeof *wiimote)) == NULL) {
cwiid_err(NULL, "Memory allocation error (cwiid_wiimote_t)");
goto ERR_HND;
}
/* set sockets and flags */
wiimote->ctl_socket = ctl_socket;
wiimote->int_socket = int_socket;
wiimote->flags = flags;
/* Global Lock, Store and Increment wiimote_id */
err = pthread_mutex_lock(&global_mutex);
if (err) {
cwiid_err(NULL, "Mutex lock error (global mutex): %s", strerror(err));
goto ERR_HND;
}
wiimote->id = wiimote_id++;
err = pthread_mutex_unlock(&global_mutex);
if (err) {
cwiid_err(wiimote, "Mutex unlock error (global mutex) - "
"deadlock warning: %s", strerror(err));
goto ERR_HND;
}
/* Create pipes */
if (pipe(wiimote->mesg_pipe)) {
cwiid_err(wiimote, "Pipe creation error (mesg pipe): %s", strerror(errno));
goto ERR_HND;
}
mesg_pipe_init = 1;
if (pipe(wiimote->status_pipe)) {
cwiid_err(wiimote, "Pipe creation error (status pipe): %s", strerror(errno));
goto ERR_HND;
}
status_pipe_init = 1;
if (pipe(wiimote->rw_pipe)) {
cwiid_err(wiimote, "Pipe creation error (rw pipe): %s", strerror(errno));
goto ERR_HND;
}
rw_pipe_init = 1;
/* Setup blocking */
if (fcntl(wiimote->mesg_pipe[1], F_SETFL, O_NONBLOCK)) {
cwiid_err(wiimote, "File control error (mesg write pipe): %s", strerror(errno));
goto ERR_HND;
}
if (wiimote->flags & CWIID_FLAG_NONBLOCK) {
if (fcntl(wiimote->mesg_pipe[0], F_SETFL, O_NONBLOCK)) {
cwiid_err(wiimote, "File control error (mesg read pipe): %s", strerror(errno));
goto ERR_HND;
}
}
/* Init mutexes */
err = pthread_mutex_init(&wiimote->state_mutex, NULL);
if (err) {
cwiid_err(wiimote, "Mutex initialization error (state mutex): %s", strerror(err));
goto ERR_HND;
}
state_mutex_init = 1;
err = pthread_mutex_init(&wiimote->rw_mutex, NULL);
if (err) {
cwiid_err(wiimote, "Mutex initialization error (rw mutex): %s", strerror(err));
goto ERR_HND;
}
rw_mutex_init = 1;
err = pthread_mutex_init(&wiimote->rpt_mutex, NULL);
if (err) {
cwiid_err(wiimote, "Mutex initialization error (rpt mutex): %s", strerror(err));
goto ERR_HND;
}
rpt_mutex_init = 1;
/* Set rw_status before starting router thread */
wiimote->rw_status = RW_IDLE;
/* Launch interrupt socket listener and dispatch threads */
err = pthread_create(&wiimote->router_thread, NULL,
(void *(*)(void *))&router_thread, wiimote);
if (err) {
cwiid_err(wiimote, "Thread creation error (router thread): %s", strerror(err));
goto ERR_HND;
}
router_thread_init = 1;
err = pthread_create(&wiimote->status_thread, NULL,
(void *(*)(void *))&status_thread, wiimote);
if (err) {
cwiid_err(wiimote, "Thread creation error (status thread): %s", strerror(err));
goto ERR_HND;
}
status_thread_init = 1;
/* Success! Update state */
memset(&wiimote->state, 0, sizeof wiimote->state);
wiimote->mesg_callback = NULL;
cwiid_set_led(wiimote, 0);
cwiid_request_status(wiimote);
return wiimote;
ERR_HND:
if (wiimote) {
/* Close threads */
if (router_thread_init) {
pthread_cancel(wiimote->router_thread);
err = pthread_join(wiimote->router_thread, &pthread_ret);
if (err) {
cwiid_err(wiimote, "Thread join error (router thread): %s", strerror(err));
}
else if (!((pthread_ret == PTHREAD_CANCELED) &&
(pthread_ret == NULL))) {
cwiid_err(wiimote, "Bad return value from router thread");
}
}
if (status_thread_init) {
pthread_cancel(wiimote->status_thread);
err = pthread_join(wiimote->status_thread, &pthread_ret);
if (err) {
cwiid_err(wiimote, "Thread join error (status thread): %s", strerror(err));
}
else if (!((pthread_ret == PTHREAD_CANCELED) && (pthread_ret == NULL))) {
cwiid_err(wiimote, "Bad return value from status thread");
}
}
/* Close Pipes */
if (mesg_pipe_init) {
if (close(wiimote->mesg_pipe[0]) || close(wiimote->mesg_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (mesg pipe): %s", strerror(errno));
}
}
if (status_pipe_init) {
if (close(wiimote->status_pipe[0]) ||
close(wiimote->status_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (status pipe): %s", strerror(errno));
}
}
if (rw_pipe_init) {
if (close(wiimote->rw_pipe[0]) || close(wiimote->rw_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (rw pipe): %s", strerror(errno));
}
}
/* Destroy Mutexes */
if (state_mutex_init) {
err = pthread_mutex_destroy(&wiimote->state_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (state mutex): %s", strerror(err));
}
}
if (rw_mutex_init) {
err = pthread_mutex_destroy(&wiimote->rw_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (rw mutex): %s", strerror(err));
}
}
if (rpt_mutex_init) {
err = pthread_mutex_destroy(&wiimote->rpt_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (rpt mutex): %s", strerror(err));
}
}
free(wiimote);
}
return NULL;
}
//cwiid_mesg_callback_t cwiid_callback;
void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count, union cwiid_mesg mesg[], struct timespec *timestamp)
{
int i, j;
int valid_source;
int servo_steering, servo_accel, servo_speed;
unsigned int btn;
for (i=0; i < mesg_count; i++)
{
//printf ( "wii msg%d\n", mesg[i].type );
switch (mesg[i].type) {
case CWIID_MESG_STATUS:
printf("Status Report: battery=%d\n", mesg[i].status_mesg.battery);
break;
case CWIID_MESG_BTN:
//printf("Button Report: %.4X\n", mesg[i].btn_mesg.buttons);
btn = mesg[i].btn_mesg.buttons;
if ( btn & CWIID_BTN_PLUS )
{
printf ("wii: get perm\n" );
servo_getperm ( SERVO_PERM_WII, 0 );
wii_watchdog = 0;
}
if ( btn & CWIID_BTN_MINUS )
{
if (as==0)
as = 1;
else
as = 0;
printf ("wii: enable/disable schlupf to %d\n", as);
servo_as ( as, 0, SERVO_PERM_WII, 0 );
wii_watchdog = 0;
}
if ( btn & CWIID_BTN_HOME )
{
if (speedacc==0)
speedacc = 1;
else
speedacc = 0;
printf ("wii: switch speed/acc to %d\n", speedacc);
servo_setspeedacc ( speedacc, 0, SERVO_PERM_WII, 0 );
wii_watchdog = 0;
}
if ( btn & CWIID_BTN_A )
{
if ( accmode == WII_ACCMODE_TILT )
{
accmode = WII_ACCMODE_BTN;
cwiid_set_led(wiimote, 0x02);
} else {
accmode = WII_ACCMODE_TILT;
cwiid_set_led(wiimote, 0x01);
}
}
if ( accmode == WII_ACCMODE_BTN )
{
if ( (btn & CWIID_BTN_1) && (btn &CWIID_BTN_2) )
{
servo_setservo ( 0, 7000, 0, SERVO_PERM_WII, 0 );
wii_watchdog = 0;
//printf ( "AB\n" );
} else {
if ( btn & (CWIID_BTN_2) )
{
//printf ( "B\n" );
servo_setservo ( 0, 8000, 0, SERVO_PERM_WII, 0 );
wii_watchdog = 0;
} else if ( btn & (CWIID_BTN_1) ) {
//printf ( "A\n" );
servo_setservo ( 0, 5500, 0, SERVO_PERM_WII, 0 );
wii_watchdog = 0;
} else {
if ( btn & CWIID_BTN_LEFT)
{
servo_setservo ( 0, 2000, 0, SERVO_PERM_WII, 0 );
wii_watchdog = 0;
} else {
//printf ( "0\n" );
servo_setservo ( 0, 4000, 0, SERVO_PERM_WII, 0 );
wii_watchdog = 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]);
gettimeofday(&t2,NULL);
diff = ((t2.tv_sec)*1000000+(t2.tv_usec))
- ((t1.tv_sec)*1000000+(t1.tv_usec));
if ( (diff < 100000) )
{
//printf ( "N: servo: Only %010.0fus have passed since last write; Ignoring this command\n", diff );
} else {
//printf ( "N: servo: %010.0fus have passed since last write; OK\n", diff );
gettimeofday(&t1,NULL);
if (speedacc == 1 )
{
//speed mode
servo_speed = wii_to_speed ( mesg[i].acc_mesg.acc[CWIID_X], 0 );
servo_steering = wii_to_servo ( mesg[i].acc_mesg.acc[CWIID_Y], 0 );
servo_setspeedraw ( servo_speed, 0, SERVO_PERM_WII, 0 );
wii_watchdog = 0;
servo_setservo ( 1, servo_steering, 0, SERVO_PERM_WII, 0 );
wii_watchdog = 0;
} else {
//accell mode
servo_accel = wii_to_servo ( mesg[i].acc_mesg.acc[CWIID_X], 0 );
servo_steering = wii_to_servo ( mesg[i].acc_mesg.acc[CWIID_Y], 0 );
//printf ( "servo: accel: %d, steer: %d\n", servo_accel, servo_steering );
if ( accmode == WII_ACCMODE_TILT )
{
servo_setservo ( 0, servo_accel, 0, SERVO_PERM_WII, 0 );
wii_watchdog = 0;
}
servo_setservo ( 1, servo_steering, 0, SERVO_PERM_WII, 0 );
wii_watchdog = 0;
}
}
break;
case CWIID_MESG_ERROR:
wii_connected = 0;
//reset servos
servo_setservo ( 0, 4000, 0, SERVO_PERM_WII, 0 );
servo_setservo ( 1, 4000, 0, SERVO_PERM_WII, 0 );
fprintf(stderr, "E: wii: received error, disconnecting...\n");
wii_close();
break;
} //switch msg.type
} //foreach msg
}
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;
}