void strat_go_get_elts_on_table(struct strategy * strat){
static uint8_t elts_to_store;
static uint24_t starttime;
static uint8_t storage_lock;
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*/
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:
Strat_Action=0;
Strat_State=STRAT_EMERGENCY_BEHAVIOUR;
break;
/*
case 2:
//before continuing the seq, we check how many elts we got
elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN);
Strat_Action++;
break;*/
case 3:
if(elts_to_store==2){
if(strat_store_elt(elts_to_store)){
Strat_Action++;
setBit(strat_flags,TWO_ELTS_LOADED);
i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE); //after storage, it stops bouffing ??
}
}
else //if we don't have 2elts we just continu the seq.
Strat_Action++;
break;
case 4:
/*here, if we got 1 or 2 elements, they're stored, and the pince is well placed*/
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_SLOW, QR_SPEED_D_NEG_SLOW); // Vitesse
if(beudabot.color==COLOR_GREEN)
trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X+60,ON_TABLE_ELT_G1_Y+25);
else
trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X+60,ON_TABLE_ELT_R1_Y-25);
Strat_Action++;
break;
case 5:
elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN);
Strat_Action++;
break;
case 6:
if(elts_to_store==2){
if(strat_store_elt(elts_to_store)){
/*Warning: no timeout here, we trust the tapis-roulo at this time...*/
if(strat_flags & TWO_ELTS_LOADED){
//Yes !! we got 4 elts, lets build a real temple!
i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_PROTECT); //we don't want more for the moment
Strat_Action = 0;
Strat_State++;
setBit(strat_flags,I_CAN_BUILD_A_TEMPLE); //for lint deposit operation...
clearBit(strat_flags,TWO_ELTS_LOADED);
}
}
}
else
Strat_Action++;
break;
case 7:
/*
here the situation is:
- we got 2 elts stored, and we are looking for 2 others...
- we have nothing.. (check strat_flags & TWO_ELTS_LOADED the know the answer...)
*/
if(beudabot.color==COLOR_GREEN)
trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X,ON_TABLE_ELT_G1_Y+25);
else
trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X,ON_TABLE_ELT_R1_Y-25);
Strat_Action++;
break;
case 8:
elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN);
Strat_Action++;
break;
case 9:
if(elts_to_store==2){ //owkey, we have now 2 elts
if(strat_store_elt(elts_to_store)){
if(strat_flags & TWO_ELTS_LOADED){ //AWESOME, we got 2elts inside, and 2elts stored...
i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_PROTECT); //we don't want more for the moment
Strat_Action = 0;
Strat_State++; //let's build !!
clearBit(strat_flags,TWO_ELTS_LOADED);
setBit(strat_flags,I_CAN_BUILD_A_TEMPLE); //for lint deposit operation lock...
}
}
}
else
Strat_Action++;
break;
case 10:
if(beudabot.color==COLOR_GREEN)
trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X,ON_TABLE_ELT_G1_Y+50);
else
trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X,ON_TABLE_ELT_R1_Y-50);
Strat_Action++;
break;
case 11:
elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN);
Strat_Action++;
break;
case 12:
if(elts_to_store==2){
if(strat_store_elt(elts_to_store)){
if(strat_flags & TWO_ELTS_LOADED){ //AWESOME, we got 2elts inside, and 2elts stored...
Strat_Action = 0;
i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_PROTECT); //we don't want more for the moment
Strat_State++; //next op is building...
setBit(strat_flags,I_CAN_BUILD_A_TEMPLE); //for lint deposit operation...
clearBit(strat_flags,TWO_ELTS_LOADED);
}
}
}
else
Strat_Action++;
break;
case 13:
if(beudabot.color==COLOR_GREEN)
trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_G1_X+60,ON_TABLE_ELT_G1_Y+50);
else
trajectory_goto_xy_abs(&beudatraj,ON_TABLE_ELT_R1_X+60,ON_TABLE_ELT_R1_Y-50);
Strat_Action++;
break;
case 14:
/*I still don't have 4 elts :(, let's build with what we have*/
UART_CPutString("Going to build with less than 4elts :( go back and try again ???\r\n");
Strat_Action=0;
Strat_State++;
clearBit(strat_flags,TWO_ELTS_LOADED);
break;
}
}
void strat_get_elt_rand_disp_position(void){
int8_t i2c_ans;
if(beudatraj.state==READY){
switch(Strat_Action){
case 0:
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_SLOW, QR_SPEED_D_NEG_SLOW); // Vitesse
if(beudabot.color==COLOR_GREEN)
oa_set_target(&beudaoa, R_ELT_DISP_RAND_1_X, R_ELT_DISP_RAND_1_Y+30);
else
oa_set_target(&beudaoa, G_ELT_DISP_RAND_1_X, G_ELT_DISP_RAND_1_Y-30);
Strat_Action++;
break;
case 1:
if(beudaoa.state == OA_IN_TARGET){
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 2:
i2c_ans=i2c_send_and_receive(I2C_CAPTEUR_IR_ADDR,I2C_IR_CHECK_IN_FRONT);
if(i2c_ans==1){
/*disp is not here*/
//rand_disp_position=ELT_LOCATION_RAND2;
Strat_Action++;
}
else if(i2c_ans==2) {
/*Yeay we got it ! !*/
Strat_Action=0;
Strat_State = STRAT_CONSTRUCT_TEMPLE_2_1;
//rand_disp_position=ELT_LOCATION_RAND1;
}
break;
case 3:
trajectory_d_a_rel(&beudatraj,-50,0);
Strat_Action++;
break;
case 4:
if(beudabot.color==COLOR_GREEN)
trajectory_goto_xy_abs(&beudatraj, G_ELT_DISP_RAND_2_X, G_ELT_DISP_RAND_2_Y-30);
else
trajectory_goto_xy_abs(&beudatraj, G_ELT_DISP_RAND_2_X, G_ELT_DISP_RAND_2_Y-30);
break;
case 5:
/*the disp isn't the 1st, we get to the 2nd*/
if(beudabot.color==COLOR_GREEN)
oa_set_target(&beudaoa, R_ELT_DISP_RAND_1_X, R_ELT_DISP_RAND_1_Y+30);
else
oa_set_target(&beudaoa, G_ELT_DISP_RAND_1_X, G_ELT_DISP_RAND_1_Y-30);
Strat_Action++;
break;
}
}
}
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;
}
}
}
void strat_go_get_elts_disp(struct strategy *strat,int8_t type){ //type = 0 FIXED,1 RAND1,2 RAND2
static uint8_t elts_to_store;
uint24_t starttime;
if(beudatraj.state==READY){
switch(Strat_Action){
case 0:
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_A_POS_SLOW, QR_SPEED_A_NEG_SLOW); // Vitesse
if(beudabot.color==COLOR_GREEN){
if(type==0)
oa_set_target(&beudaoa, G_ELT_DISP_FIXED_X-30, G_ELT_DISP_FIXED_Y);
else if(type==1)
oa_set_target(&beudaoa, G_ELT_DISP_RAND_1_X, G_ELT_DISP_RAND_1_Y-30);
else if(type==2)
oa_set_target(&beudaoa, G_ELT_DISP_RAND_2_X, G_ELT_DISP_RAND_2_Y-30);
}
else{
if(type==0)
oa_set_target(&beudaoa, R_ELT_DISP_FIXED_X-30, R_ELT_DISP_FIXED_Y);
else if(type==1)
oa_set_target(&beudaoa, R_ELT_DISP_RAND_1_X, R_ELT_DISP_RAND_1_Y-30);
else if(type==2)
oa_set_target(&beudaoa, R_ELT_DISP_RAND_2_X, R_ELT_DISP_RAND_2_Y-30);
}
Strat_Action++;
break;
case 1:
if(beudaoa.state == OA_IN_TARGET)
if(beudabot.color ==COLOR_GREEN){
if(type==0)
trajectory_goto_xy_abs(&beudatraj,G_ELT_DISP_FIXED_X, G_ELT_DISP_FIXED_Y);
else if(type==1)
trajectory_goto_xy_abs(&beudatraj,G_ELT_DISP_RAND_1_X, G_ELT_DISP_RAND_1_Y);
else if (type==2)
trajectory_goto_xy_abs(&beudatraj,G_ELT_DISP_RAND_2_X, G_ELT_DISP_RAND_2_Y);
}
else{
if(type==0)
trajectory_goto_xy_abs(&beudatraj,R_ELT_DISP_FIXED_X, R_ELT_DISP_FIXED_Y);
else if(type==1)
trajectory_goto_xy_abs(&beudatraj,R_ELT_DISP_RAND_1_X, R_ELT_DISP_RAND_1_Y);
else if (type==2)
trajectory_goto_xy_abs(&beudatraj,R_ELT_DISP_RAND_2_X, R_ELT_DISP_RAND_2_Y);
}
Strat_Action++;
break;
case 2:
quadramp_set_1st_order_vars(&beudabot.qr_d, QR_SPEED_D_POS_VERY_SLOW, QR_SPEED_D_NEG_VERY_SLOW); // Vitesse
i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE);
trajectory_d_a_rel(&beudatraj, ELT_DISP_IN_OUT_DIST, 0); //we move till the disp
starttime = gettime();
Strat_Action++;
clearBit(strat_flags,TWO_ELTS_LOADED); //we prepare the flag...
break;
case 3:
elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN);
if(elts_to_store==2){
/*Woow I got 2 elts, let's store them now, I suppose the tapis-roulo stoped rolling by himself now..*/
if(strat_store_elt(elts_to_store)){
/*if storage proc ended...*/
Strat_Action++; //we got 2, let's take some other two..
setBit(strat_flags,TWO_ELTS_LOADED);
}
}
else{
//if it's taking too much time we go builg smtg
/*we make a special move to get them....or not*/
if(gettime()> starttime + DELAY_TAPIS_BOUFFE)
Strat_Action++;
}
break;
case 4:
/*we suck again here...*/
i2c_send_only(I2C_TAPIS_ROULEAUX_ADDR, I2C_TAPIS_BOUFFE);
Strat_Action++;
break;
case 5:
elts_to_store = i2c_send_and_receive(I2C_TAPIS_ROULEAUX_ADDR,I2C_TAPIS_GET_NB_ELTS_IN);
if(elts_to_store==2){
/*Woow I got 2 elts, let's store them now, I suppose the tapis-roulo stoped rolling by himself now..*/
if(strat_store_elt(elts_to_store)){
/*if storage proc ended...*/
Strat_State=STRAT_CONSTRUCT_TEMPLE_2_1; //we got 2, let's build smtg
setBit(strat_flags,I_CAN_BUILD_A_TEMPLE);
}
}
else{
//if it's taking too much time we go builg smtg
/*we make a special move to get them....or not*/
if(gettime()> starttime + DELAY_TAPIS_BOUFFE){
Strat_State=STRAT_CONSTRUCT_TEMPLE_2_1;
/*we can't wait more... let's just build smtg*/
clearBit(strat_flags,I_CAN_BUILD_A_TEMPLE);//without a lint...
}
}
break;
}
}
}
void goto_abs(short x, short y)
{
trajectory_goto_xy_abs(&robot.traj, ((double) x) / 10.0, ((double) y / 10.0));
}
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_goto_quad(struct obstacle_avoidance * oa,uint8_t destquad){
xy_position dest;
dest = oa_quad_to_xy(destquad);
trajectory_goto_xy_abs(oa->traj,dest.x,dest.y);
}
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;
}
