void
WiimoteController::set_rumble_real(uint8_t left, uint8_t right)
{
if (left > 127 || right > 127)
{
cwiid_set_rumble(m_wiimote, 1);
}
else
{
cwiid_set_rumble(m_wiimote, 0);
}
}
virtual Ice::Int changeRumbleMode(const Ice::Current&) {
if (cwiid_set_rumble(wiimote, 1)) {
fprintf(stderr, "Error setting rumble\n");
}
return 1;
}
static void rumble(int flag)
{
static int localflag=0;
if (flag!=localflag){
localflag=flag;
cwiid_set_rumble(wiimote,flag);
}
}
void chkRumble_toggled(void)
{
if (wiimote) {
if (cwiid_set_rumble(wiimote,
gtk_check_menu_item_get_active(GTK_CHECK_MENU_ITEM(chkRumble)))) {
message(GTK_MESSAGE_ERROR, "error setting rumble",
GTK_WINDOW(winMain));
}
}
}
static int
Wiimote_set_rumble(Wiimote *self, PyObject *PyRumble, void *closure)
{
long rumble;
if (((rumble = PyInt_AsLong(PyRumble)) == -1) && PyErr_Occurred()) {
return -1;
}
if (cwiid_set_rumble(self->wiimote, (uint8_t)rumble)) {
PyErr_SetString(PyExc_AttributeError,
"Error setting wiimote rumble state");
return -1;
}
return 0;
}
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;
}
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;
}
ErrorID_t acceleration_mode(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
typedef enum WiimoteAccelStateType
{
WIIMOTE_ACCEL_WAIT_FOR_START,
WIIMOTE_ACCEL_READ_CAL_DATA,
WIIMOTE_ACCEL_READ_DATA,
WIIMOTE_ACCEL_WAIT_FOR_EXIT,
WIIMOTE_ACCEL_EXIT,
} WiimoteAccelStateType;
const uint8_t RETRY_VALUE = 2;
uint8_t retry_count = 0;
ErrorID_t error_flag = ERR_NONE;
WiimoteAccelStateType state = WIIMOTE_ACCEL_WAIT_FOR_START;
debug_print("Entering acceleration mode...\n");
write_status_led(STATUS_LED_OFF, 0);
for (;;)
{
switch (state)
{
case WIIMOTE_ACCEL_WAIT_FOR_START:
if (0 > wait_for_wiimotedata(wiimote_status, INFINITE_TIMEOUT))
{
debug_print("No start signal received\n");
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_ACCEL_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_A))
{
state = WIIMOTE_ACCEL_READ_CAL_DATA;
}
break;
case WIIMOTE_ACCEL_READ_CAL_DATA:
error_flag = WII_ERROR_ACCEL_CAL;
do
{
if (0
== cwiid_get_acc_cal(
wiimote,
CWIID_EXT_NONE,
(struct acc_cal *) &wiimote_status->accel_cal_data))
{
error_flag = ERR_NONE;
break;
}
} while (retry_count++ < RETRY_VALUE);
if (error_flag != ERR_NONE)
{
debug_print("Cannot read cal data\n");
return error_flag;
}
if (0 > cwiid_set_rpt_mode(wiimote, CWIID_RPT_ACC | CWIID_RPT_BTN))
{
debug_print("Cannot set report mode to ACCEL\n");
return false;
}
set_lcd(0, "Accel Mode...");
set_lcd(1, "P = %4d R = %4d", 0, 0);
state = WIIMOTE_ACCEL_READ_DATA;
break;
case WIIMOTE_ACCEL_READ_DATA:
if (0 > wait_for_wiimotedata(wiimote_status, INFINITE_TIMEOUT))
{
debug_print("@%u: WII_ERROR_DATA_TIMEOUT\n", get_tick_count());
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_ACCEL_EXIT;
}
else
{
if (wiimote_status->button_data & CWIID_BTN_A)
{
state = WIIMOTE_ACCEL_WAIT_FOR_EXIT;
}
else
{
error_flag = computer_motor_levels_accel(wiimote_status);
set_lcd(1, "P = %4d R = %4d",
wiimote_status->accel_computed_data.pitch / 100,
wiimote_status->accel_computed_data.roll / 100);
if (ERR_EXEC == error_flag)
cwiid_set_rumble(wiimote, true);
else
{
cwiid_set_rumble(wiimote, false);
if (error_flag < 0)
{
debug_print("@%u: CONTROL CAR COMM ERROR: %d\n",
get_tick_count(), error_flag);
state = WIIMOTE_ACCEL_EXIT;
}
}
}
}
break;
case WIIMOTE_ACCEL_WAIT_FOR_EXIT:
stop_motors();
if (0 < wait_for_wiimotedata(wiimote_status, INFINITE_TIMEOUT))
{
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_ACCEL_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_A))
{
state = WIIMOTE_ACCEL_EXIT;
}
break;
case WIIMOTE_ACCEL_EXIT:
debug_print("Exiting acceleration mode.\n\n");
return error_flag;
}
}
return ERR_NONE;
}
/*!
@brief Determines motor parameters from IR data.
*/
ErrorID_t infrared_mode(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
typedef enum WiimoteInfraredStateType
{
WIIMOTE_INFRARED_WAIT_FOR_START,
WIIMOTE_INFRARED_ENABLE,
WIIMOTE_INFRARED_READ_DATA,
WIIMOTE_INFRARED_WAIT_FOR_EXIT,
WIIMOTE_INFRARED_EXIT,
} WiimoteInfraredStateType;
ErrorID_t error_flag = ERR_NONE;
bool last_valid = false;
bool valid_points = false;
WiimoteInfraredStateType state = WIIMOTE_INFRARED_WAIT_FOR_START;
write_status_led(STATUS_LED_OFF, 0);
for (;;)
{
switch (state)
{
case WIIMOTE_INFRARED_WAIT_FOR_START:
if (0 < wait_for_wiimotedata(wiimote_status, 500))
{
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_INFRARED_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_B))
{
state = WIIMOTE_INFRARED_ENABLE;
}
break;
case WIIMOTE_INFRARED_ENABLE:
set_ir_led(STATUS_LED_ON);
debug_print("Setting IR Report\n");
if (0 < cwiid_set_rpt_mode(wiimote, CWIID_RPT_IR | CWIID_RPT_BTN))
{
debug_print("Cannot set report mode to IR\n");
return false;
}
set_lcd(0, "Infrared Mode");
set_lcd(1, "Point wiimote at car");
write_status_led(STATUS_LED_ON, 0);
state = WIIMOTE_INFRARED_READ_DATA;
break;
case WIIMOTE_INFRARED_READ_DATA:
if (0 < wait_for_wiimotedata(wiimote_status, 500))
{
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_INFRARED_EXIT;
}
else if (wiimote_status->button_data & CWIID_BTN_B)
{
state = WIIMOTE_INFRARED_WAIT_FOR_EXIT;
}
else
{
error_flag = WiiComputeMotorLevelsInfrared(wiimote_status,
&valid_points);
if (valid_points != last_valid)
{
write_status_led(
valid_points ? STATUS_LED_OFF : STATUS_LED_ON, 0);
set_lcd(
1,
valid_points ?
"LED's seen" : "Point wiimote at car");
}
last_valid = valid_points;
if (ERR_EXEC == error_flag)
cwiid_set_rumble(wiimote, true);
else
{
cwiid_set_rumble(wiimote, false);
if (error_flag < 0)
state = WIIMOTE_INFRARED_EXIT;
}
}
break;
case WIIMOTE_INFRARED_WAIT_FOR_EXIT:
if (0 < wait_for_wiimotedata(wiimote_status, 500))
{
state = WIIMOTE_INFRARED_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_B))
{
state = WIIMOTE_INFRARED_EXIT;
}
break;
case WIIMOTE_INFRARED_EXIT:
stop_motors();
write_status_led(STATUS_LED_OFF, 0);
set_ir_led(false);
return error_flag;
break;
break;
}
}
return true;
}
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;
}
int cwiid_close(cwiid_wiimote_t *wiimote)
{
void *pthread_ret;
int err;
/* Stop rumbling, otherwise wiimote continues to rumble for
few seconds after closing the connection! There should be no
need to check if stopping fails: we are closing the connection
in any case. */
if (wiimote->state.rumble) {
cwiid_set_rumble(wiimote, 0);
}
/* Cancel and join router_thread and status_thread */
if (pthread_cancel(wiimote->router_thread)) {
/* if thread quit abnormally, would have printed it's own error */
}
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 (pthread_cancel(wiimote->status_thread)) {
/* if thread quit abnormally, would have printed it's own error */
}
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");
}
if (wiimote->mesg_callback) {
if (cancel_mesg_callback(wiimote)) {
/* prints it's own errors */
}
}
if (cancel_rw(wiimote)) {
/* prints it's own errors */
}
/* Close sockets */
if (close(wiimote->int_socket)) {
cwiid_err(wiimote, "Socket close error (interrupt socket): %s", strerror(errno));
}
if (close(wiimote->ctl_socket)) {
cwiid_err(wiimote, "Socket close error (control socket): %s", strerror(errno));
}
/* Close Pipes */
if (close(wiimote->mesg_pipe[0]) || close(wiimote->mesg_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (mesg pipe): %s", strerror(errno));
}
if (close(wiimote->status_pipe[0]) || close(wiimote->status_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (status pipe): %s", strerror(errno));
}
if (close(wiimote->rw_pipe[0]) || close(wiimote->rw_pipe[1])) {
cwiid_err(wiimote, "Pipe close error (rw pipe): %s", strerror(errno));
}
/* Destroy mutexes */
err = pthread_mutex_destroy(&wiimote->state_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (state): %s", strerror(err));
}
err = pthread_mutex_destroy(&wiimote->rw_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (rw): %s", strerror(err));
}
err = pthread_mutex_destroy(&wiimote->rpt_mutex);
if (err) {
cwiid_err(wiimote, "Mutex destroy error (rpt): %s", strerror(err));
}
free(wiimote);
return 0;
}
void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
union cwiid_mesg mesg[], struct timespec *ts)
{
static double ff_start, ff_diff;
static double fp_start, fp_diff;
double a_x, a_y, a_z, accel;
struct cwiid_acc_mesg *am;
for (int i = 0; i < mesg_count; i++) {
if (mesg[i].type == CWIID_MESG_BTN) {
if (mesg[i].btn_mesg.buttons & CWIID_BTN_LEFT)
set_led_fun(cur_mode - 1);
if (mesg[i].btn_mesg.buttons & CWIID_BTN_RIGHT)
set_led_fun(cur_mode + 1);
if (mesg[i].btn_mesg.buttons & CWIID_BTN_PLUS)
cnt_max -= (cnt_max > 1 ? 1 : 0);
if (mesg[i].btn_mesg.buttons & CWIID_BTN_MINUS)
cnt_max += 1;
if (mesg[i].btn_mesg.buttons & CWIID_BTN_UP) {
auto_mode = !auto_mode;
if (auto_mode) {
x_max = X_MAXP;
puts("Auto mode enabled");
puts("- Fap to vibrate");
puts("- Down arrow to toggle vibration");
puts("- Tilt to change animation speed");
} else {
puts("Auto mode disabled");
puts("- Down arrow to vibrate");
puts("- Left/Right to change animation");
puts("- +/- to change animation speed");
}
}
if (mesg[i].btn_mesg.buttons & CWIID_BTN_DOWN) {
if (auto_mode)
auto_rumble = !auto_rumble;
else if (!rumble)
cwiid_set_rumble(wiimote, (rumble = 1));
}
else if (rumble && !auto_mode)
cwiid_set_rumble(wiimote, (rumble = 0));
if (mesg[i].btn_mesg.buttons & CWIID_BTN_HOME) {
exit(0);
}
}
/* else if ((mesg[i].type == CWIID_MESG_MOTIONPLUS) && auto_mode ) {
if ((mesg[i].motionplus_mesg.angle_rate[2] < 8000) && !fp_start)
fp_start = ((uint64_t)ts->tv_sec * 1000000000) + ts->tv_nsec;
else if ((mesg[i].motionplus_mesg.angle_rate[2] > 8200) && fp_start) {
fp_diff = ((((uint64_t)ts->tv_sec * 1000000000)
+ ts->tv_nsec
- ff_start) / 1000000000);
printf("mpdel_t %.3fs - Fell approx. %.2fm\n", ff_diff,
(double)((9.81 * (double)(fp_diff) * (double)(fp_diff)) / (double)2));
fp_start = 0;
}
}
*/ else if ((mesg[i].type == CWIID_MESG_ACC) && auto_mode) {
am = &mesg[i].acc_mesg;
a_x = ((double)am->acc[CWIID_X] - wm_cal.zero[CWIID_X]) /
(wm_cal.one[CWIID_X] - wm_cal.zero[CWIID_X]);
a_y = ((double)am->acc[CWIID_Y] - wm_cal.zero[CWIID_Y]) /
(wm_cal.one[CWIID_Y] - wm_cal.zero[CWIID_Y]);
a_z = ((double)am->acc[CWIID_Z] - wm_cal.zero[CWIID_Z]) /
(wm_cal.one[CWIID_Z] - wm_cal.zero[CWIID_Z]);
accel = sqrt(pow(a_x,2)+pow(a_y,2)+pow(a_z,2));
if ((accel < 0.07) && !ff_start)
ff_start = ((uint64_t)ts->tv_sec * 1000000000) + ts->tv_nsec;
else if ((accel > 1.0) && ff_start) {
ff_diff = ((((uint64_t)ts->tv_sec * 1000000000)
+ ts->tv_nsec
- ff_start) / 1000000000);
printf("delta_t %.3fs - Fell approx. %.2fm\n", ff_diff,
(double)((9.81 * (double)(ff_diff) * (double)(ff_diff)) / (double)2));
ff_start = 0;
}
if (auto_rumble && (accel > 1.5)) {
if (!rumble)
cwiid_set_rumble(wiimote, (rumble = 1));
}
else {
if (rumble)
cwiid_set_rumble(wiimote, (rumble = 0));
}
if (mesg[i].acc_mesg.acc[CWIID_X] < 123) {
if (cur_mode != 0)
set_led_fun(0);
cnt_max = (mesg[i].acc_mesg.acc[CWIID_X] - 95) / 2;
if (cnt_max < 2)
cnt_max = 2;
}
else {
if (cur_mode != 1)
set_led_fun(1);
cnt_max = (150 - mesg[i].acc_mesg.acc[CWIID_X]) / 2;
if (cnt_max < 2)
cnt_max = 2;
}
}
}
}
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;
}