case LINE_R_UP:
return COLOR_A(30);
case LINE_R_DOWN:
- return COLOR_A(-90);
+ return COLOR_A(-30);
case LINE_L_UP:
return COLOR_A(150);
case LINE_L_DOWN:
}
}
+int16_t get_nearest_dir_angle(int16_t a)
+{
+ uint8_t dir, min_dir = 0;
+ int16_t min_diff = 0x7FFF, diff;
+
+ for (dir = LINE_UP; dir <= LINE_R_UP; dir++) {
+ diff = abs(linedir2angle(dir) - a);
+ if (diff > 360)
+ diff -= 360;
+ if (diff > 360)
+ diff -= 360;
+ if (diff < min_diff) {
+ min_diff = diff;
+ min_dir = dir;
+ }
+ }
+ return linedir2angle(min_dir);
+}
+
/* return true if a waypoint belongs to a line */
uint8_t wp_belongs_to_line(uint8_t i, uint8_t j, uint8_t linenum, uint8_t dir)
{
static int16_t get_score(uint32_t wcorn_retrieved,
uint32_t ucorn_retrieved,
uint16_t tomato_retrieved,
+ uint16_t utomato_retrieved,
uint8_t len, uint8_t opp_on_path)
{
int16_t score = 0;
uint32_t wcorn_retrieved = 0; /* bit mask */
uint32_t ucorn_retrieved = 0; /* bit mask */
uint16_t tomato_retrieved = 0; /* bit mask */
+ uint16_t utomato_retrieved = 0; /* bit mask */
uint8_t opponent_on_path = 0;
uint8_t len = 0, found = 0;
uint8_t i, j, prev_i, prev_j;
uint8_t ni = 0, nj = 0;
- uint8_t dir, color, idx;
+ uint8_t dir, color, idx, visited;
int8_t step = faceA ? 1 : -1;
int16_t x, y;
int32_t d, prev_d = 0;
/* is there a tomato ? */
if (strat_db.wp_table[i][j].type == WP_TYPE_TOMATO &&
strat_db.wp_table[i][j].present) {
- DPR(" TOMATO\n");
- tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
+ if (strat_db.wp_table[i][j].opp_visited) {
+ DPR(" TOMATO (opp visited)\n");
+ utomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
+ }
+ else {
+ DPR(" TOMATO\n");
+ tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx);
+ }
}
/* behind left */
if (wp_get_neigh(i, j, &ni, &nj, (dir + 2) % 6) == 0) {
color = get_corn_type(ni, nj);
idx = strat_db.wp_table[ni][nj].corn.idx;
- if (color == I2C_COB_WHITE) {
+ visited = strat_db.wp_table[ni][nj].opp_visited;
+ if (color == I2C_COB_WHITE && !visited) {
DPR(" LEFT WCORN (%d)\n", idx);
wcorn_retrieved |= (1UL << idx);
}
+ else if (color == I2C_COB_WHITE && visited) {
+ DPR(" LEFT CORN visited (%d)\n", idx);
+ ucorn_retrieved |= (1UL << idx);
+ }
else if (color == I2C_COB_UNKNOWN) {
DPR(" LEFT UCORN (%d)\n", idx);
ucorn_retrieved |= (1UL << idx);
if (wp_get_neigh(i, j, &ni, &nj, (dir + 4) % 6) == 0) {
color = get_corn_type(ni, nj);
idx = strat_db.wp_table[ni][nj].corn.idx;
- if (color == I2C_COB_WHITE) {
+ visited = strat_db.wp_table[ni][nj].opp_visited;
+ if (color == I2C_COB_WHITE && !visited) {
DPR(" RIGHT WCORN (%d)\n", idx);
wcorn_retrieved |= (1UL << idx);
}
+ else if (color == I2C_COB_WHITE && visited) {
+ DPR(" RIGHT CORN visited (%d)\n", idx);
+ ucorn_retrieved |= (1UL << idx);
+ }
else if (color == I2C_COB_UNKNOWN) {
DPR(" RIGHT UCORN (%d)\n", idx);
ucorn_retrieved |= (1UL << idx);
/* write score and exit */
*score = get_score(wcorn_retrieved, ucorn_retrieved,
- tomato_retrieved, len, opponent_on_path);
+ tomato_retrieved, utomato_retrieved,
+ len, opponent_on_path);
return 0;
}
strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
strat_want_pack = 1;
- printf("PACK PACK\n");
-
x = position_get_x_s16(&mainboard.pos);
y = position_get_y_s16(&mainboard.pos);
- if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
+ if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) {
DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d",
__FUNCTION__, x, y);
err = END_ERROR;
goto fail;
strat_want_pack = 0;
- printf("UNPACK UNPACK\n");
/* do all lines of circuit */
for (idx = 1; idx < len; idx ++) {
+ retry:
linenum = circuit_wpline[idx].line_num;
dir = circuit_wpline[idx].dir;
__FUNCTION__, prev_linenum, prev_dir, linenum, dir);
err = line2line(prev_linenum, prev_dir, linenum, dir,
TRAJ_FLAGS_NO_NEAR);
+
+ /* in some cases it is better to wait that obstacle is
+ * gone before starting to avoid it */
+ if (err == END_OBSTACLE &&
+ strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE &&
+ time_get_s() > strat_conf.prev_wait_obstacle + 5) {
+ strat_conf.prev_wait_obstacle = time_get_s();
+ time_wait_ms(2000);
+ goto retry;
+ }
if (!TRAJ_SUCCESS(err))
goto fail;
/* try to unblock in any situation */
uint8_t strat_unblock(void)
{
- int16_t x, y;
- uint8_t i, j, k;
+ int16_t x, y, posx, posy, posa;
+ uint8_t i, j, k, cpt;
uint16_t old_dspeed, old_aspeed;
uint8_t err;
- uint16_t d_min = 0xFFFF, d;
+ uint16_t d_min = 0x7FFF, d;
const struct xy_point *pt;
DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
strat_get_speed(&old_dspeed, &old_aspeed);
strat_hardstop();
+ posa = position_get_a_deg_s16(&mainboard.pos);
+
strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
- x = position_get_x_s16(&mainboard.pos);
- y = position_get_y_s16(&mainboard.pos);
+ posx = position_get_x_s16(&mainboard.pos);
+ posy = position_get_y_s16(&mainboard.pos);
+ x = posx;
+ y = posy;
- if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0)
+ if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0)
x = -1;
else if (strat_db.wp_table[i][j].on_circuit == 0)
x = -1;
/* find the nearest unblock point */
if (x == -1) {
- /* position may have been modified */
- x = position_get_x_s16(&mainboard.pos);
- y = position_get_y_s16(&mainboard.pos);
/* browse all points and find the nearest */
for (k = 0; k < sizeof(unblock_pts)/sizeof(*unblock_pts); k++) {
pt = &unblock_pts[k];
- d = distance_between(x, y, pt->x, COLOR_Y(pt->y));
+ d = distance_between(posx, posy, pt->x, COLOR_Y(pt->y));
if (d < d_min) {
d_min = d;
x = pt->x;
}
}
}
+ DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d",
+ __FUNCTION__, x, y);
+
+ for (cpt = 0; cpt < 2; cpt++) {
- /* XXX if opponent is too close, go back, or wait ? */
+ /* go to nearest waypoint */
+ trajectory_goto_xy_abs(&mainboard.traj, x, y);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (err == END_TIMER)
+ return err;
- /* go to nearest waypoint */
- trajectory_goto_xy_abs(&mainboard.traj, x, y);
+ if (TRAJ_SUCCESS(err))
+ break;
+
+ if (cpt == 1)
+ break;
+
+ /* aie... do a S */
+ trajectory_d_a_rel(&mainboard.traj, 100, 20);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ trajectory_d_a_rel(&mainboard.traj, 100, -20);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ trajectory_d_a_rel(&mainboard.traj, -100, -20);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ trajectory_d_a_rel(&mainboard.traj, -100, 20);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ }
+
+ trajectory_a_abs(&mainboard.traj, get_nearest_dir_angle(posa));
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
if (err == END_TIMER)
return err;
return err;
strat_set_speed(old_dspeed, old_aspeed);
- strat_want_pack = 0;
return END_TRAJ;
}
git.droids-corp.org / aversive.git / blobdiff