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 int Wiimote_init(Wiimote* self, PyObject* args, PyObject *kwds)
{
static char *kwlist[] = {"bdaddr", "flags", NULL};
PyObject *PyObj;
cwiid_wiimote_t *wiimote = NULL;
char *str_bdaddr = NULL;
bdaddr_t bdaddr;
int flags = 0;
/* Overloaded function - if a single CObject is passed in, it's
* an existing CObject. Otherwise, create a new one */
if (PyTuple_Size(args) == 1) {
PyObj = PyTuple_GET_ITEM(args, 0);
if (PyCObject_Check(PyObj)) {
wiimote = PyCObject_AsVoidPtr(PyObj);
self->close_on_dealloc = 0;
}
}
if (!wiimote) {
if (!PyArg_ParseTupleAndKeywords(args, kwds, "|si:cwiid.Wiimote.init",
kwlist, &str_bdaddr, &flags)) {
return -1;
}
if (str_bdaddr) {
if (str2ba(str_bdaddr, &bdaddr)) {
PyErr_SetString(PyExc_ValueError, "bad bdaddr");
return -1;
}
}
else {
bdaddr = *BDADDR_ANY;
}
Py_BEGIN_ALLOW_THREADS
wiimote = cwiid_open(&bdaddr, flags);
Py_END_ALLOW_THREADS
if (!wiimote) {
PyErr_SetString(PyExc_RuntimeError,
"Error opening wiimote connection");
return -1;
}
else {
self->close_on_dealloc = 1;
}
}
cwiid_set_data(wiimote, self);
self->wiimote = 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;
/* Make stdout unbuffered, which is useful for piping the output of
* this program into a timestamping utility, such as tai64n(1) */
setvbuf(stdout, NULL, _IOLBF, 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;
}
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");
exit(1);
}
if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) {
fprintf(stderr, "Unable to set message callback\n");
exit(1);
}
toggle_bit(rpt_mode, CWIID_RPT_ACC);
toggle_bit(rpt_mode, CWIID_RPT_BTN);
set_rpt_mode(wiimote, rpt_mode);
if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) {
fprintf(stderr, "Unable to set message flag\n");
exit(1);
}
while(1) {
sleep(1);
}
}
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;
}
Wiimote::Wiimote(bdaddr_t addr,QObject *parent) : QObject(parent), buttons_old(0), addr(addr) {
Q_ASSERT(instance == NULL);
qDebug("setting up cwiid");
{
cwiid_set_err(cwiid_err_callback);
instance = cwiid_open(&addr,0);
qinstance = this;
Q_ASSERT(instance);
cwiid_set_mesg_callback(instance,cwiid_msg_callback);
cwiid_enable(instance,CWIID_FLAG_MESG_IFC);
cwiid_command(instance,CWIID_CMD_LED,CWIID_LED2_ON | CWIID_LED3_ON);
//cwiid_command(instance,CWIID_CMD_RPT_MODE,CWIID_RPT_EXT | CWIID_RPT_BTN | CWIID_RPT_ACC | CWIID_RPT_STATUS | CWIID_RPT_IR);
cwiid_command(instance,CWIID_CMD_RPT_MODE,CWIID_RPT_EXT | CWIID_RPT_BTN | CWIID_RPT_STATUS | CWIID_RPT_ACC);
}
}
void wiimoteController::setup(){
bdaddr_t bdaddr;
str2ba("00:1A:E9:44:40:E5", &bdaddr);
if ((wiimote = cwiid_open(&bdaddr, CWIID_FLAG_MESG_IFC)) == NULL){
cout << "Unable to connect to the Wiimote!" << endl;
}
else{
cout << "Wiimote connected!" << endl;
//cwiid_set_rumble(wiimote, true);
//sleep(1);
//cwiid_set_rumble(wiimote, false);
//cwiid_enable(wiimote, CWIID_FLAG_NONBLOCK);
//cwiid_enable(wiimote, CWIID_FLAG_CONTINUOUS);
cwiid_command(wiimote, CWIID_CMD_RPT_MODE, CWIID_RPT_IR);
cwiid_set_mesg_callback(wiimote, messageCallback);
}
}
/* call to init an uninitialized wiimote device
* typically called once at setup
* turns the IR leds on
* this function may block for up to 3 seconds
* a return value < 0 indicates error */
int ltr_int_tracker_init(struct camera_control_block *ccb) {
(void) ccb;
bdaddr_t bdaddr;
bdaddr = *BDADDR_ANY;
//fprintf(stderr, "Put Wiimote in discoverable mode now (press 1+2)...\n");
if (!(gWiimote = cwiid_open(&bdaddr, 0))) {
//fprintf(stderr, "Wiimote not found\n");
return -1;
} else {
set_leds_running();
cwiid_set_rpt_mode(gWiimote, CWIID_RPT_STATUS | CWIID_RPT_IR);
ltr_int_log_message("Wiimote connected\n");
}
return 0;
}
void runWiimote() {
//cwiid_set_err(err);
unsigned char mesg;
mesg = 0;
this->led_state = 0;
this->rumbleM = 0;
this->exit = 0;
//Connect to address given on command-line, if present
bdaddr = *BDADDR_ANY;
usleep(100);
cout << endl;
//Connect to the wiimote
cout << "*******************************************************" << endl;
printf("* Put Wiimote in discoverable mode now (press 1+2)... *\n");
cout << "*******************************************************" << endl;
if (!(wiimote = cwiid_open(&bdaddr, CWIID_FLAG_MESG_IFC))) {
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");
}else{
cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &nc_cal);
}
uint8_t rpt_mode;
rpt_mode = CWIID_RPT_STATUS | CWIID_RPT_BTN;
cwiid_set_rpt_mode(wiimote, rpt_mode);
cout << endl;
cout << "Info: wiimote ready to use" << endl;
}
// Creadora del device
TPFC_device_wiimote::TPFC_device_wiimote(int ident, string bta):TPFC_device(ident){
// creamos el bufer de datos
data = new TrackingPFC_data(TrackingPFC_data::TPFCDATA2D);
// sobreescribimos el mensaje de error por defecto con el nuestro, para evitar spam
cwiid_set_err(err);
// comprobamos si tenemos una direccion asignada
if (bta.compare("")==0){
// conectarse al primer wiimote que se encuentre
bdaddr = *BDADDR_ANY;// bluetooth device address
}else{
// conectarse al wiimote con direccion == bta
str2ba(bta.c_str(), &bdaddr);
}
// Conectar los wiimotes
printf("Pon el wiimote en modo discoverable (pulsa 1+2)...\n");
wiimote=NULL;
while (!wiimote){
if ( !(wiimote = cwiid_open(&bdaddr, 0)) ){
printf("Esperando al wiimote (pulsa 1+2)...\n");
}
}
// registramos el wiimote para poder identificar despues los callbacks
registerwiimote(wiimote, this);
// registramos nuestro callback con el wiimote
if (cwiid_set_mesg_callback(wiimote, callback)) {
fprintf(stderr, "No se ha podido registrar el callback\n");
}
// activamos el paso de mensajes desde el wiimote
if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) {
fprintf(stderr, "Error activando los mensajes\n");
}
// configuramos el report mode
unsigned char rpt_mode = 0;
toggle_bit(rpt_mode, CWIID_RPT_IR);
set_rpt_mode(wiimote, rpt_mode);
printf("Wiimote conectado!\n");
}
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;
}
}
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;
}
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;
}
/*!
@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;
}
}
}
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);
}
DEFINE_THREAD_ROUTINE(wiimote_logic, data){
//wiimote connection variables
static bdaddr_t bdaddr = {{0}};
static cwiid_wiimote_t *wiimote = NULL;
union cwiid_mesg *msg = NULL;
struct timespec timestamp;
int wiimote_connected = 0;
int drone_in_sight = 0;
int trigger_button = 0;
int number_of_led = 0;
int recharger_in_sight = 0;
int recharging_button = 0;
int bullets = magazine_capacity;
int i = 0;
int j = 0;
int msg_count = 0;
struct timespec shot_rumble_time;
shot_rumble_time.tv_sec = 1;
shot_rumble_time.tv_nsec = 0000000;
struct timespec recharging_time;
recharging_time.tv_sec = 10;
recharging_time.tv_nsec = 0000000;
while(game_active){
//CONNECT TO THE WIIMOTE
if(wiimote_connected == 0) {
if( ! (wiimote = cwiid_open(&bdaddr, CWIID_FLAG_MESG_IFC)) ) {
printf("Unable to connect to wiimote\n");
} else {
wiimote_connected = 1;
printf("Wiimote found\n");
cwiid_command(wiimote, CWIID_CMD_LED, CWIID_LED1_ON|CWIID_LED2_ON|CWIID_LED3_ON|CWIID_LED4_ON);
cwiid_command(wiimote, CWIID_CMD_RPT_MODE, CWIID_RPT_IR|CWIID_RPT_BTN);
}
//ALREADY CONNECTED
} else {
if(match_active){
//--- RESET VARIABLES ---//
number_of_led = 0;
drone_in_sight = 0;
recharger_in_sight = 0;
trigger_button = 0;
msg_count = 0;
//--- GET INPUTS FROM THE WIIMOTE ---//
//get messages (blocking)
cwiid_get_mesg(wiimote, &msg_count, &msg, ×tamp);
//scan the messages for the event "pression of trigger_button" or "pression of recharging_button"
//and to count the number of IR leds found
//NOTE: the wiimote find false positive (sometimes 1led == 4leds :O)
//NOTE: the wiimote is REALLY sensitive to sun light
for(i = 0; i < msg_count; i++){
if(msg[i].type == CWIID_MESG_BTN){
//is button B pressed?
if(msg[i].btn_mesg.buttons == CWIID_BTN_B){
trigger_button = 1;
printf("SHOOT\n");
} else {
trigger_button = 0;
}
//is button A pressed?
if(msg[i].btn_mesg.buttons == CWIID_BTN_A){
recharging_button = 1;
printf("BUTTON A\n");
} else {
recharging_button = 0;
}
//TODO: this is here in case the ar.drone stop sending video update
if(msg[i].btn_mesg.buttons == CWIID_BTN_HOME){
printf("The program will shutdown...\n");
match_active = 0; //This tell the drone_logic thread to land the drone
game_active = 0; //This make all the threads exit the while loop
exit_program = 0; // Force ardrone_tool to close
// Sometimes, ardrone_tool might not finish properly.
//This happens mainly because a thread is blocked on a syscall, in this case, wait 5 seconds then kill the app
sleep(5);
exit(0);
}
}
//are there leds?
if(msg[i].type == CWIID_MESG_IR){
for(j = 0; j < CWIID_IR_SRC_COUNT; j++){
if(msg[i].ir_mesg.src[j].valid != 0){
number_of_led++;
}
}
if(number_of_led > 0){
printf("LEDS\n");
drone_in_sight = 1;
} else {
drone_in_sight = 0;
}
}
}
//--- WIIMOTE LOGIC ---//
//SHOOTING
if((bullets > 0) && trigger_button){
bullets--;
printf("lost one bullet\n");
//haptic feedback
cwiid_command(wiimote, CWIID_CMD_RUMBLE, 1);
nanosleep(&shot_rumble_time, NULL);
cwiid_command(wiimote, CWIID_CMD_RUMBLE, 0);
if(drone_in_sight){
vp_os_mutex_lock(&drone_score_mutex);
drone_lose_score = 1;
vp_os_mutex_unlock(&drone_score_mutex);
printf("DRONE HIT\n");
} else {
printf("DRONE MISSED!!\n");
}
//This is to limit the frequency of shooting (i.e. the gun need to load)
nanosleep(&shot_rumble_time, NULL);
//you can recharge only if you don't have bullets any more
} else if(bullets < 1){
if(recharging_button){
//TODO: How should I let the enemy know that the wiimote is full again?
nanosleep(&recharging_time, NULL);
bullets = magazine_capacity;
}
}
}
}
}
return C_OK;
}
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;
}
int main(int argc, char *argv[])
{
if (argc != 2) {
printf("Usage: %s 3, where 3 is the number of gestures to recognize\n", argv[0]);
exit(1);
}
n_gestures = atoi(argv[1]);
hmms = malloc(n_gestures * sizeof(HmmStateRef));
int n_states = n_gestures+3;
int n_obs = 17;
for(int i = 0; i < n_gestures; i++) {
hmms[i] = hmm_new(n_states, n_obs);
}
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;
/* Make stdout unbuffered, which is useful for piping the output of
* this program into a timestamping utility, such as tai64n(1) */
setvbuf(stdout, NULL, _IOLBF, 0);
cwiid_set_err(err);
#if WE_ACTUALLY_CARE_ABOUT_BLUETOOTH
/* Connect to address given on command-line, if present */
if (argc > 1) {
str2ba(argv[1], &bdaddr);
}
else {
#endif
bdaddr = *BDADDR_ANY;
#if WE_ACTUALLY_CARE_ABOUT_BLUETOOTH
}
#endif
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");
exit(1);
}
if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) {
fprintf(stderr, "Unable to set message callback\n");
exit(1);
}
toggle_bit(rpt_mode, CWIID_RPT_ACC);
toggle_bit(rpt_mode, CWIID_RPT_BTN);
set_rpt_mode(wiimote, rpt_mode);
if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) {
fprintf(stderr, "Unable to set message flag\n");
exit(1);
}
while(1) {
sleep(1);
}
}
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;
}
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;
}
// 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);
}
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;
}