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)); }
/* * must be called from start area. * get 4 static columns and build a temple on the disc */ uint8_t strat_static_columns(uint8_t configuration) { uint8_t err; uint8_t col1_present = 0, col4_present = 0; uint16_t old_spdd, old_spda; DEBUG(E_USER_STRAT, "%s(%d)", __FUNCTION__, configuration); strat_get_speed(&old_spdd, &old_spda); /* calibrate scanner */ i2c_sensorboard_scanner_calib(); i2c_mechboard_mode_harvest(); /* go straight. total distance is less than 5 meters */ strat_set_speed(1000, 1000); trajectory_d_rel(&mainboard.traj, BIG_DIST); /* when y > 50, break */ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(500), TRAJ_FLAGS_STD); if (TRAJ_SUCCESS(err)) /* we should not reach end */ ERROUT(END_ERROR); else if (err) ERROUT(err); /* turn to 90° abs while going forward */ DEBUG(E_USER_STRAT, "turn now"); strat_set_speed(1000, 350); trajectory_only_a_abs(&mainboard.traj, COLOR_A(90)); /* when y > 100, check the presence of column 4 */ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1000), TRAJ_FLAGS_STD); if (TRAJ_SUCCESS(err)) /* we should not reach end */ ERROUT(END_ERROR); else if (err) ERROUT(err); if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT)) col4_present = 1; if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT)) col4_present = 1; /* when y > 120, check the presence of column 1 */ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1200), TRAJ_FLAGS_STD); if (TRAJ_SUCCESS(err)) /* we should not reach end */ ERROUT(END_ERROR); else if (err) ERROUT(err); if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT)) col1_present = 1; if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT)) col1_present = 1; /* when y > 130, break */ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1300), TRAJ_FLAGS_STD); if (TRAJ_SUCCESS(err)) /* we should not reach end */ ERROUT(END_ERROR); else if (err) ERROUT(err); strat_infos.s_cols.flags |= STATIC_COL_LINE0_DONE; DEBUG(E_USER_STRAT, "col4=%d col1=%d", col4_present, col1_present); DEBUG(E_USER_STRAT, "have %d cols", get_column_count()); if (configuration == 0) { if (get_column_count() > 2) { configuration = 1; if (col4_present || col1_present) { strat_infos.s_cols.flags |= STATIC_COL_LINE2_DONE; } else { strat_infos.s_cols.flags |= STATIC_COL_LINE1_DONE; } } /* only 2 colums on the first line */ else { /* all other colums are on line 1 */ if (col4_present && col1_present) { configuration = 2; strat_infos.s_cols.flags |= STATIC_COL_LINE2_DONE; } /* only 2 columns on line 1, so there are also * 2 on line 2 */ else if (col4_present || col1_present) { configuration = 4; strat_infos.s_cols.flags |= STATIC_COL_LINE2_DONE; } /* all other columns are on line 2 */ else { configuration = 3; strat_infos.s_cols.flags |= STATIC_COL_LINE1_DONE; } } } strat_infos.s_cols.configuration = configuration; DEBUG(E_USER_STRAT, "use configuration %d", configuration); if (configuration == 1) { /* we already got 4 columns, go to the disc directly */ strat_set_speed(1500, 900); trajectory_only_a_abs(&mainboard.traj, COLOR_A(0)); err = WAIT_COND_OR_TRAJ_END(x_is_more_than(1100), TRAJ_FLAGS_STD); if (TRAJ_SUCCESS(err)) /* we should not reach end */ ERROUT(END_ERROR); else if (err) ERROUT(err); } else if (configuration == 2 /* go from line 0 to line 1 */) { strat_set_speed(800, 1000); /* relative is needed here */ trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180)); err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1300), TRAJ_FLAGS_STD); if (TRAJ_SUCCESS(err)) /* we should not reach end */ ERROUT(END_ERROR); else if (err) ERROUT(err); strat_set_speed(1000, 600); err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1100), TRAJ_FLAGS_STD); if (TRAJ_SUCCESS(err)) /* we should not reach end */ ERROUT(END_ERROR); else if (err) ERROUT(err); } else if (configuration == 3 /* go from line 0 to line 2 and there is 4 columns on line 2*/) { strat_set_speed(1000, 600); /* relative is needed here */ trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180)); err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1110), TRAJ_FLAGS_STD); if (TRAJ_SUCCESS(err)) /* we should not reach end */ ERROUT(END_ERROR); else if (err) ERROUT(err); } else if (configuration == 4 /* go from line 0 to line 2 and there is 2 columns on line 2 */) { strat_set_speed(1000, 600); /* relative is needed here */ trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180)); err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(600), TRAJ_FLAGS_STD); if (TRAJ_SUCCESS(err)) /* we should not reach end */ ERROUT(END_ERROR); else if (err) ERROUT(err); } else { trajectory_stop(&mainboard.traj); } ERROUT(END_TRAJ); end: strat_set_speed(old_spdd, old_spda); return err; }