/* This function is called in the update function, at a refresh rate of 20 ms */
C_RESULT ardrone_tool_update_custom() {
// read from memdb and send to ardrone
char buffer[1024];
int hover = 0; // boolean indicating whether it's hovering or not
float phi; // left/right angle
float theta; // front/back angle
float gaz; // verticle speed
float yaw; // angular speed
while (db_tryget(db, "drone_command", buffer, sizeof(buffer)) != -1)
{
sscanf(buffer, "%d,%f,%f,%f,%f", &hover, &phi, &theta, &gaz, &yaw);
if (hover == 1) // hover
{
ardrone_at_set_progress_cmd(0, 0, 0, 0, 0);
}
else if (hover == -1) // land
{
ardrone_tool_set_ui_pad_start(0);
}
else // do stuff...
{
ardrone_at_set_progress_cmd(1, phi, theta, gaz, yaw);
}
}
return C_OK;
}
C_RESULT update_teleop(void)
{
// This function *toggles* the emergency state, so we only want to toggle the emergency
// state when we are in the emergency state (because we want to get out of it).
ardrone_tool_set_ui_pad_select(needs_reset);
needs_reset = false;
// This function sets whether or not the robot should be flying. If it is flying and you
// send 0, the robot will slow down the motors and slowly descend to the floor.
ardrone_tool_set_ui_pad_start(is_flying);
float left_right = cmd_vel.linear.y;
float front_back = cmd_vel.linear.x;
float up_down = cmd_vel.linear.z;
float turn = cmd_vel.angular.z;
///printf("LR:%f FB:%f UD:%f TURN:%f\n", left_right, front_back, up_down, turn);
if(left_right == 0 && front_back == 0 && up_down == 0 && turn == 0)
{
printf("Drone im Hover Mode\n");
ardrone_at_set_progress_cmd(0, left_right, front_back, up_down, turn);
}else{
ardrone_at_set_progress_cmd(1, left_right, front_back, up_down, turn);
}
return C_OK;
}
DEFINE_THREAD_ROUTINE( comm_control, data )
{
int seq_no = 0,prev_start = 0;
control_data_t l_control;
PRINT( "Initilizing Thread 1\n" );
while( end_all_threads == 0 )
{
vp_os_delay(10);
//PRINT("E:%d,S:%d\n\n",end_all_threads,control_data.start );
vp_os_mutex_lock( &control_data_lock ); //Taking Control Mutex
l_control = control_data;
vp_os_mutex_unlock( &control_data_lock );
if( prev_start == 0 && l_control.start == 1 ) //To Start Drone
{
ardrone_tool_set_ui_pad_start( 1 );
prev_start = 1;
}
else if( prev_start == 1 && l_control.start == 0 ) //To Stop the Drone
{
ardrone_tool_set_ui_pad_start( 0 );
prev_start = 0;
}
//PRINT("YAW : %f\n\n",l_control.yaw);
ardrone_at_set_progress_cmd( ++seq_no,l_control.roll,l_control.pitch,l_control.gaz,l_control.yaw); //command to make drone move.
//ardrone_at_set_progress_cmd( ++seq_no,0,0,0,-1);
}
PRINT( "Communication Thread Ending\n" );
return C_OK;
}
void drone_fly(float pitch, float strafe, float upwards, float yaw) {
pthread_mutex_lock(&drone_sharedDataMutex);
if (!drone_targetEmergency && !drone_emergency) {
if (drone_atCommands++ < MAX_AT_COMMANDS_PER_NAVDATA) {
ardrone_at_set_progress_cmd(1, strafe, pitch, upwards, yaw);
}
}
pthread_mutex_unlock(&drone_sharedDataMutex);
}
void drone_hover() {
pthread_mutex_lock(&drone_sharedDataMutex);
if (!drone_targetEmergency && !drone_emergency) {
if (drone_atCommands++ < MAX_AT_COMMANDS_PER_NAVDATA) {
ardrone_at_set_progress_cmd(0, 0, 0, 0, 0);
}
}
pthread_mutex_unlock(&drone_sharedDataMutex);
}
C_RESULT ardrone_control() {
const char * linefiller=" ";
static int nCountFrequency=0;
static bool function_first_call = true;
nCountFrequency++;
if(nCountFrequency%50==0)
{
printf("过去了 %d 秒！！！！\n",nCountFrequency/50);
if(nCountFrequency%5000==0)
nCountFrequency=0;
}
if (function_first_call) {
printf("Sending flat trim - make sure the drone is horizontal at client startup.\n");
ardrone_at_set_flat_trim(); //向无人机发送确定无人机是水平躺着，每次无人机启动都要发送，不可在无人机飞行时调用此函数
function_first_call=false;
// vp_os_memcpy(previous_keyboardState,keyboardState,sizeof(keyboardState));
return C_OK;
}
ARWin32Demo_AcquireConsole();
ARWin32Demo_SetConsoleCursor(0,12);
if(emergency!=emergency_p) { //以下代码 待服务器端完成后，需再次修改
if(emergency=0)
{
ardrone_tool_set_ui_pad_start(0);
ardrone_tool_set_ui_pad_select(1);
printf("Sending emergency.%s\n",linefiller);
}
else//若之前按下，现在没按，说明是emergency状态转常规飞行
{
ardrone_tool_set_ui_pad_select(0);
}
}
if((takeoff!=takeoff_p)&&takeoff==1)
{
start^=1;
ardrone_tool_set_ui_pad_start(start);
printf("飞机起飞 %i.%s\n",start,linefiller);
}
if((trim!=trim_p)&&trim==1)
{
ardrone_at_set_flat_trim();
printf("水平矫正.%s\n",linefiller);
}
ardrone_at_set_progress_cmd(hover,roll, pitch, gaz, yaw);
printf("[Pitch %f] [Roll %f] [Yaw %f] [Gaz %f]%\n",pitch,roll,yaw,gaz);
ARWin32Demo_ReleaseConsole();
return C_OK;
}
int ardrone_at_set_radiogp_inputC(int32_t y, int32_t x, int32_t z, int32_t yaw){
if(y == lasty && x == lastx && z == lastz && yaw == lastyaw && repeatCount < 10)
{
repeatCount++;
return 0; //only continue if does not mach last command sent
}
repeatCount = 0;
/*if(commandLog == NULL)
{
char filename[256];
sprintf(filename, "ControlLog-%d.txt", (int)time(NULL));
commandLog = fopen(filename, "w");
fprintf(commandLog, "Time Pitch Roll Gaz Yaw\n");
}
else
{
timeval t;
gettimeofday(&t, NULL);
fprintf(commandLog, "%d.%06d %d %d %d %d \n", (int)t.tv_sec, (int)t.tv_usec, y, x, z, yaw);
}*/
/************* at_set_radiogp_input ****************
* Description : Fill struct radiogp_cmd,
* used with at_cmds_loop function.
* pitch : y-axis (rad) (-25000, +25000)
* roll : x-axis (rad) (-25000, +25000)
* gaz : altitude (mm/s) (-25000, +25000)
* yaw : z-axis (rad/s) (-25000, +25000)
*/
//Make sure commands are in range
if(x < -25000) x = -25000;
if(x > 25000) x = 25000;
if(y < -25000) y = -25000;
if(y > 25000) y = 25000;
if(z < -25000) z = -25000;
if(z > 25000) z = 25000;
if(yaw < -25000) yaw = -25000;
if(yaw > 25000) yaw = 25000;
ardrone_at_set_progress_cmd( 1,
x/25000.0,
y/25000.0,
-z/25000.0,
yaw/25000.0);
//ardrone_at_set_radiogp_input(y, x, z, yaw); old command
// PRINT("Pitch:%3d Roll:%3d Gaz:%3d Yaw:%3d\n", y, x, z, yaw);
lastx = x;
lasty = y;
lastz = z;
lastyaw = yaw;
return 1;
}
C_RESULT update_fpad(void){
/*FuzzyControl( &X , 0.0 , h1 );
FuzzyControl( &Y , 0.0 , h2 );
FuzzyControl( &Yaw , 0.0 , h3 );
FuzzyControl( &Z , 0.0 , h4 );//*/
//printf("%d\n",fcx);
//dispControl(&Z);
/*ardrone_at_set_progress_cmd( 1,
//roll/(float)(stick1LR-center_x1)/32767.0f,
//pitch/(float)(stick1UD-center_y1)/32767.0f,
//gaz/-(float)(stick2UD-center_x2)/32767.0f,
//yaw/(float)(stick2LR-center_y2)/32767.0f );*/
//printf("++%0.2f++\n",Z.vout);
if(heightTestMode==0){
if(theControl.phi!=0||theControl.theta!=0||theControl.gaz!=0||theControl.yaw!=0){
ardrone_at_set_progress_cmd( 1,theControl.phi,theControl.theta,theControl.gaz,theControl.yaw);
ardrone_at_set_led_animation(FIRE,10,1);
}
else{
bringItOn();
}
}
else{
static int timer=0;
static int theFlag=0;
if(timer<heightTestModeTop){
if(theFlag%2==0){
Z.ref=heightTestModeH1;
zg.ref=Z.ref;
}
else{
Z.ref=heightTestModeH2;
zg.ref=Z.ref;
}
timer++;
//printf("%d!\n",timer);
}
else {
printf("CHANGE to %0.2f !!!\n\n",Z.ref);
theFlag++;
timer=0;
}
bringItOn();
}
return C_OK;
}
void bringItOn(){
//static GdkPixbuf *pixbuf = NULL;
//gui_t *gui = get_gui();
zg.z=Z.vin;
zg.x=X.vin;
zg.y=Y.vin;
zg.w=Yaw.vin;
fuzzyGraph( &zg, zvg );
fuzzyControl(&Z,&X,&Y,&Yaw);
//printf(">>>%5.2f",fabs(Z.vin-Z.ref));
//if(fabs(Z.vin-Z.ref)<200||Z.vin>Z.ref){
ardrone_at_set_progress_cmd( 0,-X.vout,Y.vout,Z.vout,Yaw.vout);
/*}
else
ardrone_at_set_progress_cmd( 0,0,0,Z.vout,0);//*/
//printf("-");
}
C_RESULT update_teleop(void)
{
// This function *toggles* the emergency state, so we only want to toggle the emergency
// state when we are in the emergency state (because we want to get out of it).
//fprintf(stderr, "%d\n", int(needs_reset));
if (needs_reset)
{
// fprintf(stderr, "needs_reset true\n");
ardrone_tool_set_ui_pad_select(needs_reset);
needs_reset = false;
// fprintf(stderr, "needs_reset false\n");
}
// ******dep****** This function sets whether or not the robot should be flying. If it is flying and you
// ******dep****** send 0, the robot will slow down the motors and slowly descend to the floor.
//rujian July 30
if (needs_takeoff)
{
ardrone_tool_set_ui_pad_start(true);
needs_takeoff = false;
}
if (needs_land)
{
ardrone_tool_set_ui_pad_start(false);
needs_land = false;
}
float left_right = cmd_vel.linear.y;
float front_back = cmd_vel.linear.x;
float up_down = cmd_vel.linear.z;
float turn = cmd_vel.angular.z;
ardrone_at_set_progress_cmd(do_not_hover, left_right, front_back, up_down, turn); //rujian July27
return C_OK;
}
DEFINE_THREAD_ROUTINE( thread1, data )
{
int seq_no = 0,prev_start = 0;
PRINT( "Initilizing Thread 1\n" );
while(1)
{
vp_os_delay(100);
vp_os_mutex_lock( &control_data_lock ); //Taking Control Mutex
if( prev_start == 0 && control_data.start == 1 )
{
ardrone_tool_set_ui_pad_start( 1 );
prev_start = 1;
}
else if( prev_start == 1 && control_data.start == 0 )
{
ardrone_tool_set_ui_pad_start( 0 );
prev_start = 0;
}
ardrone_at_set_progress_cmd( ++seq_no,control_data.roll, control_data.pitch, control_data.gaz, control_data.yaw); //command to make drone move.
vp_os_mutex_unlock( &control_data_lock );
}
}
int _stdcall SetProgressCmd(BOOL bhovering, float roll, float pitch, float gaz, float yaw)
{
ardrone_at_set_progress_cmd((hovering)? 0:1, roll, pitch, gaz, yaw);
printf("Sent Progress Cmd\n");
return 0;
}
void sendControls(bool_t accelero, bool_t left, bool_t right, bool_t up, bool_t down)
{
ardrone_at_set_progress_cmd(accelero, ctrldata.accelero_phi, ctrldata.accelero_theta, ctrldata.gaz, ctrldata.yaw);
}
C_RESULT update_gamepad(void) {
static int32_t start;
static float phi = 0, theta = 0, gaz = 0, yaw = 0;
phi = theta = gaz = yaw = 0.0;
struct js_event js_e_buffer[64];
ssize_t res = read(joy_dev, js_e_buffer, sizeof(struct js_event) * 64);
if( !res || (res < 0 && errno == EAGAIN) )
return C_OK;
if( res < 0 )
return C_FAIL;
if (res < (int) sizeof(struct js_event))// If non-complete bloc: ignored
return C_OK;
// Buffer decomposition in blocs (if the last is incomplete, it's ignored)
bool_t refresh_values = TRUE;
bool_t enable = FALSE;
int32_t idx = 0;
printf("Num events: %d\n", sizeof(struct js_event));
for (idx = 0; idx < res / sizeof(struct js_event); idx++) {
unsigned char type = js_e_buffer[idx].type;
unsigned char number = js_e_buffer[idx].number;
short value = js_e_buffer[idx].value;
if (type & JS_EVENT_INIT ) {
break;
}
else if (!value) {
break;
}
else if (type & JS_EVENT_BUTTON ) {
printf("BUTTON: %d\n", number);
refresh_values = TRUE;
switch(number ) {
case BUTTON_START :
start ^= 1;
ardrone_tool_set_ui_pad_start( start );
g_autopilot = FALSE;
break;
case BUTTON_SELECT :
ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
//g_exit = TRUE;
return C_OK;
case BUTTON_ZAP:
agent_zap();
break;
case BUTTON_GAZ_UP:
enable = TRUE;
gaz = +1;
break;
case BUTTON_GAZ_DOWN:
enable = TRUE;
printf("GAZ DOWN: %f \n", -1);
gaz = -1;
break;
case BUTTON_YAW_R:
enable = TRUE;
yaw = +js_e_buffer[idx].value;
break;
case BUTTON_YAW_L:
enable = TRUE;
yaw = -js_e_buffer[idx].value;
break;
/*case BUTTON_AUTO:
if (g_autopilot) {
refresh_values = TRUE;
}
else {
g_autopilot = TRUE;
}
break;*/
}
}
else if (type & JS_EVENT_AXIS ) {
printf("AXIS: %d\n", number);
if (number != AXIS_IGNORE3 && number != AXIS_IGNORE4) {
refresh_values = TRUE;
enable = TRUE;
float angle = value / (float)SHRT_MAX;
printf("ANGLE: %f", angle);
switch (number) {
case AXIS_PHI:
phi = angle;
break;
case AXIS_THETA:
theta = angle;
break;
//case AXIS_GAZ:
// gaz = angle;
// break;
//case AXIS_YAW:
// yaw = angle;
// break;
}
}
}
} // loop over events
if (refresh_values) {
g_autopilot = FALSE;
printf("\nSENDING: %f, %f, %f, %f\n", phi, theta, gaz, yaw);
ardrone_at_set_progress_cmd(enable, phi, theta, gaz, yaw);
}
return C_OK;
}
C_RESULT update_radioGP(void)
{
static float32_t roll = 0, pitch = 0, gaz=0, yaw=0;
static bool_t refresh_values = FALSE;
ssize_t res;
static struct js_event js_e_buffer[64];
res = read(joy_dev, js_e_buffer, sizeof(struct js_event) * 64);
if( !res || (res < 0 && errno == EAGAIN) )
return C_OK;
if( res < 0 )
return C_FAIL;
if (res < (int) sizeof(struct js_event))// If non-complete bloc: ignored
return C_OK;
// Buffer decomposition in blocs (if the last is incomplete, it's ignored)
int32_t idx = 0;
refresh_values = FALSE;
for (idx = 0; idx < res / sizeof(struct js_event); idx++)
{
if(js_e_buffer[idx].type & JS_EVENT_INIT )// If Init, the first values are ignored
{
break;
}
else if(js_e_buffer[idx].type & JS_EVENT_BUTTON )// Event Button detected
{
switch( js_e_buffer[idx].number )
{
case GP_BOARD_LEFT :
ardrone_tool_set_ui_pad_start(js_e_buffer[idx].value);
break;
case GP_SIDE_RIGHT :
ardrone_tool_set_ui_pad_r2(js_e_buffer[idx].value);
break;
case GP_IMPULSE :
ardrone_tool_set_ui_pad_select(js_e_buffer[idx].value);
break;
case GP_SIDE_LEFT_DOWN :
ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value);
break;
case GP_SIDE_LEFT_UP :
ardrone_tool_set_ui_pad_ad(js_e_buffer[idx].value);
break;
default: break;
}
}
else if(js_e_buffer[idx].type & JS_EVENT_AXIS )// Event Axis detected
{
refresh_values = TRUE;
switch( js_e_buffer[idx].number )
{
case GP_PITCH:
pitch = js_e_buffer[idx].value;
break;
case GP_GAZ:
gaz = js_e_buffer[idx].value;
break;
case GP_ROLL:
roll = js_e_buffer[idx].value;
break;
case GP_PID:
break;
case GP_YAW:
yaw = js_e_buffer[idx].value;
break;
default:
break;
}
}
else
{// TODO: default: ERROR (non-supported)
}
}
if(refresh_values)// Axis values to refresh
{
ardrone_at_set_progress_cmd( 1,
roll/25000.0,
pitch/25000.0,
-gaz/25000.0,
yaw/25000.0);
}
return C_OK;
}
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;
}
void ardronewin32_progress(int enable, float roll, float pitch, float gaz, float yaw)
{
ardrone_at_set_progress_cmd(enable, roll, pitch, gaz, yaw);
}
//NEW THREADS ADDED BY ME
DEFINE_THREAD_ROUTINE(drone_logic, data){
//the game is active from start, but the logic will start only when a match is active
struct timespec flying_sleep_time;
flying_sleep_time.tv_sec = 0;
flying_sleep_time.tv_nsec = 0000000;
int emptiness_counter = 0;
int shooting_counter = 0;
int hovering = 0; //0 hover, 1 move
float phi = 0.0; //left/right angle. Between [-1.0,+1.0], with negatives being leftward movement
float theta = 0.0; //front/back angle. Between [-1.0, +1.0], with negatives being frontward movement
float gaz = 0.0; //vertical speed. Between [-1.0, +1.0]
float yaw = 0.0; //Angular speed. Between [-1.0, +1.0]
while(game_active){
if(match_active){
if(takeoff){
//TODO:Uncomment this when you are ready to make the drone move autonomously
//ardrone_tool_set_ui_pad_start(1);
//ardrone_at_set_progress_cmd(0,0.0,0.0,0.0,0.0);
takeoff = 0;
}
//--- CHASING ---//
//NOTE: Hill have higher priority than enemy, this imply that if there is the enemy and an hill
//the drone will choose the hill.
//NOTE: doing so, the drone won't care if the enemy is standing between it and the hill
if(hill_in_sight){
emptiness_counter = 0;
shooting_counter = 0;
//move toward the hill
if((hill_distance > HILL_MIN_DISTANCE) && (hill_distance < HILL_MAX_DISTANCE)){
printf("MOVING TOWARD THE HILL\n");
hovering = 1;
phi = 0.0;
gaz = 0.0;
//--- SET THE YAW ---//
//This is to correct the direction of the drone
if(abs(hill_offset_from_center) > ERROR_FROM_CENTER_FOR_HILL){
//TODO: find the right multiplier for yaw and discover which way the drone turn
yaw = (hill_offset_from_center) * YAW_COEFF; //YAW_COEFF = 0.007
//yaw has to be between -1.0 and +1.0
if(yaw > 1.0) {
yaw = 1.0;
} else if(yaw < -1.0){
yaw = -1.0;
}
}
//--- SET THE APPROACHING SPEED ---//
//The closer the drone is to the hill, the slower it goes
//Need to be negative to move forward
theta = -1*((hill_distance) / THETA_COEFF); //TODO: find the right theta_coeff
if(theta > 0.0) {
theta = -0.1;
} else if(theta < -1.0){ //TODO: I may want to rethink the maximum speed that the drone can reach!
theta = -1.0;
}
flying_sleep_time.tv_sec = 1;
flying_sleep_time.tv_nsec = 0000000;
//hover over the hill
} else if(hill_distance < HILL_MIN_DISTANCE) {
//ardrone_at_set_progress_cmd(0,0,0,0,0); //to hover
printf("HOVERING ON TOP OF THE HILL\n");
hovering = 0;
phi = 0.0;
theta = 0.0;
gaz = 0.0;
yaw = 0.0;
//TODO: if you are close enough, you have to switch the cam and then inizialize
//the recognition procedure. (wait tot secs)
//TODO: I have problem switching cam
vp_os_mutex_lock(&drone_score_mutex);
drone_add_score = 1;
vp_os_mutex_unlock(&drone_score_mutex);
//TODO: here, you switch the camera back
flying_sleep_time.tv_sec = 5;
flying_sleep_time.tv_nsec = 0000000;
}
} else if(enemy_in_sight){
emptiness_counter = 0;
//back up! Too close to the enemy (we don't want to phisically hit the human player!)
if(enemy_distance < ENEMY_MIN_DISTANCE){
shooting_counter = 0;
hovering = 1;
theta = 1.0; //Move backward at maximum speed
phi = 0.0;
gaz = 0.0;
yaw = 0.0;
//TODO: test this!
flying_sleep_time.tv_sec = 2;
flying_sleep_time.tv_nsec = 0000000;
printf("BACKING UP FROM THE ENEMY\n");
} else if((enemy_distance > ENEMY_MIN_DISTANCE) && (enemy_distance < ENEMY_SHOOTING_DISTANCE)){
printf("INSIDE SHOOTING!!!!\n");
shooting_counter++;
if(shooting_counter > 5){
//TODO: make the drone move
//TODO:set some sleep time
printf("ENOUGH WITH THE SHOOTING\n");
} else {
//TODO: shoot!!
//make animation.
if(enemy_offset_from_center < ERROR_FROM_CENTER_FOR_ENEMY){
vp_os_mutex_lock(&enemy_score_mutex);
enemy_lose_score = 1;
vp_os_mutex_lock(&enemy_score_mutex);
}
printf("SHOOTING!!!!!!!!\n");
hovering = 0;
phi = 0.0;
theta = 0.0;
gaz = 0.0;
//--- SET THE YAW ---//
//This is to correct the direction of the drone
if(abs(enemy_offset_from_center) > ERROR_FROM_CENTER_FOR_ENEMY){
//TODO: find the right multiplier for yaw and discover wich way the drone turn
yaw = (enemy_offset_from_center) * YAW_COEFF; //YAW_COEFF = 0.007
//yaw has to be between -1.0 and +1.0
if(yaw > 1.0) {
yaw = 1.0;
} else if(yaw < -1.0){
yaw = -1.0;
}
}
flying_sleep_time.tv_sec = 1;
flying_sleep_time.tv_nsec = 0000000;
}
} else if((enemy_distance > ENEMY_SHOOTING_DISTANCE) && (enemy_distance < ENEMY_MAX_DISTANCE)){
shooting_counter = 0;
//TODO: in this case I may want to just make the drone search for hills
//TODO: it may escape, turning itself so the led won't face the enemy anymore
//TODO: set some sleep time
//--- SET THE YAW ---//
//This is to correct the direction of the drone
if(abs(enemy_offset_from_center) > ERROR_FROM_CENTER_FOR_ENEMY){
//TODO: find the right multiplier for yaw and discover wich way the drone turn
//TODO: in this case, I won't to make the drone move away from the enemy, so the algorithm below
//has to be changed
yaw = (enemy_offset_from_center) * YAW_COEFF; //YAW_COEFF = 0.007
//yaw has to be between -1.0 and +1.0
if(yaw > 1.0) {
yaw = 1.0;
} else if(yaw < -1.0){
yaw = -1.0;
}
}
}
//NOTHING IN SIGHT
} else {
//If nothing is in sight I set a counter that increment every time I pass directly from here
//and make the drone rotate around itself. After tot passages with nothing in sight I take measure to land the drone.
if(emptiness_counter == 0){
//TODO: set up everything for the algorithm to start
emptiness_counter = 1;
//yaw = something != to zero;
//everything else if zero (I don't know about hovering... problably should be 1 here)
} else {
emptiness_counter++;
//TODO: set some sleep time
if(emptiness_counter > 10){
//TODO:land the drone and make the game stop
//so set everything that should to zero
}
}
}
//---TELL THE DRONE TO MOVE---//
//TODO: uncomment the line below when you're ready!
//ardrone_at_set_progress_cmd(hovering,phi,theta,gaz,yaw);
nanosleep(&flying_sleep_time, NULL);
//NOTE: if the sleep remain under 2 secs it's ok.
//Only the hill recognition can have more, because the drone can't be hurt at that time.
//NOTE: I don't know why, but if I add this printf the condition below works
//printf("%d",drone_wounded);
//---BEING HIT---//
if(drone_wounded){
//TODO: make the drone move as if it was being shot
//maybe this should be moved in the flying thread
vp_os_mutex_lock(&drone_wound_mutex);
drone_wounded = 0;
vp_os_mutex_unlock(&drone_wound_mutex);
//TODO: you can choose between this animation, defined in Soft/Common/config.h
/*ARDRONE_ANIM_PHI_M30_DEG= 0,
A RDRONE_ANIM_PHI_30_DE*G,
ARDRONE_ANIM_THETA_M30_DEG,
ARDRONE_ANIM_THETA_30_DEG,
ARDRONE_ANIM_THETA_20DEG_YAW_200DEG,
ARDRONE_ANIM_THETA_20DEG_YAW_M200DEG,
ARDRONE_ANIM_TURNAROUND,
ARDRONE_ANIM_TURNAROUND_GODOWN,
ARDRONE_ANIM_YAW_SHAKE,
ARDRONE_ANIM_YAW_DANCE,
ARDRONE_ANIM_PHI_DANCE,
ARDRONE_ANIM_THETA_DANCE,
ARDRONE_ANIM_VZ_DANCE,
ARDRONE_ANIM_WAVE,
ARDRONE_ANIM_PHI_THETA_MIXED,
ARDRONE_ANIM_DOUBLE_PHI_THETA_MIXED,
ARDRONE_ANIM_FLIP_AHEAD,
ARDRONE_ANIM_FLIP_BEHIND,
ARDRONE_ANIM_FLIP_LEFT,
ARDRONE_ANIM_FLIP_RIGHT,
ARDRONE_NB_ANIM_MAYDAY*/
//anim_mayday_t param;
//ARDRONE_TOOL_CONFIGURATION_ADDEVENT (flight_anim, param, myCallback);
ardrone_at_set_led_animation(BLINK_GREEN_RED, 0.25, 4);
//TODO: freeze the drone for some time, also?
//nanosleep(&shot_rumble_time, NULL);
}
//MATCH OVER
} else {
//land the drone
ardrone_at_set_progress_cmd(0,0.0,0.0,0.0,0.0);
ardrone_tool_set_ui_pad_start(0);
//TODO: I may need some sleep time to make the drone land before closing everyting
}
}
return C_OK;
}
C_RESULT display_stage_transform (display_stage_cfg_t *cfg, vp_api_io_data_t *in, vp_api_io_data_t *out)
{
if(count > 1)
{
temp_x = cur_x;
temp_y = cur_y;
}
int i = 0;
const char *cascade_name = "/usr/share/opencv/haarcascades/haarcascade_frontalface_alt.xml";
CvHaarClassifierCascade *cascade = 0;
CvMemStorage *storage = 0;
CvSeq *faces;
cascade = (CvHaarClassifierCascade *) cvLoad (cascade_name, 0, 0, 0);
if(!cascade)
{
printf("Could not load cascade.\n");
return -1;
}
uint32_t width = 0, height = 0;
getPicSizeFromBufferSize (in->size, &width, &height);
//Obtain image from ARDrone and convert it to OpenCV format.
IplImage *img = ipl_image_from_data((uint8_t*)in->buffers[0], 1, 640, 360);
storage = cvCreateMemStorage (0);
cvClearMemStorage (storage);
//DetectFaces
faces = cvHaarDetectObjects (img, cascade, storage, 1.11, 4, 0, cvSize(30, 30), cvSize (0, 0));
for (i = 0; i < (faces ? faces->total : 0); i++)
{
CvRect *r = (CvRect *) cvGetSeqElem (faces, i);
doMosaic(img, r->x, r->y, r->width, r->height, 10);
cur_x = r->x;
cur_y = r->y;
//printf("In the loop.\n");
count++; //increases count when detect faces
}
if(count > 2)
{
printf("prev_x = %d, prev_y = %d, cur_x = %d, cur_y = %d\n", temp_x, temp_y, cur_x, cur_y);
if(cur_x - temp_x > 3)
{
//printf("Move Right.\n");
ardrone_at_set_progress_cmd( 1, 1.0, 0.0, 0.0, 0.0 );
}
if(cur_x - temp_x < -3)
{
//printf("Move Left.\n");
ardrone_at_set_progress_cmd( 1, -1.0, 0.0, 0.0, 0.0 );
}
if(cur_y - temp_y > 3)
{
//printf("Move Down.\n");
ardrone_at_set_progress_cmd( 1, 0.0, 0.0, -1.0, 0.0 );
}
if(cur_y - temp_y < -5)
{
//printf("Move Up.\n");
//ardrone_at_reset_com_watchdog();
ardrone_tool_set_progressive_cmd( 1, 0.0, 0.0, 0.3, 0.0 );
//vp_os_delay(1000);
}
}
//emergency++;p
if(count > 50)
{
printf("Exit\n");
ardrone_tool_set_ui_pad_start(0);
return -1;
}
cvNamedWindow("FaceDetect", CV_WINDOW_AUTOSIZE);
cvShowImage("FaceDetect", img);
cvWaitKey(1);
cvReleaseImage(&img);
return C_OK;
}
