/** Converts relative angle/distance coordinates to absolute. */ void strat_da_rel_to_xy_abs(float a_deg, float distance_mm, int *x_mm, int *y_mm) { *x_mm = distance_mm * cos(RAD(a_deg)) + position_get_x_s16(&robot.pos); *y_mm = distance_mm * sin(RAD(a_deg)) + position_get_y_s16(&robot.pos); }
void strat_do_emergency_behaviour(void){ //when pince is down... :s static uint24_t starttime; if(beudatraj.state==READY){ int16_t my_y = position_get_y_s16(&beudabot.posr); switch (Strat_Action) { case 0: /*Go p1*/ i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE); if(beudabot.color==COLOR_GREEN) trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X,ON_TABLE_ELT_G1_Y); else trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X,ON_TABLE_ELT_R1_Y); Strat_Action++; break; case 1: /*Go p2*/ i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE); if(beudabot.color==COLOR_GREEN) trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X+60,ON_TABLE_ELT_G1_Y); else trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X+60,ON_TABLE_ELT_R1_Y); Strat_Action++; break; case 2: i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE); if(beudabot.color==COLOR_GREEN) trajectory_goto_xy_abs(&beudatraj, BUILD_AREA_1b1_X-30,BUILD_AREA_1b1_Y); else trajectory_goto_xy_abs(&beudatraj, BUILD_AREA_1b4_X-30,BUILD_AREA_1b4_Y); Strat_Action++; break; case 3: trajectory_a_abs(&beudatraj,0); Strat_Action++; break; case 4: quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_VERY_SLOW, QR_SPEED_D_NEG_VERY_SLOW); // Vitesse trajectory_d_a_rel(&beudatraj, 30, 0); Strat_Action++; break; case 5: i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_EJECT); starttime = gettime(); Strat_Action++; break; case 6: if(gettime()>starttime + DELAY_TAPIS_EJECT) Strat_Action++; break; case 7: trajectory_d_a_rel(&beudatraj, -30, 0); Strat_Action++; break; case 8: Strat_Action=0; Strat_State=STRAT_EXIT; break; } } }
/* return the distance to a point in the area */ int16_t distance_from_robot(int16_t x, int16_t y) { return distance_between(position_get_x_s16(&mainboard.pos), position_get_y_s16(&mainboard.pos), x, y); }
/* return 1 or 0 depending on which side of a line (y=cste) is the * robot. works in yellow or blue color. */ uint8_t y_is_more_than(int16_t y) { int16_t posy; posy = position_get_y_s16(&mainboard.pos); return __y_is_more_than(posy, y); }
/* return true if the point is in area */ uint8_t robot_is_in_area(int16_t margin) { return is_in_area(position_get_x_s16(&mainboard.pos), position_get_y_s16(&mainboard.pos), margin); }
void oa_manage(struct obstacle_avoidance *oa){ xy_position my_pos_xy; my_pos_xy.x = position_get_x_s16(oa->traj->position); my_pos_xy.y = position_get_y_s16(oa->traj->position); uint8_t my_quad = oa_get_quad(my_pos_xy); if(oa->state==OA_TARGET_SET && oa->traj->state==READY){ //new target where to go ! uint8_t opp_quad = oa_get_quad(oa->opponent_xy); uint8_t target_quad = oa_get_quad(oa->target_xy); //oa->state==OA_PROCESSING; uint8_t tmpquad; #ifdef DEBUG_OA UART_CPutString("oa> processing new target\r\n"); #endif if(oa_get_com_dt(my_pos_xy,oa->target_xy)!=0){ //!=0 #ifdef DEBUG_OA UART_CPutString("oa> same dt than target\r\n"); #endif if(oa_get_com_dt(my_pos_xy,oa->opponent_xy)!=0){ #ifdef DEBUG_OA UART_CPutString("oa> same dt than opp-> sixt\r\n"); #endif oa_sixt(oa); } else{ #ifdef DEBUG_OA UART_CPutString("oa> not same dt than opp ->xy!\r\n"); #endif trajectory_goto_xy_abs(oa->traj,oa->target_xy.x,oa->target_xy.y); oa->state=OA_IN_TARGET; } } else{ //==0 #ifdef DEBUG_OA UART_CPutString("oa> not in same dt than target\r\n"); #endif #ifdef DEBUG_OA sprintf(uart_out_buffer,"oa> quads: me %d opp %d targ %d\r\n",my_quad,opp_quad,target_quad); UART_PutString(&uart_out_buffer); #endif if(oa_is_adj(my_quad,opp_quad)){ //me & opp are adj #ifdef DEBUG_OA UART_CPutString("oa> I'm adj to opponent\r\n"); #endif if(my_quad==target_quad){ //my_quad==target_quad #ifdef DEBUG_OA UART_CPutString("oa> my_qd=tg_qd->my_qd center\r\n"); #endif oa_goto_quad(oa,my_quad); } else{ //my_quad!=target_quad #ifdef DEBUG_OA UART_CPutString("oa> my_qd!=tg_qd\r\n"); #endif if(target_quad==opp_quad){ //target_quad==opp_quad #ifdef DEBUG_OA UART_CPutString("oa> tg_qd=op_qd->!opp half\r\n"); #endif tmpquad = oa_get_no_half_quad(oa->opponent_xy,target_quad,my_quad); oa_goto_quad(oa,tmpquad); #ifdef DEBUG_OA sprintf(uart_out_buffer,"oa> !opp half is qd %d\r\n",tmpquad); UART_PutString(&uart_out_buffer); #endif } else{//target_quad!=opp_quad #ifdef DEBUG_OA UART_CPutString("oa> tg_qd!=op_qd->sym opp quad (but first, my quad)\r\n"); #endif // /*last modified stuff: before: oa_goto_quad(oa,oa_get_sym_quad(opp_quad)); only..*/ if(!(oa->flags & OA_FLAG_GOSYMQD_PART1_DONE)){ setBit(oa->flags,OA_FLAG_GOSYMQD_PART1_DONE); #ifdef DEBUG_OA UART_CPutString("oa> my quad...\r\n"); #endif oa_goto_quad(oa,my_quad); } else { clearBit(oa->flags,OA_FLAG_GOSYMQD_PART1_DONE); #ifdef DEBUG_OA UART_CPutString("oa> opp sym quad...\r\n"); #endif oa_goto_quad(oa,oa_get_sym_quad(opp_quad)); } } } } else{ //me & opp aren't adj #ifdef DEBUG_OA UART_CPutString("oa> I'm not adj to opponent\r\n"); #endif if(my_quad==opp_quad){ //my_quad==opp_quad #ifdef DEBUG_OA UART_CPutString("oa> my_qd=opp_qd->sixt\r\n"); #endif oa_sixt(oa); //apply the celebre "Sixt()" algorithm.. } else{ // !my_quad==opp_quad #ifdef DEBUG_OA UART_CPutString("oa> my_qd!=opp_qd\r\n"); #endif if(target_quad==opp_quad){ //target_quad==opp_quad #ifdef DEBUG_OA UART_CPutString("oa> tg_qd=opp_qd->!opp half\r\n"); #endif //tmpquad = (my_quad-1+oa_get_no_half_quad(oa->opponent_xy,target_quad))%4+1; tmpquad = oa_get_no_half_quad(oa->opponent_xy,target_quad,my_quad); //oa_goto_quad(oa,tmpquad); if(!(oa->flags & OA_FLAG_GOOPPQD_PART1_DONE)){ setBit(oa->flags,OA_FLAG_GOOPPQD_PART1_DONE); #ifdef DEBUG_OA UART_CPutString("oa> my quad...\r\n"); #endif oa_goto_quad(oa,my_quad); } else { clearBit(oa->flags,OA_FLAG_GOOPPQD_PART1_DONE); #ifdef DEBUG_OA UART_CPutString("oa> !opp quad...\r\n"); #endif oa_goto_quad(oa,tmpquad); } //-1 because we don't want to go to the same half quad than the opponent #ifdef DEBUG_OA sprintf(uart_out_buffer,"oa> !opp half is qd %d\r\n",tmpquad); UART_PutString(&uart_out_buffer); #endif } else{ // !target_quad==opp_quad #ifdef DEBUG_OA UART_CPutString("oa> tg_qd!=opp_qd->tg qd\r\n"); #endif oa_goto_quad(oa,target_quad); //0 middle, +1 upper half, -1 lower half } } } } } /* //we calc the dist between me and the opp. oa->curr_dist_me_opp =(int16_t)(abs(my_pos_xy.x - oa->opponent_xy.x)*abs(my_pos_xy.x - oa->opponent_xy.x)+abs(my_pos_xy.y - oa->opponent_xy.y)*abs(my_pos_xy.y - oa->opponent_xy.y)); //if we are getting closer, and we are tooo close, we just stop, wait, if((oa->curr_dist_me_opp < oa->prev_dist_me_opp) && (oa->curr_dist_me_opp < MIN_COLLISION_DIST)){ #ifdef DEBUG_OA sprintf(uart_out_buffer,"oa> opp @ %d(^1/2) cm !!!! \r\n",oa->curr_dist_me_opp); UART_PutString(&uart_out_buffer); #endif trajectory_hardstop(oa->traj); oa->state=OA_COLLISION_AVOIDED; oa->timeout=gettime(); } if(oa->state==OA_COLLISION_AVOIDED && (gettime()-oa->timeout)>COLLISION_AVOIDED_TIMEOUT ){ //hope nothing's broken... #ifdef DEBUG_OA UART_CPutString("oa> collision avoided ?\r\n"); #endif oa->state=OA_TARGET_SET; oa->traj->state=READY; oa->timeout = 0; } */ oa->prev_dist_me_opp = oa->curr_dist_me_opp; oa->prev_quad = my_quad; }
void oa_sixt(struct obstacle_avoidance * oa){ /*we check different possible "next_area" that are in fact the adj_areas and try to find the shortest, and the most safe one*/ //we start by computing the possibles next areas int16_t my_pos_x,my_pos_y; play_area my_area,next_area,target_area,opp_area; uint8_t i,j; int16_t d_nxt_area_opp[3],d_nxt_area_target[3]; float tmp; uint8_t index0,index1,index2; xy_position my_pos_xy; next_area.x=0; next_area.y=0; my_pos_x=position_get_x_s16(oa->traj->position); my_pos_y=position_get_y_s16(oa->traj->position); my_pos_xy.x = my_pos_x; my_pos_xy.y = my_pos_y; my_area = oa_xy_to_area(my_pos_x,my_pos_y); target_area = oa_xy_to_area(oa->target_xy.x,oa->target_xy.y); opp_area = oa_xy_to_area(oa->opponent_xy.x,oa->opponent_xy.y); //:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: char is_opp_adj=oa_is_in_v4(&my_area,&opp_area); //:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: #ifdef DEBUG_OA sprintf(uart_out_buffer,"oa> my area {%d,%d}\r\n",my_area.x,my_area.y); UART_PutString(&uart_out_buffer); sprintf(uart_out_buffer,"oa> target area {%d,%d}\r\n",target_area.x,target_area.y); UART_PutString(&uart_out_buffer); sprintf(uart_out_buffer,"oa> opp area {%d,%d}\r\n",opp_area.x,opp_area.y); UART_PutString(&uart_out_buffer); #endif /*let's calc adj areas now..*/ oa_find_adj_areas_to_target(oa,&my_area,&target_area); /*we check if one or more, of the computed adj areas isn't/aren't one of the forbidden areas or just out of the game_area*/ for(i=0;i<3;i++){ for(j=0;j<NB_FORBIDDEN_AREAS;j++){ xy_position pos1 = oa_area_to_xy(oa->adj_area_tab[i]); // xy_position pos2 = oa_area_to_xy(target_area); if( ( (oa->adj_area_tab[i].x == oa->forbidden_area[j].x) && (oa->adj_area_tab[i].y == oa->forbidden_area[j].y)) || oa_is_out_of_table(oa->adj_area_tab[i]) || !( oa_get_com_dt(pos1,oa->target_xy)) || !( oa_get_com_dt(my_pos_xy,pos1)) || ((oa->adj_area_tab[i].x ==opp_area.x)&&(oa->adj_area_tab[i].y==opp_area.y)) ){ /*if adj area is target, just forget about the forbidden areas...*/ if((oa->adj_area_tab[i].x == target_area.x) && (oa->adj_area_tab[i].y == target_area.y)){ //not forbidden areas when it's out target! d_nxt_area_opp[i] = 0; d_nxt_area_target[i] = 0; } else { //:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: if (is_opp_adj) { if (oa_is_in_v4(&oa->adj_area_tab[i],&opp_area)) { //on a une case en diagonale, on veut la remplacer if( oa->adj_area_tab[i].x==opp_area.x) //on remplace par une case du voisinage de my_area qui n'est pas l'adacent oa->adj_area_tab[i].x=my_area.x; else //oa->adj_area_tab[i].y==opp_area.y oa->adj_area_tab[i].y=my_area.y; } else { d_nxt_area_opp[i] = MARKED_FORBIDDEN; d_nxt_area_target[i] = MARKED_FORBIDDEN; } } else { d_nxt_area_opp[i] = MARKED_FORBIDDEN; d_nxt_area_target[i] = MARKED_FORBIDDEN; } //:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: } //if((oa->adj_area_tab[i].x != target_area.x) && (oa->adj_area_tab[i].y != target_area.y)){} #ifdef DEBUG_OA sprintf(uart_out_buffer,"oa> area %d is forb. or out of table!\r\n",i); UART_PutString(&uart_out_buffer); #endif //} } } } /*we calc the dist from opponent and from target for authorized next areas we'll choose the shortest one from the target AND the most far from the opponent -(NO YET)distance from target has a coeff of 1/2 -distance from opponent has a coeff 1 we'll choose the smallest result[] */ uint16_t dist_target_opp = ABS(target_area.x - opp_area.x)*ABS(target_area.x - opp_area.x) + ABS(target_area.y - opp_area.y)*ABS(target_area.y - opp_area.y); uint16_t dist_me_opp = ABS(opp_area.x - my_area.x)*ABS(opp_area.x - my_area.x) + ABS(opp_area.y - my_area.y)*ABS(opp_area.y - my_area.y); if((dist_me_opp > MIN_DIST_BEFORE_IGNORING_OPP) ){ //&& (dist_me_opp > MIN_DIST_BEFORE_IGNORING_OPP) /*if we, and the target are too far from the opponent, we ignore the opponent for the next_area choice*/ #ifdef DEBUG_OA sprintf(uart_out_buffer,"oa> opp too far! d:%d(^1/2) check. d aj<->targ.!\r\n",dist_target_opp); UART_PutString(&uart_out_buffer); #endif for(i=0;i<3;i++){ if( d_nxt_area_target[i] != MARKED_FORBIDDEN){ d_nxt_area_target[i] = (int16_t)((oa->adj_area_tab[i].x - target_area.x)*(oa->adj_area_tab[i].x - target_area.x) + (oa->adj_area_tab[i].y - target_area.y)*(oa->adj_area_tab[i].y - target_area.y)); } } /*sorting d_my_area_target ascending (0 shortest, 3 longest)*/ uint8_t sorted_d_nxt_area_target[3]; oa_sort_tab3(d_nxt_area_target,sorted_d_nxt_area_target); index0=sorted_d_nxt_area_target[0]; index1=sorted_d_nxt_area_target[1]; index2=sorted_d_nxt_area_target[2]; #ifdef DEBUG_OA sprintf(uart_out_buffer,"oa> sorted areas {%d,%d}{%d,%d}{%d,%d}\r\n", oa->adj_area_tab[index0].x,oa->adj_area_tab[index0].y, oa->adj_area_tab[index1].x,oa->adj_area_tab[index1].y, oa->adj_area_tab[index2].x,oa->adj_area_tab[index2].y); UART_PutString(&uart_out_buffer); #endif /*electing next area (we have to choose the shortest)*/ for(i=0;i<3;i++){ j = sorted_d_nxt_area_target[2-i]; //var reuse if(d_nxt_area_target[j]!=MARKED_FORBIDDEN){ next_area = oa->adj_area_tab[j]; } } } else{ if(dist_target_opp==0){ //really close if(oa->wait<TIMEOUT_WAIT_IN_PLACE){ #ifdef DEBUG_OA sprintf(uart_out_buffer,"oa> opp in same place than target! waiting %d \r\n",oa->wait); UART_PutString(&uart_out_buffer); #endif next_area.x = 0; next_area.y = 0; oa->wait++; } else{ /**/ #ifdef DEBUG_OA UART_CPutString("oa> waiting for soooo long... going back to prev quad\r\n"); #endif oa_goto_quad(oa,oa->prev_quad); } } else{ #ifdef DEBUG_OA sprintf(uart_out_buffer,"oa> opp too close d:%d(^1/2) checking dist op<->adj\r\n",dist_me_opp); UART_PutString(&uart_out_buffer); #endif for(i=0;i<3;i++){ if( d_nxt_area_opp[i] != MARKED_FORBIDDEN) d_nxt_area_opp[i] = (int16_t)((oa->adj_area_tab[i].x - opp_area.x)*(oa->adj_area_tab[i].x - opp_area.x) + (oa->adj_area_tab[i].y - opp_area.y)*(oa->adj_area_tab[i].y - opp_area.y)); } uint8_t sorted_d_nxt_area_opp[3]; oa_sort_tab3(d_nxt_area_opp,sorted_d_nxt_area_opp); index0= sorted_d_nxt_area_opp[0]; index1= sorted_d_nxt_area_opp[1]; index2= sorted_d_nxt_area_opp[2]; #ifdef DEBUG_OA sprintf(uart_out_buffer,"oa> sorted areas {%d,%d}{%d,%d}{%d,%d}\r\n", oa->adj_area_tab[index0].x,oa->adj_area_tab[index0].y, oa->adj_area_tab[index1].x,oa->adj_area_tab[index1].y, oa->adj_area_tab[index2].x,oa->adj_area_tab[index2].y); UART_PutString(&uart_out_buffer); #endif //electing next area (we have to choose the longest) for(i=0;i<3;i++){ j = sorted_d_nxt_area_opp[2-i]; if(d_nxt_area_opp[j]!=MARKED_FORBIDDEN){ next_area = oa->adj_area_tab[j]; } } } } /* We have to take the farest from the opp, and the shortest from target if opp is too far(), we have to ignore it */ //UART_CPutString("oa>election done..\r\n"); if((next_area.x == target_area.x) && (next_area.y == target_area.y)){ trajectory_goto_xy_abs(oa->traj,(double)oa->target_xy.x,(double)oa->target_xy.y); oa->state=OA_IN_TARGET; } else{ if(next_area.x==0 && next_area.y==0){ //no one has been elected, stay in place for the moment #ifdef DEBUG_OA UART_CPutString("oa> sixt: nowhere to go, going back \r\n"); #endif next_area.x= oa->prev_area.x; next_area.y= oa->prev_area.y; } else{ #ifdef DEBUG_OA sprintf(uart_out_buffer,"oa> sixt: going to area {%d,%d}\r\n",next_area.x,next_area.y); UART_PutString(&uart_out_buffer); #endif } xy_position pos; pos = oa_area_to_xy(next_area); /*we save our previous area, is next time we have nowhere to go..*/ trajectory_goto_xy_abs(oa->traj,(double)pos.x,(double)pos.y); } oa->prev_area.x = next_area.x; oa->prev_area.y = next_area.y; }