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)); }
/* * get last 2 columns * must be called after the first temple building */ uint8_t strat_static_columns_pass2(void) { uint16_t old_spdd, old_spda; uint8_t side, err, next_mode; DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); strat_get_speed(&old_spdd, &old_spda); if (get_color() == I2C_COLOR_RED) side = I2C_RIGHT_SIDE; else side = I2C_LEFT_SIDE; if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2) next_mode = I2C_MECHBOARD_MODE_STORE; else next_mode = I2C_MECHBOARD_MODE_HARVEST; switch (strat_infos.s_cols.configuration) { /* configuration 1: 4 cols on line 0 */ case 1: if (strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE) { /* go on line 2 */ strat_set_speed(2000, 700); trajectory_d_a_rel(&mainboard.traj, -450, COLOR_A(35)); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) ERROUT(err); i2c_mechboard_mode_prepare_pickup_next(side, next_mode); strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); trajectory_goto_forward_xy_abs(&mainboard.traj, LINE2_X, COLOR_Y(400)); err = WAIT_COND_OR_TRAJ_END(get_column_count() == 2, TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) ERROUT(err); } else { /* go on line 1 */ strat_set_speed(2000, 700); trajectory_d_a_rel(&mainboard.traj, -650, COLOR_A(55)); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) ERROUT(err); i2c_mechboard_mode_prepare_pickup_next(side, next_mode); strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); err = goto_and_avoid_forward(LINE1_X, COLOR_Y(400), TRAJ_FLAGS_NO_NEAR, TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) ERROUT(err); } ERROUT(END_TRAJ); break; /* configuration 2: 2 cols on line 0, all other colums are on line 1 */ case 2: /* go on line 1 */ strat_set_speed(2000, 700); trajectory_d_a_rel(&mainboard.traj, -410, COLOR_A(-20)); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) ERROUT(err); i2c_mechboard_mode_prepare_pickup_next(side, next_mode); strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); err = goto_and_avoid_forward(COL10_X, COLOR_Y(400), TRAJ_FLAGS_NO_NEAR, TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) ERROUT(err); ERROUT(END_TRAJ); break; /* configuration 3: 2 cols on line 0, all other colums are on line 2 */ case 3: /* go on line 2 */ strat_set_speed(2000, 700); trajectory_d_a_rel(&mainboard.traj, -150, COLOR_A(-30)); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) ERROUT(err); i2c_mechboard_mode_prepare_pickup_next(side, next_mode); strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); trajectory_goto_forward_xy_abs(&mainboard.traj, LINE2_X, COLOR_Y(400)); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) ERROUT(err); ERROUT(END_TRAJ); break; /* configuration 4: 2 cols on line 0, 2 on line 1, 2 on line 2 */ case 4: /* go on line 1 */ strat_set_speed(600, 2000); trajectory_d_a_rel(&mainboard.traj, -BIG_DIST, COLOR_A(-135)); err = WAIT_COND_OR_TRAJ_END(y_is_more_than(900), TRAJ_FLAGS_STD); if (TRAJ_SUCCESS(err)) /* we should not reach end */ ERROUT(END_ERROR); else if (err) ERROUT(err); DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); i2c_mechboard_mode_prepare_pickup_next(side, next_mode); strat_set_speed(2000, 2000); trajectory_d_rel(&mainboard.traj, -BIG_DIST); 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); DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); trajectory_d_a_rel(&mainboard.traj, -600, COLOR_A(40)); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) ERROUT(err); DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__); strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST); err = goto_and_avoid_forward(LINE1_X, COLOR_Y(400), TRAJ_FLAGS_NO_NEAR, TRAJ_FLAGS_NO_NEAR); ERROUT(END_TRAJ); break; default: break; } /* should not reach this point */ ERROUT(END_ERROR); end: strat_set_speed(old_spdd, old_spda); return err; }
int strat_goto_avoid(int x, int y, int flags) { int i, len, ret; int retry_count; int opp_x, opp_y; poly_t *pol_opp; point_t *p; oa_init(); /* The robot will try 3 times before giving up. */ for(retry_count=0;retry_count<3;retry_count++) { /* Creates one polygon for each opponent robot. */ for(i=0;i<robot.beacon.nb_beacon;i++) { pol_opp = oa_new_poly(4); strat_da_rel_to_xy_abs(robot.beacon.beacon[i].direction, robot.beacon.beacon[i].distance*10, &opp_x, &opp_y); NOTICE(0, "Op is at %d;%d", opp_x, opp_y); create_opp_polygon(pol_opp, 800, 0); /* Checks if the arrival point is in an opponent. */ if(is_point_in_poly(pol_opp, x, y)) { WARNING(0, "Destination point is in opponent."); return END_ERROR; } } /* Sets starting and ending point of the path. */ oa_start_end_points(position_get_x_s16(&robot.pos), position_get_x_s16(&robot.pos), x, y); /* Computes the path */ len = oa_process(); /* Checks if a path was found. */ if(len == 0) { WARNING(0, "Cannot find a suitable path."); return END_ERROR; } p = oa_get_path(); /* For all the points in the path. */ for(i=0;i<len;i++) { /* Goes to the point. */ trajectory_goto_forward_xy_abs(&robot.traj, p->x, p->y); /* Waits for the completion of the trajectory. */ ret = wait_traj_end(flags); /* If we were blocked or met an obstacle, we will retry. */ if(ret == END_BLOCKING || ret == END_OBSTACLE) { WARNING(0, "Retry"); break; // we retry once more } else if(!TRAJ_SUCCESS(ret)) { /* If it was an other error, we simply abort and return it. */ WARNING(0, "Unknown error code : %d", ret); return ret; } /* Increments pointer to load next point. */ p++; } /* If we reached last point, no need to retry. */ if(ret == END_TRAJ) return END_TRAJ; } /* If we reach here, it means 3 try were not enough. */ return END_ERROR; }