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