static PyObject *Wiimote_get_acc_cal(Wiimote *self, PyObject *args,
PyObject *kwds)
{
static char *kwlist[] = { "ext_type", NULL };
int ext_type;
struct acc_cal acc_cal;
PyObject *PyAccCal;
if (!self->wiimote) {
SET_CLOSED_ERROR;
return NULL;
}
if (!PyArg_ParseTupleAndKeywords(args, kwds,
"i:cwiid.Wiimote.get_acc_cal", kwlist,
&ext_type)) {
return NULL;
}
if (cwiid_get_acc_cal(self->wiimote, ext_type, &acc_cal)) {
PyErr_SetString(PyExc_RuntimeError,
"Error getting wiimote acc calibration");
return NULL;
}
if (!(PyAccCal = Py_BuildValue("([i,i,i],[i,i,i])", acc_cal.zero[0],
acc_cal.zero[1], acc_cal.zero[2],
acc_cal.one[0], acc_cal.one[1],
acc_cal.one[2]))) {
return NULL;
}
return PyAccCal;
}
struct wmplugin_data *wmplugin_exec(int mesg_count, union cwiid_mesg mesg[], struct timespec *timestamp)
{
int i;
enum cwiid_ext_type ext_type = CWIID_EXT_NONE;
struct wmplugin_data *ret = NULL;
for (i=0; i < mesg_count; i++) {
switch (mesg[i].type) {
case CWIID_MESG_STATUS:
if ((mesg[i].status_mesg.ext_type == CWIID_EXT_NUNCHUK) &&
(ext_type != CWIID_EXT_NUNCHUK)) {
if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NUNCHUK, &acc_cal)) {
wmplugin_err(plugin_id, "calibration error");
}
}
ext_type = mesg[i].status_mesg.ext_type;
break;
case CWIID_MESG_NUNCHUK:
process_nunchuk(&mesg[i].nunchuk_mesg);
ret = &data;
break;
default:
break;
}
}
return ret;
}
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;
}
void Wiimote::StartReporting(uint8_t aReportMode)
{
ULOG_DEBUG_F();
if (cwiid_set_mesg_callback(mWiimote, &Wiimote::CWiidCallback)) {
ULOG_DEBUG_F("Error setting callback");
if (cwiid_close(mWiimote)) {
ULOG_DEBUG_F("Error on disconnect");
}
mWiimote = NULL;
NotifyConnectionStatus(IWiimoteObserver::EConnectionError);
NotifyConnectionStatus(IWiimoteObserver::ENotConnected);
} else {
ULOG_DEBUG_F("Connected");
if (cwiid_get_acc_cal(mWiimote, CWIID_EXT_NONE, &mWmCal)) {
ULOG_DEBUG_F("Unable to retrieve accelerometer calibration");
}
NotifyConnectionStatus(IWiimoteObserver::EConnected);
/*
set_gui_state();
set_report_mode();
cwiid_enable(wiimote, CWIID_FLAG_MOTIONPLUS);
*/
//uint8_t rpt_mode;
//rpt_mode = CWIID_RPT_STATUS | CWIID_RPT_BTN | CWIID_RPT_ACC;
if (cwiid_set_rpt_mode(mWiimote, aReportMode)) {
ULOG_DEBUG_F("Error setting report mode");
}
cwiid_request_status(mWiimote);
}
}
int wmplugin_init(int id, cwiid_wiimote_t *wiimote)
{
plugin_id = id;
data.buttons = 0;
data.axes[0].valid = 1;
data.axes[1].valid = 1;
if (wmplugin_set_rpt_mode(id, CWIID_RPT_ACC)) {
return -1;
}
if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &acc_cal)) {
wmplugin_err(id, "calibration error");
return -1;
}
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;
}
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 wmplugin_init(int id, cwiid_wiimote_t *wiimote)
{
plugin_id = id;
ccd_center.valid=1;
ccd_center.size=1;
ccd_center.pos[0]=512;
ccd_center.pos[1]=384;
motionplus_cal.zero[0] = 8050;
motionplus_cal.zero[1] = 8000;
motionplus_cal.zero[2] = 8580;
data.buttons = 0;
data.axes[0].valid = 1;
data.axes[1].valid = 1;
data.axes[2].valid = 1;
data.axes[3].valid = 1;
data.axes[4].valid = 1;
data.axes[4].value = 0;
data.axes[5].valid = 1;
if (wmplugin_set_rpt_mode(id, CWIID_RPT_ACC | CWIID_RPT_MOTIONPLUS | CWIID_RPT_IR | CWIID_RPT_BTN)) {
return -1;
}
if(cwiid_enable(wiimote, CWIID_FLAG_MOTIONPLUS)) {
wmplugin_err(id, "You need wiimotion plus to run this plugin");
return -1;
}
if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NONE, &acc_cal)) {
wmplugin_err(id, "accelerometers calibration error");
return -1;
}
return 0;
}
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;
}
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");
}
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;
}
// 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);
}
static void wiimote_cwiid_message(t_wiimote *x, union cwiid_mesg*mesg) {
if(NULL==x){
return;
}
if(NULL==mesg) {
return;
}
switch (mesg->type) {
case CWIID_MESG_STATUS:
wiimote_cwiid_battery(x, mesg->status_mesg.battery);
switch (mesg->status_mesg.ext_type) {
case CWIID_EXT_NONE:
verbose(1, "No extension attached");
break;
#ifdef CWIID_RPT_NUNCHUK
case CWIID_EXT_NUNCHUK:
post("Nunchuk extension attached");
if(cwiid_get_acc_cal(x->wiimote, CWIID_EXT_NUNCHUK, &x->nc_acc_cal)) {
post("Unable to retrieve nunchuk calibration");
} else {
post("Retrieved nunchuk calibration: zero=(%02d,%02d,%02d) one=(%02d,%02d,%02d)",
x->nc_acc_cal.zero[CWIID_X],
x->nc_acc_cal.zero[CWIID_Y],
x->nc_acc_cal.zero[CWIID_Z],
x->nc_acc_cal.one [CWIID_X],
x->nc_acc_cal.one [CWIID_Y],
x->nc_acc_cal.one [CWIID_Z]);
}
break;
#endif
#ifdef CWIID_RPT_CLASSIC
case CWIID_EXT_CLASSIC:
post("Classic controller attached. There is no real support for this yet.");
break;
#endif
#ifdef CWIID_RPT_BALANCE
case CWIID_EXT_BALANCE:
if(cwiid_get_balance_cal(x->wiimote, &x->balance_cal)) {
post("Unable to retrieve balance calibration");
}
break;
#endif
#ifdef CWIID_RPT_MOTIONPLUS
case CWIID_EXT_MOTIONPLUS:
post("MotionPlus controller attached.");
/* no calibration needed for MotionPlus */
break;
#endif
case CWIID_EXT_UNKNOWN:
post("Unknown extension attached");
break;
default:
post("ext mesg %d/%d unknown", mesg->type, mesg->status_mesg.ext_type);
break;
}
break;
case CWIID_MESG_BTN:
wiimote_cwiid_btn(x, &mesg->btn_mesg);
break;
case CWIID_MESG_ACC:
wiimote_cwiid_acc(x, &mesg->acc_mesg);
break;
case CWIID_MESG_IR:
wiimote_cwiid_ir(x, &mesg->ir_mesg);
break;
#ifdef CWIID_RPT_NUNCHUK
case CWIID_MESG_NUNCHUK:
wiimote_cwiid_nunchuk(x, &mesg->nunchuk_mesg);
break;
#endif
#ifdef CWIID_RPT_CLASSIC
case CWIID_MESG_CLASSIC:
wiimote_cwiid_classic(x, &mesg->classic_mesg);
// todo
break;
#endif
#ifdef CWIID_RPT_MOTIONPLUS
case CWIID_MESG_MOTIONPLUS:
wiimote_cwiid_motionplus(x, &mesg->motionplus_mesg);
break;
#endif
#ifdef CWIID_RPT_BALANCE
case CWIID_MESG_BALANCE:
wiimote_cwiid_balance(x, &mesg->balance_mesg);
break;
#endif
case CWIID_MESG_ERROR:
wiimote_cwiid_error(x, &mesg->error_mesg);
default:
post("mesg %d unknown", (mesg->type));
break;
}
}
/*if (cwiid_beep(wiimote)) {
message(GTK_MESSAGE_ERROR, "Wiimote sound error", GTK_WINDOW(winMain));
}*/
}
void set_report_mode(void)
{
uint8_t rpt_mode;
rpt_mode = CWIID_RPT_STATUS | CWIID_RPT_BTN;
if (gtk_check_menu_item_get_active(GTK_CHECK_MENU_ITEM(chkIR))) {
rpt_mode |= CWIID_RPT_IR;
}
if (gtk_check_menu_item_get_active(GTK_CHECK_MENU_ITEM(chkAcc))) {
rpt_mode |= CWIID_RPT_ACC;
}
if (gtk_check_menu_item_get_active(GTK_CHECK_MENU_ITEM(chkExt))) {
rpt_mode |= CWIID_RPT_EXT;
}
if (cwiid_set_rpt_mode(wiimote, rpt_mode)) {
message(GTK_MESSAGE_ERROR, "error setting report mode",
GTK_WINDOW(winMain));
}
}
#define BATTERY_STR_LEN 14 /* "Battery: 100%" + '\0' */
void cwiid_callback(cwiid_wiimote_t *wiimote, int mesg_count,
union cwiid_mesg mesg_array[], struct timespec *timestamp)
{
int i;
char battery[BATTERY_STR_LEN];
char *ext_str;
static enum cwiid_ext_type ext_type = CWIID_EXT_NONE;
(void)timestamp;
gdk_threads_enter();
for (i=0; i < mesg_count; i++) {
switch (mesg_array[i].type) {
case CWIID_MESG_STATUS:
snprintf(battery, BATTERY_STR_LEN,"Battery:%d%%",
(int) (100.0 * mesg_array[i].status_mesg.battery /
CWIID_BATTERY_MAX));
gtk_statusbar_push(GTK_STATUSBAR(statBattery), 0, battery);
switch (mesg_array[i].status_mesg.ext_type) {
case CWIID_EXT_NONE:
ext_str = "No extension";
break;
case CWIID_EXT_NUNCHUK:
ext_str = "Nunchuk";
if (ext_type != CWIID_EXT_NUNCHUK) {
if (cwiid_get_acc_cal(wiimote, CWIID_EXT_NUNCHUK,
&nc_cal)) {
message(GTK_MESSAGE_ERROR,
"Unable to retrieve accelerometer calibration",
GTK_WINDOW(winMain));
}
}
break;
case CWIID_EXT_CLASSIC:
ext_str = "Classic controller";
break;
case CWIID_EXT_BALANCE:
ext_str = "Balance Board";
case CWIID_EXT_MOTIONPLUS:
ext_str = "MotionPlus";
break;
case CWIID_EXT_UNKNOWN:
ext_str = "Unknown extension";
break;
}
gtk_statusbar_push(GTK_STATUSBAR(statExtension), 0, ext_str);
clear_nunchuk_widgets();
clear_classic_widgets();
clear_motionplus_widgets();
ext_type = mesg_array[i].status_mesg.ext_type;
break;
case CWIID_MESG_BTN:
cwiid_btn(&mesg_array[i].btn_mesg);
break;
case CWIID_MESG_ACC:
cwiid_acc(&mesg_array[i].acc_mesg);
break;
case CWIID_MESG_IR:
cwiid_ir(&mesg_array[i].ir_mesg);
break;
case CWIID_MESG_NUNCHUK:
cwiid_nunchuk(&mesg_array[i].nunchuk_mesg);
break;
case CWIID_MESG_CLASSIC:
cwiid_classic(&mesg_array[i].classic_mesg);
break;
case CWIID_MESG_MOTIONPLUS:
cwiid_motionplus(&mesg_array[i].motionplus_mesg);
break;
case CWIID_MESG_ERROR:
menuDisconnect_activate();
break;
default:
break;
}
}
gdk_flush();
gdk_threads_leave();
}
