void strat_autopos(int16_t x, int16_t y, int16_t a, int16_t epaisseurRobot)
{
robot.is_aligning = 1;
// Pour se recaler, on met le robot en regulation angulaire, on reduit la vitesse et l'acceleration
// On diminue la sensibilite on augmente la constante de temps de detection du bloquage
bd_set_thresholds(&robot.distance_bd, 5000, 2);
trajectory_set_speed(&robot.traj, 100, 100);
robot.mode = BOARD_MODE_DISTANCE_ONLY;
// On recule jusqu'a� qu'on ait touche un mur
trajectory_d_rel(&robot.traj, (double) -2000);
wait_traj_end(END_BLOCKING);
trajectory_hardstop(&robot.traj);
bd_reset(&robot.distance_bd);
bd_reset(&robot.angle_bd);
robot.mode = BOARD_MODE_ANGLE_DISTANCE;
position_set(&robot.pos, epaisseurRobot, 0, COLOR_A(0.));
/* On se mets a la bonne position en x. */
trajectory_d_rel(&robot.traj, 80);
wait_traj_end(END_TRAJ);
/* On se tourne face a la paroi en Y. */
trajectory_only_a_abs(&robot.traj, COLOR_A(90));
wait_traj_end(END_TRAJ);
/* On recule jusqu'a avoir touche le bord. */
trajectory_d_rel(&robot.traj, (double) -2000);
wait_traj_end(END_BLOCKING);
bd_reset(&robot.distance_bd);
bd_reset(&robot.angle_bd);
/* On reregle la position. */
robot.pos.pos_d.y = COLOR_Y(epaisseurRobot);
robot.pos.pos_s16.y = COLOR_Y(epaisseurRobot);
trajectory_d_rel(&robot.traj, 80);
wait_traj_end(END_TRAJ);
trajectory_goto_forward_xy_abs(&robot.traj, x, COLOR_Y(y));
wait_traj_end(END_TRAJ);
/* Pour finir on s'occuppe de l'angle. */
trajectory_a_abs(&robot.traj, COLOR_A((double)a));
wait_traj_end(END_TRAJ);
/* On remet le robot dans son etat initial. */
robot.mode = BOARD_MODE_ANGLE_DISTANCE;
robot.is_aligning = 0;
/* On se met en place a la position demandee. */
trajectory_set_speed(&robot.traj, speed_mm2imp(&robot.traj, 300), speed_rd2imp(&robot.traj, 2.5));
}
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_construct_temple(struct strategy * strat,uint8_t build_area,uint8_t half){
static uint24_t starttime;
if(beudatraj.state==READY){
switch(Strat_Action){
case 0:
if(build_area==3){
if(beudabot.color==COLOR_GREEN)
oa_set_target(&beudaoa, BUILD_AREA_3b1_X,BUILD_AREA_3b1_Y);
else
oa_set_target(&beudaoa, BUILD_AREA_3b2_X,BUILD_AREA_3b2_Y);
}
else if(build_area==2){
if(half == 1)
oa_set_target(&beudaoa, BUILD_AREA_2b1_X,BUILD_AREA_2b1_Y);
else
oa_set_target(&beudaoa, BUILD_AREA_2b2_X,BUILD_AREA_2b2_Y);
}
else if(build_area==1){
/*here we should check our pos and determinate the closest one*/
if(half == 1)
oa_set_target(&beudaoa, BUILD_AREA_1b1_X,BUILD_AREA_1b1_Y);
else if(half == 2)
oa_set_target(&beudaoa, BUILD_AREA_1b2_X,BUILD_AREA_1b2_Y);
else if(half == 3)
oa_set_target(&beudaoa, BUILD_AREA_1b3_X,BUILD_AREA_1b3_Y);
else if(half == 4)
oa_set_target(&beudaoa, BUILD_AREA_1b4_X,BUILD_AREA_1b4_Y);
}
Strat_Action++;
break;
case 1:
if(beudaoa.state==OA_IN_TARGET){
//trajectory_turnto_xy_behind(&beudatraj, 105, 150);
if(build_area==3){
if(beudabot.color==COLOR_GREEN)
trajectory_a_abs(&beudatraj,-90);
else
trajectory_a_abs(&beudatraj,90);
}
else if(build_area==2 || build_area==1){
trajectory_a_abs(&beudatraj,180);
}
}
Strat_Action++;
break;
case 2:
//here whe sould give the back to the build area x
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, -20, 0);
Strat_Action++;
break;
case 3:
//envoi d'ordres i2c à la pinc pour construire
i2c_send_only(I2C_PINCE_ADDR,I2C_PINCE_CONSTRUCT);
starttime = gettime();
Strat_Action++;
break;
case 4:
if((gettime() > starttime + DELAY_PINCE_CONSTRUCTION)){
if(i2c_send_and_receive(I2C_PINCE_ADDR,I2C_PINCE_DID_YOU_BUILD_SMTG)==0){
//we wait more
if(gettime() > (starttime + 2*DELAY_PINCE_CONSTRUCTION)){
//there is a problem here...
//drop the shit and let's continue the sequ...
//pince is maybe just f****d up ...
/*A CHANGER !!!!!*/
Strat_State = STRAT_EMERGENCY_BEHAVIOUR;
#ifdef DEBUG_STRAT
UART_CPutString("Pince is down... !\r\n");
#endif
setBit(strat_flags,PINCE_IS_SHITTY);
}
}
else if (i2c_send_and_receive(I2C_PINCE_ADDR,I2C_PINCE_DID_YOU_BUILD_SMTG)==2){
//Damn it! there is already a construction here... let's move and try somewhere else!..
Strat_Action = 0;
//if(Strat_State <STRAT_CONSTRUCT_TEMPLE_1_2)
strat_go_next_build_area(); //this function will change strat state to build somewhere else...
current_building_area++; //for next call of strat_go_next_build_area();
#ifdef DEBUG_STRAT
UART_CPutString("Playing against RCVA || Microb ???!\r\n");
#endif
}
else if(i2c_send_and_receive(I2C_PINCE_ADDR,I2C_PINCE_DID_YOU_BUILD_SMTG)==1){
//finish building, let's put the lint if flag I_CAN_BUILD_A_TEMPLE is set
Strat_Action++;
}
}
break;
case 5:
if(strat_flags & I_CAN_BUILD_A_TEMPLE){
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, -20, 0);
}
Strat_Action++;
break;
case 6:
if(strat_flags & I_CAN_BUILD_A_TEMPLE){
trajectory_d_a_rel(&beudatraj, 0, 180);
}
Strat_Action++;
break;
case 7:
if(strat_flags & I_CAN_BUILD_A_TEMPLE){
trajectory_d_a_rel(&beudatraj, 20, 0);
}
Strat_Action++;
break;
case 8:
if(strat_flags & I_CAN_BUILD_A_TEMPLE){
i2c_send_only(I2C_LINTO_ADDR, I2C_LINT_DEPOSIT_DOWN);
starttime = gettime();
}
Strat_Action++;
break;
case 9:
if(strat_flags & I_CAN_BUILD_A_TEMPLE){
if(gettime() > starttime + DELAY_LINT_DOWN){
Strat_State=STRAT_GET_LINT_1;
}
}
else
Strat_State=STRAT_GET_LINT_1;
//get a lint, then elts, to build another one...
break;
}
}
}
