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; }