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;
}
}
virtual Ice::Int getBatteryStatus(const Ice::Current&) {
struct cwiid_state state; // wiimote state
if (cwiid_get_state(wiimote, &state)) {
fprintf(stderr, "Error getting state\n");
}
return (int) (100.0 * state.battery / CWIID_BATTERY_MAX);
}
/* Connect is designed to be run in a different thread as it only
exits if wiiremote is either disabled or a connection is made*/
bool CWiiRemote::Connect()
{
#ifndef _DEBUG
cwiid_set_err(ErrorCallback);
#endif
while (!m_connected)
{
g_Ping->Send(m_Socket, m_MyAddr);
int flags = 0;
ToggleBit(flags, CWIID_FLAG_MESG_IFC);
ToggleBit(flags, CWIID_FLAG_REPEAT_BTN);
m_wiiremoteHandle = cwiid_connect(&m_btaddr, flags);
if (m_wiiremoteHandle != NULL)
{
SetupWiiRemote();
// get battery state etc.
cwiid_state wiiremote_state;
int err = cwiid_get_state(m_wiiremoteHandle, &wiiremote_state);
if (!err)
{
char Mesg[1024];
sprintf(Mesg, "%i%% battery remaining", static_cast<int>(((float)(wiiremote_state.battery)/CWIID_BATTERY_MAX)*100.0));
CPacketNOTIFICATION notification("Wii Remote connected", Mesg, ICON_PNG, g_BluetoothIconPath.c_str());
notification.Send(m_Socket, m_MyAddr);
}
else
{
printf("Problem probing for status of WiiRemote; cwiid_get_state returned non-zero\n");
CPacketLOG log(LOGNOTICE, "Problem probing for status of WiiRemote; cwiid_get_state returned non-zero");
log.Send(m_Socket, m_MyAddr);
CPacketNOTIFICATION notification("Wii Remote connected", "", ICON_PNG, g_BluetoothIconPath.c_str());
notification.Send(m_Socket, m_MyAddr);
}
#ifdef CWIID_OLD
/* CheckIn to say that this is the last msg, If this isn't called it could give issues if we Connects -> Disconnect and then try to connect again
the CWIID_OLD hack would automaticly disconnect the wiiremote as the lastmsg is too old. */
CheckIn();
#endif
m_connected = true;
CPacketLOG log(LOGNOTICE, "Sucessfully connected a WiiRemote");
log.Send(m_Socket, m_MyAddr);
return true;
}
//Here's a good place to have a quit flag check...
}
return false;
}
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;
}
static PyObject *Wiimote_get_state(Wiimote* self, void *closure)
{
struct cwiid_state state;
PyObject *PyState;
(void)closure;
if (!self->wiimote) {
SET_CLOSED_ERROR;
return NULL;
}
if (cwiid_get_state(self->wiimote, &state)) {
PyErr_SetString(PyExc_IOError, "get state error");
return NULL;
}
PyState = Py_BuildValue("{s:B,s:B,s:B,s:B,s:i,s:i}",
"rpt_mode", state.rpt_mode,
"led", state.led,
"rumble", state.rumble,
"battery", state.battery,
"ext_type", state.ext_type,
"error", state.error);
if (state.rpt_mode & CWIID_RPT_BTN) {
PyObject *PyBtn = Py_BuildValue("I", state.buttons);
if (!PyBtn) {
Py_DECREF(PyState);
return NULL;
}
if (PyDict_SetItemString(PyState, "buttons", PyBtn)) {
Py_DECREF(PyState);
Py_DECREF(PyBtn);
return NULL;
}
Py_DECREF(PyBtn);
}
if (state.rpt_mode & CWIID_RPT_ACC) {
PyObject *PyAcc = Py_BuildValue("(B,B,B)",
state.acc[CWIID_X],
state.acc[CWIID_Y],
state.acc[CWIID_Z]);
if (!PyAcc) {
Py_DECREF(PyState);
return NULL;
}
if (PyDict_SetItemString(PyState, "acc", PyAcc)) {
Py_DECREF(PyState);
Py_DECREF(PyAcc);
return NULL;
}
Py_DECREF(PyAcc);
}
if (state.rpt_mode & CWIID_RPT_IR) {
int i;
PyObject *PyIr = PyList_New(CWIID_IR_SRC_COUNT);
if (!PyIr) {
Py_DECREF(PyState);
return NULL;
}
if (PyDict_SetItemString(PyState, "ir_src", PyIr)) {
Py_DECREF(PyState);
Py_DECREF(PyIr);
return NULL;
}
Py_DECREF(PyIr);
for (i=0; i < CWIID_IR_SRC_COUNT; i++) {
PyObject *PyIrSrc;
PyObject *PySize;
if (state.ir_src[i].valid) {
PyIrSrc = Py_BuildValue("{s:(I,I)}",
"pos",
state.ir_src[i].pos[CWIID_X],
state.ir_src[i].pos[CWIID_Y]);
if (!PyIrSrc) {
Py_DECREF(PyState);
return NULL;
}
if (state.ir_src[i].size != -1) {
if (!(PySize = PyInt_FromLong(
(long)state.ir_src[i].size))) {
Py_DECREF(PyState);
Py_DECREF(PyIrSrc);
return NULL;
}
if (PyDict_SetItemString(PyIrSrc, "size", PySize)) {
Py_DECREF(PyState);
Py_DECREF(PyIrSrc);
Py_DECREF(PySize);
return NULL;
}
Py_DECREF(PySize);
}
}
else {
Py_INCREF(PyIrSrc = Py_None);
}
PyList_SET_ITEM(PyIr, i, PyIrSrc);
}
}
switch (state.ext_type) {
PyObject *PyExt;
case CWIID_EXT_NUNCHUK:
if (state.rpt_mode & CWIID_RPT_NUNCHUK) {
PyExt = Py_BuildValue("{s:(B,B),s:(B,B,B),s:I}",
"stick",
state.ext.nunchuk.stick[CWIID_X],
state.ext.nunchuk.stick[CWIID_Y],
"acc",
state.ext.nunchuk.acc[CWIID_X],
state.ext.nunchuk.acc[CWIID_Y],
state.ext.nunchuk.acc[CWIID_Z],
"buttons", state.ext.nunchuk.buttons);
if (!PyExt) {
Py_DECREF(PyState);
return NULL;
}
if (PyDict_SetItemString(PyState, "nunchuk", PyExt)) {
Py_DECREF(PyState);
Py_DECREF(PyExt);
return NULL;
}
Py_DECREF(PyExt);
}
break;
case CWIID_EXT_CLASSIC:
if (state.rpt_mode & CWIID_RPT_CLASSIC) {
PyExt = Py_BuildValue("{s:(B,B),s:(B,B),s:B,s:B,s:I}",
"l_stick",
state.ext.classic.l_stick[CWIID_X],
state.ext.classic.l_stick[CWIID_Y],
"r_stick",
state.ext.classic.r_stick[CWIID_X],
state.ext.classic.r_stick[CWIID_Y],
"l", state.ext.classic.l,
"r", state.ext.classic.r,
"buttons", state.ext.classic.buttons);
if (!PyExt) {
Py_DECREF(PyState);
return NULL;
}
if (PyDict_SetItemString(PyState, "classic", PyExt)) {
Py_DECREF(PyState);
Py_DECREF(PyExt);
return NULL;
}
Py_DECREF(PyExt);
}
break;
case CWIID_EXT_BALANCE:
if (state.rpt_mode & CWIID_RPT_BALANCE) {
PyExt = Py_BuildValue("{s:I,s:I,s:I,s:I}",
"right_top",
state.ext.balance.right_top,
"right_bottom",
state.ext.balance.right_bottom,
"left_top",
state.ext.balance.left_top,
"left_bottom",
state.ext.balance.left_bottom);
if (!PyExt) {
Py_DECREF(PyState);
return NULL;
}
if (PyDict_SetItemString(PyState, "balance", PyExt)) {
Py_DECREF(PyState);
Py_DECREF(PyExt);
return NULL;
}
Py_DECREF(PyExt);
}
break;
case CWIID_EXT_MOTIONPLUS:
if (state.rpt_mode & CWIID_RPT_MOTIONPLUS) {
PyExt = Py_BuildValue("{s:(I,I,I),s:(I,I,I)}",
"angle_rate",
state.ext.motionplus.angle_rate[CWIID_PHI],
state.ext.motionplus.angle_rate[CWIID_THETA],
state.ext.motionplus.angle_rate[CWIID_PSI],
"low_speed",
state.ext.motionplus.low_speed[CWIID_PHI],
state.ext.motionplus.low_speed[CWIID_THETA],
state.ext.motionplus.low_speed[CWIID_PSI]);
if (!PyExt) {
Py_DECREF(PyState);
return NULL;
}
if (PyDict_SetItemString(PyState, "motionplus", PyExt)) {
Py_DECREF(PyState);
Py_DECREF(PyExt);
return NULL;
}
Py_DECREF(PyExt);
}
break;
default:
break;
}
return PyState;
}
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);
}
void manual_mode(cwiid_wiimote_t *wiimote)
{
struct cwiid_state state; /* wiimote state */
int fd = init_maestro();
int target;
/*Set wiimote to report accelerometer readings*/
rpt_mode=toggle_bit(rpt_mode, CWIID_RPT_ACC);
if (cwiid_set_rpt_mode(wiimote, rpt_mode)) fprintf(stderr, "Error setting report mode\n");
int i = 0;
struct acc_cal wm_cal;
cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &wm_cal);
double a, a_x, a_y, a_z, pitch, roll;
double deltaT_x, deltaT_y;
double t_x_curr, t_x_past, t_y_curr, t_y_past;
struct timeval tim;
char man_mode_select = 0;
//Initialise PID parameters for the x-axis and the wait or DELTA_T/2
pid_params x = {KC, TAU_I, TAU_D, TAU_F, 0, 0, x_cord/1000, 0, 0, 0, 0, 0}; //initialise PID parameters for x axis
gettimeofday(&tim, NULL);
t_x_past=tim.tv_sec+(tim.tv_usec/1000000.0); //initialise t_x_past with current time (in seconds)
wait_for_deltat(&tim, &t_x_curr, &t_x_past, &deltaT_x, DELTA_T/2); //Wait until Delta_T/2
// Initialise PID parameters for the y-axis
pid_params y = {KC, TAU_I, TAU_D, TAU_F, 0, 0, y_cord/1000, 0, 0, 0, 0, 0}; //initialise PID paramaters for y axis
gettimeofday(&tim, NULL);
t_y_past=tim.tv_sec+(tim.tv_usec/1000000.0); //initialise t_y_past with current time (in seconds)
while(!next_mode)
{
if (cwiid_get_state(wiimote, &state)) {
fprintf(stderr, "Error getting state\n");
}
a_x = ((double)state.acc[CWIID_X] - wm_cal.zero[CWIID_X]) / (wm_cal.one[CWIID_X] - wm_cal.zero[CWIID_X]);
a_y = ((double)state.acc[CWIID_Y] - wm_cal.zero[CWIID_Y]) / (wm_cal.one[CWIID_Y] - wm_cal.zero[CWIID_Y]);
a_z = ((double)state.acc[CWIID_Z] - wm_cal.zero[CWIID_Z]) / (wm_cal.one[CWIID_Z] - wm_cal.zero[CWIID_Z]);
a = sqrt(pow(a_x,2)+pow(a_y,2)+pow(a_z,2));
roll = atan(a_x/a_z);
if (a_z <= 0.0) {
roll += PI * ((a_x > 0.0) ? 1 : -1);
}
pitch = atan(a_y/a_z*cos(roll));
roll *= (-180/PI);
pitch *= (180/PI);
roll *= 0.5;
pitch *= 0.5;
if (two_button_pressed)
{
if (man_mode_select ==0){
man_mode_select = 1; //mode 0 is accelerometer mode 1 is keypad
playsound("/usr/share/sounds/ball_plate/key_input.wav");}
else{
man_mode_select = 0;
playsound("/usr/share/sounds/ball_plate/acc_input.wav");}
two_button_pressed = 0;
}
if (left_button_pressed && man_mode_select == 1)
{
x.set_pt -= 0.005;
printf("x= %f\n", x.set_pt);
if (x.set_pt > 0.15) x.set_pt = 0.15;
if (x.set_pt < -0.15) x.set_pt = -0.15;
left_button_pressed = 0;
}
if (right_button_pressed && man_mode_select == 1)
{
x.set_pt += 0.005;
printf("x= %f\n", x.set_pt);
if (x.set_pt > 0.15) x.set_pt = 0.15;
if (x.set_pt < -0.15) x.set_pt = -0.15;
right_button_pressed = 0;
}
if (up_button_pressed && man_mode_select == 1)
{
y.set_pt += 0.005;
printf("y= %f\n", y.set_pt);
if (y.set_pt > 0.12) x.set_pt = 0.12;
if (y.set_pt < -0.12) x.set_pt = -0.12;
up_button_pressed = 0;
}
if (down_button_pressed && man_mode_select == 1)
{
y.set_pt -= 0.005;
printf("y= %f\n", y.set_pt);
if (y.set_pt > 0.12) x.set_pt = 0.12;
if (y.set_pt < -0.12) x.set_pt = -0.12;
down_button_pressed = 0;
}
if (man_mode_select == 0)
{
if ((int)pitch > 4 || (int)pitch < -4)
{
x.set_pt += -0.00005*pitch/10;
if (x.set_pt > 0.15) x.set_pt = 0.15;
if (x.set_pt < -0.15) x.set_pt = -0.15;
}
if ((int)roll > 4 || (int)roll < -4)
{
y.set_pt += -0.00005*roll/10;
if (y.set_pt > 0.12) y.set_pt = 0.12;
if (y.set_pt < -0.12) y.set_pt = -0.12;
}
}
wait_for_deltat(&tim, &t_x_curr, &t_x_past, &deltaT_x, DELTA_T); //Wait until DELTA_T for x-axis
x.pos_past = x.pos_curr; //store past ball position
x.pos_curr = x_cord/1000; //current ball position is equal to the coordinates read from the touchscreen
x.u_D_past = x.u_D; //store past derivative control signal
x.u_act_past = x.u_act; //store past control signal
x.error = (x.set_pt - x.pos_curr); //calculate error r(t) - y(t)
t_x_past = t_x_curr; //Save new time
//Calculate new derivative term
x.u_D = (x.tauF/(x.tauF+deltaT_x/1000))*x.u_D_past + ((x.kc*x.tauD)/(x.tauF+deltaT_x/1000))*(x.pos_curr - x.pos_past);
//Caluclate new control signal
x.u_act = x.u_act_past + x.kc*(-x.pos_curr + x.pos_past) + ((x.kc * deltaT_x/1000)/x.tauI)*(x.error) - x.u_D + x.u_D_past;
if (x.u_act > UMAX) x.u_act = UMAX;
if (x.u_act < UMIN) x.u_act = UMIN;
//Output Control Signal
target=(int)((x.u_act*(2.4*180/PI))*40+(4*X_SERVO_CENTRE));
maestroSetTarget(fd, 0, target);
//printf("Test Control Signal u_act_x = %f degrees\n", x.u_act*(180/PI));
wait_for_deltat(&tim, &t_y_curr, &t_y_past, &deltaT_y, DELTA_T); //Get Accurate timings
y.pos_past = y.pos_curr; //store past ball position
y.pos_curr = y_cord/1000; //current ball position is equal to the coordinates read from the touchscreen
y.u_D_past = y.u_D; //store past derivative control signal
y.u_act_past = y.u_act;
y.error = (y.set_pt - y.pos_curr);
t_y_past = t_y_curr; //Save new time
//Calculate new derivative term
y.u_D = (y.tauF/(y.tauF+deltaT_y/1000))*y.u_D_past + ((y.kc*y.tauD)/(y.tauF+deltaT_y/1000))*(y.pos_curr - y.pos_past);
//Caluclate new control signal
y.u_act = y.u_act_past + y.kc*(-y.pos_curr + y.pos_past) + ((y.kc * deltaT_y/1000)/y.tauI)*(y.error) - y.u_D + y.u_D_past;
if (x.u_act > UMAX) x.u_act = UMAX;
if (x.u_act < UMIN) x.u_act = UMIN;
//Output control signal
target=(int)(y.u_act*(2.4*180/PI)*40+(4*Y_SERVO_CENTRE));
maestroSetTarget(fd, 1, target);
//printf("Test Control Signal u_act_y = %f degrees\n", y.u_act*(180/PI));
}
printf("\n\n");
close_maestro(fd);
/*Cancel accelerometer readings*/
rpt_mode=toggle_bit(rpt_mode, CWIID_RPT_ACC);
if (cwiid_set_rpt_mode(wiimote, rpt_mode)) fprintf(stderr, "Error setting report mode\n");
}
/* 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;
}
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;
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;
}
C_RESULT update_wiimote(void)
{
C_RESULT res = C_OK;
static struct cwiid_state state,previous_state;
static int control_mode = 1;
static int valid_domain = 0;
int8_t x, y;
static int start=0;
static int select=0;
static vec3 a={0,0,0},s={0,0,0};
static vec3sph r={0,0,0},pr={0,0,0},rref={0,0,0};
static float pitch=0.0f,roll=0.0f,gaz=0.0f,yaw=0.0f;
float tmp;
static int fly=0;
if (cwiid_get_state(wiimote, &state))
{
fprintf(stderr, "Error getting state\n");
res = C_FAIL;
}
#define SWITCHING(X) ((state.buttons&X) && !(previous_state.buttons&X))
#define RELEASING(X) ((!state.buttons&X) && (previous_state.buttons&X))
#define PRESSED(X) ((state.buttons&X))
static int flag_rumble=0;
#define RUMBLE_ON { if (!flag_rumble) {flag_rumble=1; } }
/* Sets how to use the wiimote */
#define MAXMODE 4
if (SWITCHING(CWIID_BTN_MINUS)) {
control_mode--;
if (control_mode<1) control_mode=MAXMODE;
leds(1<<(control_mode-1));
}
if (SWITCHING(CWIID_BTN_PLUS)) {
control_mode++;
if (control_mode>MAXMODE) control_mode=1;
leds(1<<(control_mode-1));
}
/* Gets gravitation G projection on x,y,z axis */
a.x = - (float32_t) ((((double)state.acc[CWIID_X] - wm_cal.zero[CWIID_X]) /
(wm_cal.one[CWIID_X] - wm_cal.zero[CWIID_X])));
a.y = - (float32_t) ((((double)state.acc[CWIID_Y] - wm_cal.zero[CWIID_Y]) /
(wm_cal.one[CWIID_Y] - wm_cal.zero[CWIID_Y])));
a.z = + (float32_t) ((((double)state.acc[CWIID_Z] - wm_cal.zero[CWIID_Z]) /
(wm_cal.one[CWIID_Z] - wm_cal.zero[CWIID_Z])));
s.x = (a.x<0.0f)?(1.0f):(-1.0f);
s.y = (a.y<0.0f)?(1.0f):(-1.0f);
s.z = (a.z<0.0f)?(1.0f):(-1.0f);
float ax2 = a.x*a.x;
float ay2 = a.y*a.y;
float az2 = a.z*a.z;
r.r = sqrtf((ax2+ay2)+az2);
switch(control_mode)
{
case 1:
case 2:
if (r.r==0.0f) { r.p=r.t=0.0f; } else {
// Angle gauche/droite
r.p = asin(a.y); if (isnan(r.p)) r.p=0.0f;
// Sur plan vertical radial
r.t = acos(a.z/(sqrtf(az2+ax2))); if (isnan(r.t)) r.t=0.0f;
r.t*=s.x;
}
break;
case 3:
case 4:
if (r.r==0.0f) { r.p=r.t=0.0f; } else {
// Angle entre le projete de G sur le plan vertical longitudinal (yz) et l'axe z
r.p = acos(a.z/(sqrtf(az2+ay2))); if (isnan(r.p)) r.p=0.0f;
/* If wiimote faces the ground */
//if (a.z<0.0f) r.p= M_PI-r.p;
r.p*=s.y;
// Idem sur le plan vertical radial
r.t = acos(a.z/(sqrtf(az2+ax2))); if (isnan(r.t)) r.t=0.0f;
r.t*=s.x;
}
break;
}
r.r = (r.r+pr.r)/2.0f;
r.t = (r.t+pr.t)/2.0f;
r.p = (r.p+pr.p)/2.0f;
switch(control_mode)
{
case 1:
case 2:
/* Wiimote is handled horizontally.
* '2' button under left thumb
* directionnal cross under right thumb
*/
/* 0 -> buttons facing sky */
if ((SWITCHING(CWIID_BTN_1)||SWITCHING(CWIID_BTN_2)||SWITCHING(CWIID_BTN_B))){ rref=r; }
if (PRESSED(CWIID_BTN_1)||PRESSED(CWIID_BTN_2)||PRESSED(CWIID_BTN_B))
{
/* If wiimote facing ground */
if (a.z<0 && a.x>0)
{
rumble(1);
}
else
{
rumble(0);
leds(1<<(control_mode-1));
pitch = (r.t-rref.t)*1.0f; if (pitch<-1.0f) pitch=-1.0f; if (pitch>1.0f) pitch=1.0f;
roll = -(r.p-rref.p)*0.75f; if (roll<-1.0f) roll=-1.0f; if (roll>1.0f) roll=1.0f;
fly=1;
}
}
else
{
pitch=roll=0;
rumble(0);
leds(1<<(control_mode-1));
fly=0;
}
gaz = (PRESSED(CWIID_BTN_LEFT))? 1.0f: (PRESSED(CWIID_BTN_RIGHT)) ? -1.0f : 0.0f;
yaw = (PRESSED(CWIID_BTN_DOWN))? -1.0f: (PRESSED(CWIID_BTN_UP)) ? 1.0f : (control_mode==2) ? (-pitch*roll) : 0.0f;
break;
case 3:
case 4:
if (PRESSED(CWIID_BTN_B))
{
if (a.z<-0.5f)
{
rumble(1);
}
else
{
rumble(0);
leds(1<<(control_mode-1));
pitch = -(r.p-rref.p)*1.5f; if (pitch<-1.0f) pitch=-1.0f; if (pitch>1.0f) pitch=1.0f;
roll = (r.t-rref.t)*1.5f; if (roll<-1.0f) roll=-1.0f; if (roll>1.0f) roll=1.0f;
fly=1;
}
}
else
{
rumble(0);
leds(1<<(control_mode-1));
fly=0;
pitch=roll=0;
}
if (SWITCHING(CWIID_BTN_B)) { rref = r; }
gaz = (PRESSED(CWIID_BTN_DOWN))? -1.0f: (PRESSED(CWIID_BTN_UP)) ? +1.0f : 0.0f;
yaw = (PRESSED(CWIID_BTN_LEFT))? -1.0f: (PRESSED(CWIID_BTN_RIGHT)) ? 1.0f : (control_mode==4) ? (-pitch*roll) : 0.0f;
}
/* Buttons common to all modes */
if (SWITCHING(CWIID_BTN_A)) { start^=1; ardrone_tool_set_ui_pad_start(start); }
if (SWITCHING(CWIID_BTN_HOME)) {
select^=1;
ardrone_tool_set_ui_pad_select(select);
ardrone_tool_set_ui_pad_start(0);
}
//
//printf("Wiimote mode 2 [ax %f][ay %f][az %f]\n\033[1A", a.x,a.y,a.z);
printf("Wiimode %i [rr %3.2f][rp %3.2f][rt %3.2f]\n", control_mode,r.r,r.p,r.t);
printf("[Fly %i][Ptc %3.2f][Rl %3.2f][Yw %3.2f][Gz %3.2f][Strt. %i][Sel. %i]\n\033[2A", fly,pitch,roll,yaw,gaz,start,select);
ardrone_at_set_progress_cmd( fly,/*roll*/roll,/*pitch*/pitch,/*gaz*/gaz,/*yaw*/yaw);
/*api_set_iphone_acceleros(
(state.buttons&CWIID_BTN_2 ? 1 : 0)|(state.buttons&CWIID_BTN_A ? 2 : 0),
a_x, a_y, a_z);*/
previous_state = state;
pr=r;
return C_OK;
}
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(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;
}
cwiid_state Wiimote::getState() const {
cwiid_state state;
cwiid_get_state(instance,&state);
return state;
}
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;
}
