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);
}
}
virtual Ice::Int changeAccMode(const Ice::Current&) {
uint8_t rpt_mode;
rpt_mode |= CWIID_RPT_ACC | CWIID_RPT_NUNCHUK;
if(cwiid_set_rpt_mode(wiimote, rpt_mode)){
std::cout << "error setting report mode" << std::endl;
}
return 1;
}
virtual Ice::Int changeNunchukMode(const Ice::Current&) {
uint8_t rpt_mode;
rpt_mode = CWIID_RPT_STATUS | CWIID_RPT_BTN;
rpt_mode |= CWIID_RPT_EXT;
if(cwiid_set_rpt_mode(wiimote, rpt_mode)){
std::cout << "error setting report mode" << std::endl;
}
return 1;
}
/* Tell cwiid wich data will be reported */
void CWiiRemote::SetRptMode()
{ //Sets our wiiremote to report something, for example IR, Buttons
#ifdef CWIID_OLD
if (cwiid_command(m_wiiremoteHandle, CWIID_CMD_RPT_MODE, m_rptMode))
#else
if (cwiid_set_rpt_mode(m_wiiremoteHandle, m_rptMode))
#endif
{
CPacketLOG log(LOGERROR, "Error setting WiiRemote report mode");
log.Send(m_Socket, m_MyAddr);
}
}
int wii_open()
{
bdaddr = *BDADDR_ANY;
accmode = WII_ACCMODE_BTN;
as = 0;
speedacc = 0;
gettimeofday(&t1,NULL);
printf ( "N: wii: trying to find wiimote...\n" );
if (!(wiimote = cwiid_open(&bdaddr, 0))) {
fprintf(stderr, "E: wii: Unable to connect to wiimote\n");
return -1;
}
printf ("N: wii: open\n" );
if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) {
fprintf(stderr, "E: wii: Unable to set message callback\n");
cwiid_close(wiimote);
return -1;
}
printf ("N: wii: callback\n" );
if (cwiid_set_led(wiimote, 0x02)) {
fprintf(stderr, "Error setting LEDs \n");
cwiid_close(wiimote);
return -1;
}
printf ("N: wii: led\n" );
if (cwiid_set_rpt_mode(wiimote, CWIID_RPT_BTN | CWIID_RPT_ACC)) {
fprintf(stderr, "E: wii: could not set report mode\n");
cwiid_close(wiimote);
return -1;
}
printf ("N: wii: report mode set\n" );
if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC | CWIID_FLAG_CONTINUOUS)) {
fprintf(stderr, "E: wii: could not enable reporting\n");
cwiid_close(wiimote);
return -1;
}
printf ("N: wii: reporting enabled\n" );
wii_connected = 1;
return 0;
}
static int
Wiimote_set_rpt_mode(Wiimote *self, PyObject *PyRptMode, void *closure)
{
long rpt_mode;
if (((rpt_mode = PyInt_AsLong(PyRptMode)) == -1) && PyErr_Occurred()) {
return -1;
}
if (cwiid_set_rpt_mode(self->wiimote, (uint8_t)rpt_mode)) {
PyErr_SetString(PyExc_AttributeError,
"Error setting wiimote report mode");
return -1;
}
return 0;
}
int wminput_set_report_mode()
{
unsigned char rpt_mode_flags;
int i;
rpt_mode_flags = conf.rpt_mode_flags;
for (i=0; (i < CONF_MAX_PLUGINS) && conf.plugins[i].name; i++) {
rpt_mode_flags |= conf.plugins[i].rpt_mode_flags;
}
if (cwiid_set_rpt_mode(wiimote, rpt_mode_flags)) {
wminput_err("Error setting report mode");
return -1;
}
return 0;
}
/* 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;
}
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));
}
}
int cwiid_command(cwiid_wiimote_t *wiimote, enum cwiid_command command,
int flags) {
int ret;
switch (command) {
case CWIID_CMD_STATUS:
ret = cwiid_request_status(wiimote);
break;
case CWIID_CMD_LED:
ret = cwiid_set_led(wiimote, flags);
break;
case CWIID_CMD_RUMBLE:
ret = cwiid_set_rumble(wiimote, flags);
break;
case CWIID_CMD_RPT_MODE:
ret = cwiid_set_rpt_mode(wiimote, flags);
break;
default:
ret = -1;
break;
}
return ret;
}
ErrorID_t button_mode(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
int32_t speed = SPEED_NULL_VALUE;
int32_t direction = 0;
bool run = true;
cwiid_set_rpt_mode(wiimote, CWIID_RPT_BTN);
debug_print("Entering button mode:\n");
write_status_led(STATUS_LED_OFF, 0);
set_lcd(0, "Button Mode");
set_lcd(1, "2-fwd,1-back,dpad-steer");
do
{
wait_for_wiimotedata(wiimote_status, 25);
if ((wiimote_status->button_data & CWIID_BTN_2)
&& (wiimote_status->button_data & CWIID_BTN_1))
{
if (speed > 0)
{
speed -= MAX_FORWARD_SPEED / 10;
if (speed < 0)
speed = 0;
}
else if (speed < 0)
{
speed -= MAX_REVERSE_SPEED / 10;
if (speed > 0)
speed = 0;
}
}
else if (wiimote_status->button_data & CWIID_BTN_2)
{
speed += MAX_FORWARD_SPEED / 10;
if (speed > MAX_FORWARD_SPEED)
speed = MAX_FORWARD_SPEED;
}
else if (wiimote_status->button_data & CWIID_BTN_1)
{
speed += MAX_REVERSE_SPEED / 10;
if (speed < MAX_REVERSE_SPEED)
speed = MAX_REVERSE_SPEED;
}
else
{
speed = 0;
direction = 0;
run = false;
}
if (wiimote_status->button_data & CWIID_BTN_UP)
{
direction -= (MAX_LEFT_DIRECTION / 10);
if (direction < -MAX_LEFT_DIRECTION)
direction = -MAX_LEFT_DIRECTION;
}
else if (wiimote_status->button_data & CWIID_BTN_DOWN)
{
direction += (MAX_RIGHT_DIRECTION / 10);
if (direction > MAX_RIGHT_DIRECTION)
direction = MAX_RIGHT_DIRECTION;
}
else
direction = 0;
debug_print("@%u: Button (speed, heading) = %d %d\n", get_tick_count(),
speed, direction);
write_motor_levels(speed, ComputeDirectionMotor(direction));
} while (run);
return ERR_NONE;
}
ErrorID_t acceleration_mode(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
typedef enum WiimoteAccelStateType
{
WIIMOTE_ACCEL_WAIT_FOR_START,
WIIMOTE_ACCEL_READ_CAL_DATA,
WIIMOTE_ACCEL_READ_DATA,
WIIMOTE_ACCEL_WAIT_FOR_EXIT,
WIIMOTE_ACCEL_EXIT,
} WiimoteAccelStateType;
const uint8_t RETRY_VALUE = 2;
uint8_t retry_count = 0;
ErrorID_t error_flag = ERR_NONE;
WiimoteAccelStateType state = WIIMOTE_ACCEL_WAIT_FOR_START;
debug_print("Entering acceleration mode...\n");
write_status_led(STATUS_LED_OFF, 0);
for (;;)
{
switch (state)
{
case WIIMOTE_ACCEL_WAIT_FOR_START:
if (0 > wait_for_wiimotedata(wiimote_status, INFINITE_TIMEOUT))
{
debug_print("No start signal received\n");
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_ACCEL_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_A))
{
state = WIIMOTE_ACCEL_READ_CAL_DATA;
}
break;
case WIIMOTE_ACCEL_READ_CAL_DATA:
error_flag = WII_ERROR_ACCEL_CAL;
do
{
if (0
== cwiid_get_acc_cal(
wiimote,
CWIID_EXT_NONE,
(struct acc_cal *) &wiimote_status->accel_cal_data))
{
error_flag = ERR_NONE;
break;
}
} while (retry_count++ < RETRY_VALUE);
if (error_flag != ERR_NONE)
{
debug_print("Cannot read cal data\n");
return error_flag;
}
if (0 > cwiid_set_rpt_mode(wiimote, CWIID_RPT_ACC | CWIID_RPT_BTN))
{
debug_print("Cannot set report mode to ACCEL\n");
return false;
}
set_lcd(0, "Accel Mode...");
set_lcd(1, "P = %4d R = %4d", 0, 0);
state = WIIMOTE_ACCEL_READ_DATA;
break;
case WIIMOTE_ACCEL_READ_DATA:
if (0 > wait_for_wiimotedata(wiimote_status, INFINITE_TIMEOUT))
{
debug_print("@%u: WII_ERROR_DATA_TIMEOUT\n", get_tick_count());
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_ACCEL_EXIT;
}
else
{
if (wiimote_status->button_data & CWIID_BTN_A)
{
state = WIIMOTE_ACCEL_WAIT_FOR_EXIT;
}
else
{
error_flag = computer_motor_levels_accel(wiimote_status);
set_lcd(1, "P = %4d R = %4d",
wiimote_status->accel_computed_data.pitch / 100,
wiimote_status->accel_computed_data.roll / 100);
if (ERR_EXEC == error_flag)
cwiid_set_rumble(wiimote, true);
else
{
cwiid_set_rumble(wiimote, false);
if (error_flag < 0)
{
debug_print("@%u: CONTROL CAR COMM ERROR: %d\n",
get_tick_count(), error_flag);
state = WIIMOTE_ACCEL_EXIT;
}
}
}
}
break;
case WIIMOTE_ACCEL_WAIT_FOR_EXIT:
stop_motors();
if (0 < wait_for_wiimotedata(wiimote_status, INFINITE_TIMEOUT))
{
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_ACCEL_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_A))
{
state = WIIMOTE_ACCEL_EXIT;
}
break;
case WIIMOTE_ACCEL_EXIT:
debug_print("Exiting acceleration mode.\n\n");
return error_flag;
}
}
return ERR_NONE;
}
/*!
@brief Determines motor parameters from IR data.
*/
ErrorID_t infrared_mode(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
typedef enum WiimoteInfraredStateType
{
WIIMOTE_INFRARED_WAIT_FOR_START,
WIIMOTE_INFRARED_ENABLE,
WIIMOTE_INFRARED_READ_DATA,
WIIMOTE_INFRARED_WAIT_FOR_EXIT,
WIIMOTE_INFRARED_EXIT,
} WiimoteInfraredStateType;
ErrorID_t error_flag = ERR_NONE;
bool last_valid = false;
bool valid_points = false;
WiimoteInfraredStateType state = WIIMOTE_INFRARED_WAIT_FOR_START;
write_status_led(STATUS_LED_OFF, 0);
for (;;)
{
switch (state)
{
case WIIMOTE_INFRARED_WAIT_FOR_START:
if (0 < wait_for_wiimotedata(wiimote_status, 500))
{
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_INFRARED_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_B))
{
state = WIIMOTE_INFRARED_ENABLE;
}
break;
case WIIMOTE_INFRARED_ENABLE:
set_ir_led(STATUS_LED_ON);
debug_print("Setting IR Report\n");
if (0 < cwiid_set_rpt_mode(wiimote, CWIID_RPT_IR | CWIID_RPT_BTN))
{
debug_print("Cannot set report mode to IR\n");
return false;
}
set_lcd(0, "Infrared Mode");
set_lcd(1, "Point wiimote at car");
write_status_led(STATUS_LED_ON, 0);
state = WIIMOTE_INFRARED_READ_DATA;
break;
case WIIMOTE_INFRARED_READ_DATA:
if (0 < wait_for_wiimotedata(wiimote_status, 500))
{
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_INFRARED_EXIT;
}
else if (wiimote_status->button_data & CWIID_BTN_B)
{
state = WIIMOTE_INFRARED_WAIT_FOR_EXIT;
}
else
{
error_flag = WiiComputeMotorLevelsInfrared(wiimote_status,
&valid_points);
if (valid_points != last_valid)
{
write_status_led(
valid_points ? STATUS_LED_OFF : STATUS_LED_ON, 0);
set_lcd(
1,
valid_points ?
"LED's seen" : "Point wiimote at car");
}
last_valid = valid_points;
if (ERR_EXEC == error_flag)
cwiid_set_rumble(wiimote, true);
else
{
cwiid_set_rumble(wiimote, false);
if (error_flag < 0)
state = WIIMOTE_INFRARED_EXIT;
}
}
break;
case WIIMOTE_INFRARED_WAIT_FOR_EXIT:
if (0 < wait_for_wiimotedata(wiimote_status, 500))
{
state = WIIMOTE_INFRARED_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_B))
{
state = WIIMOTE_INFRARED_EXIT;
}
break;
case WIIMOTE_INFRARED_EXIT:
stop_motors();
write_status_led(STATUS_LED_OFF, 0);
set_ir_led(false);
return error_flag;
break;
break;
}
}
return true;
}
/*!
@brief Driver for Wiimote operation menu
Handles parsing wiimote input to determine correct operating mode.
This function will never exit once entered.
*/
ErrorID_t main_menu(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
ErrorID_t error;
uint8_t menu_count;
uint16_t last_button_state = 0;
WII_OPERATE_STATE wii_operate_state = WII_OPERATE_INIT_MENU;
write_status_led(STATUS_LED_OFF, 0);
for (;;)
{
switch (wii_operate_state)
{
case WII_OPERATE_INIT_MENU:
menu_count = 0;
last_button_state = 0;
wii_operate_state = WII_OPERATE_WAIT_FOR_BUTTON_PRESS;
write_status_led(STATUS_LED_FLASH, 500);
if (0 > cwiid_set_led(wiimote, CWIID_LED1_ON))
return WII_ERROR_TRANSMIT;
if (0 > cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC))
return WII_ERROR_TRANSMIT;
if (0 > cwiid_set_rpt_mode(wiimote, CWIID_RPT_BTN))
return WII_ERROR_TRANSMIT;
/* Menu */
debug_print("%s", MENU);
set_lcd(0, "Connected.");
set_lcd(1, "Select Mode.");
break;
case WII_OPERATE_WAIT_FOR_BUTTON_PRESS:
error = wait_for_wiimotedata(wiimote_status, 2000);
if (error == ERR_WII_DATA_TIMEOUT)
{
set_lcd(0, "Select Mode");
set_lcd(1, (char *) lcd_menu_strings[menu_count]);
if (++menu_count >= MENU_ENTRIES)
menu_count = 0;
}
else if (0 < error)
{
shutdown_application(error);
}
else if (wiimote_status->button_data != last_button_state)
{
last_button_state = wiimote_status->button_data;
if (last_button_state & CWIID_BTN_A) // collect acceleration data
{
wii_operate_state = WII_OPERATE_DISPLAY_ACCELEROMETER_INFO;
}
else if (last_button_state & CWIID_BTN_B) // collect IR data
{
wii_operate_state = WII_OPERATE_DISPLAY_IR_STATUS;
}
else if (last_button_state & CWIID_BTN_HOME) // reset
{
shutdown_application(0);
}
else if (last_button_state & CWIID_BTN_PLUS)
{
sensor_cutoff_fwd += 5;
if (sensor_cutoff_fwd > 100)
sensor_cutoff_fwd = 100;
#if REVERSE_SENSOR_PRESENT
sensor_cutoff_rev += 5;
if (sensor_cutoff_rev > 100)
sensor_cutoff_rev = 100;
#endif
}
else if (last_button_state & CWIID_BTN_MINUS)
{
sensor_cutoff_fwd -= 5;
if (sensor_cutoff_fwd < 0)
sensor_cutoff_fwd = 0;
#if REVERSE_SENSOR_PRESENT
sensor_cutoff_rev -= 5;
if (sensor_cutoff_rev < 0)
sensor_cutoff_rev = 0;
#endif
}
else if (last_button_state & (CWIID_BTN_2 | CWIID_BTN_1))
{
wii_operate_state = WII_OPERATE_DISPLAY_BUTTON_STATUS;
}
}
break;
case WII_OPERATE_DISPLAY_ACCELEROMETER_INFO:
if (ERR_NONE != acceleration_mode(wiimote, wiimote_status))
wii_operate_state = WII_OPERATE_ERROR_STATE;
else
wii_operate_state = WII_OPERATE_INIT_MENU;
break;
case WII_OPERATE_DISPLAY_IR_STATUS:
if (ERR_NONE != infrared_mode(wiimote, wiimote_status))
wii_operate_state = WII_OPERATE_ERROR_STATE;
else
wii_operate_state = WII_OPERATE_INIT_MENU;
break;
case WII_OPERATE_DISPLAY_BUTTON_STATUS:
if (ERR_NONE != button_mode(wiimote, wiimote_status))
wii_operate_state = WII_OPERATE_ERROR_STATE;
else
wii_operate_state = WII_OPERATE_INIT_MENU;
break;
case WII_OPERATE_ERROR_STATE:
if (ERR_NONE == error_mode(wiimote, wiimote_status))
wii_operate_state = WII_OPERATE_INIT_MENU;
break;
}
}
return ERR_NONE;
}
int main(int argc, char** argv) {
cwiid_wiimote_t *wiimote;
struct cwiid_state state;
bdaddr_t bdaddr;
int exit = 0;
point3Df dimensions3PtsCap[3];
bdaddr = *BDADDR_ANY;
printf("connecting!\n");
if (!(wiimote = cwiid_open(&bdaddr, 0))) {
fprintf(stderr, "Unable to connect to wiimote\n");
return -1;
}
printf("connected\n");
cwiid_set_led(wiimote, CWIID_LED1_ON | CWIID_LED4_ON);
cwiid_set_rpt_mode(wiimote, CWIID_RPT_STATUS | CWIID_RPT_IR);
//*
if (cwiid_set_mesg_callback(wiimote, cwiid_callback)) {
fprintf(stderr, "Unable to set message callback\n");
}
if (cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC)) {
fprintf(stderr, "Error enabling messages\n");
}
// */
dimensions3PtsCap[0].x = 70;
dimensions3PtsCap[0].y = 80;
dimensions3PtsCap[0].z = 100;
Initialize3PCapModel(dimensions3PtsCap);
while (!exit) {
switch(getchar()) {
case 'q':
exit = 1;
break;
default:
if (cwiid_get_state(wiimote, &state)) {
printf("Error getting state\n");
} else {
print_state(&state);
}
}
}
/*
if (getchar()) {
printf("Exiting\n");
}
**/
if (cwiid_close(wiimote)) {
printf("Error on wiimote disconnect\n");
return -1;
}
return (EXIT_SUCCESS);
}
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");
}
/* unsuspend the currently suspended (and inited)
* camera device.
* IR leds will reactivate, but that is all
* a return value < 0 indicates error */
int ltr_int_tracker_resume() {
set_leds_running();
cwiid_set_rpt_mode(gWiimote, CWIID_RPT_STATUS | CWIID_RPT_IR);
return 0;
}
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;
}
void set_rpt_mode(cwiid_wiimote_t *wiimote, unsigned char rpt_mode)
{
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;
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;
}
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;
}
/* turn off all the leds, and flush the queue
* a return value < 0 indicates error */
int ltr_int_tracker_pause() {
set_leds_paused();
cwiid_set_rpt_mode(gWiimote, CWIID_RPT_STATUS);
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;
}
