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));
}
int test_traj_end(int why)
{
if((why & END_TRAJ) && trajectory_finished(&robot.traj))
return END_TRAJ;
if (why & END_NEAR) {
int16_t d_near = 100; /* mm */
/* XXX Change distance depending on speed. */
if (trajectory_in_window(&robot.traj, d_near, RAD(5.0)))
return END_NEAR;
}
if((why & END_OBSTACLE)) {
int i;
for(i=0;i<robot.beacon.nb_beacon;i++) {
if(robot.beacon.beacon[i].distance > 80) { /*cm*/
if(robot.distance_qr.previous_var > 0) {
/* Going forward. */
if(robot.beacon.beacon[i].direction > -30 && robot.beacon.beacon[i].direction < 30) {
trajectory_stop(&robot.traj);
while(robot.distance_qr.previous_var > 0);
trajectory_hardstop(&robot.traj);
bd_reset(&robot.distance_bd);
return END_OBSTACLE;
}
} else if(robot.distance_qr.previous_var < 0) {
/* Going backward */
if(robot.beacon.beacon[i].direction < -30 || robot.beacon.beacon[i].direction > 30) {
trajectory_stop(&robot.traj);
while(robot.distance_qr.previous_var < 0);
trajectory_hardstop(&robot.traj);
bd_reset(&robot.distance_bd);
return END_OBSTACLE;
}
}
}
}
}
if((why & END_BLOCKING) && bd_get(&robot.distance_bd)) {
trajectory_hardstop(&robot.traj);
bd_reset(&robot.distance_bd);
bd_reset(&robot.angle_bd);
return END_BLOCKING;
}
if((why & END_BLOCKING) && bd_get(&robot.angle_bd)) {
trajectory_hardstop(&robot.traj);
bd_reset(&robot.distance_bd);
bd_reset(&robot.angle_bd);
return END_BLOCKING;
}
if((why & END_TIMER) && strat_get_time() >= MATCH_TIME) {
trajectory_hardstop(&robot.traj);
return END_TIMER;
}
return 0;
}
