{ .i = 7, .j = 4, },
{ .i = 6, .j = 4, },
{ .i = 5, .j = 3, },
- { .i = 4, .j = 4, },
+ { .i = 4, .j = 3, },
{ .i = 3, .j = 2, },
{ .i = 2, .j = 2, },
{ .i = 1, .j = 1, },
.path = big_h_lambda_tab,
};
+const struct wp_coord letter_v_tab[] = {
+ { .i = 11, .j = 6, },
+ { .i = 10, .j = 6, },
+ { .i = 9, .j = 5, },
+ { .i = 8, .j = 5, },
+ { .i = 7, .j = 4, },
+ { .i = 6, .j = 4, },
+ { .i = 5, .j = 4, },
+ { .i = 4, .j = 5, },
+ { .i = 3, .j = 5, },
+ { .i = 2, .j = 6, },
+ { .i = 1, .j = 6, },
+ { .i = 1, .j = 5, },
+ { .i = 1, .j = 4, },
+ { .i = 2, .j = 4, },
+ { .i = 3, .j = 3, },
+ { .i = 4, .j = 3, },
+ { .i = 5, .j = 2, },
+ { .i = 6, .j = 2, },
+ { .i = 7, .j = 2, },
+ { .i = 8, .j = 3, },
+ { .i = 9, .j = 3, },
+ { .i = 10, .j = 4, },
+ { .i = 11, .j = 4, },
+ { .i = 11, .j = 5, },
+ { .i = 11, .j = 6, },
+};
+
+const struct circuit letter_v_circuit = {
+ .name = "letter_v",
+ .len = sizeof(letter_v_tab)/sizeof(struct wp_coord),
+ .path = letter_v_tab,
+};
+
+const struct wp_coord sperma_tab[] = {
+ { .i = 11, .j = 6, },
+ { .i = 10, .j = 6, },
+ { .i = 9, .j = 5, },
+ { .i = 8, .j = 5, },
+ { .i = 7, .j = 5, },
+ { .i = 6, .j = 6, },
+ { .i = 5, .j = 5, },
+ { .i = 4, .j = 5, },
+ { .i = 3, .j = 4, },
+ { .i = 2, .j = 4, },
+ { .i = 1, .j = 3, },
+ { .i = 1, .j = 4, },
+ { .i = 1, .j = 5, },
+ { .i = 1, .j = 6, },
+ { .i = 2, .j = 6, },
+ { .i = 3, .j = 5, },
+ { .i = 4, .j = 5, },
+ { .i = 5, .j = 5, },
+ { .i = 6, .j = 6, },
+ { .i = 7, .j = 5, },
+ { .i = 8, .j = 5, },
+ { .i = 9, .j = 5, },
+ { .i = 10, .j = 6, },
+ { .i = 11, .j = 6, },
+};
+
+const struct circuit sperma_circuit = {
+ .name = "sperma",
+ .len = sizeof(sperma_tab)/sizeof(struct wp_coord),
+ .path = sperma_tab,
+};
+
/* list of all possible circuits */
const struct circuit *circuits[] = {
&butterfly_circuit,
&h_lambda_circuit,
&asym_butterfly_circuit,
&big_h_lambda_circuit,
+ &letter_v_circuit,
+ &sperma_circuit,
NULL,
};
return 0;
}
-static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
+static int8_t get_line_num(int8_t i, int8_t j, uint8_t dir)
{
+ uint8_t mod;
+
switch (dir) {
case LINE_UP:
case LINE_DOWN:
+ if ((i & 1) == 0)
+ return -1;
return i/2;
case LINE_R_UP:
case LINE_L_DOWN:
+ mod = i & 3;
+ if ((mod == 0 || mod == 1) && ((j & 1) == 0))
+ return -1;
+ if ((mod == 2 || mod == 3) && ((j & 1) == 1))
+ return -1;
i &= 0xfe;
j -= i/2;
return (5-j)/2;
case LINE_R_DOWN:
case LINE_L_UP:
+ mod = i & 3;
+ if ((mod == 0 || mod == 3) && ((j & 1) == 0))
+ return -1;
+ if ((mod == 1 || mod == 2) && ((j & 1) == 1))
+ return -1;
i &= 0xfe;
j += i/2;
return (11-j)/2;
return 0xFF;
}
+/* return approximative angle of line */
+int16_t linedir2angle(uint8_t dir)
+{
+ switch (dir) {
+ case LINE_UP:
+ return COLOR_A(90);
+ case LINE_DOWN:
+ return COLOR_A(-90);
+ case LINE_R_UP:
+ return COLOR_A(30);
+ case LINE_R_DOWN:
+ return COLOR_A(-30);
+ case LINE_L_UP:
+ return COLOR_A(150);
+ case LINE_L_DOWN:
+ return COLOR_A(-150);
+ default:
+ return 0;
+ }
+}
+
+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)
{
- uint8_t ln;
+ int8_t ln;
ln = get_line_num(i, j, dir);
+ if (ln == -1)
+ return 0;
if (ln == linenum)
return 1;
return 0;
if (wp_get_neigh(i, j, &ni, &nj, dir) < 0)
continue;
- /* is there a corn cob ? */
+ /* is there a corn cob removed for more than 2 secs ? */
if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN &&
- strat_db.wp_table[ni][nj].present &&
- strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK)
+ strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK &&
+ (strat_db.wp_table[ni][nj].present ||
+ strat_db.wp_table[ni][nj].time_removed + 2 > time_get_s()))
n ++;
}
start = end + circuit->len - 1;
}
- DPR("face: %s %d\r\n", circuit->name, faceA);
+ DPR("%s(): %s face=%d\r\n", __FUNCTION__, circuit->name, faceA);
/* check that the point is present in the circuit */
for (curcircuit = start; curcircuit != end; curcircuit += step) {
linenum = get_line_num(prev_i, prev_j, dir);
circuit_wpline[path_len].line_num = linenum;
circuit_wpline[path_len].dir = dir;
+ DPR("%s(): %d %d -> %d %d / len=%d num=%d dir=%d\r\n",
+ __FUNCTION__, prev_i, prev_j, i, j, path_len, linenum, dir);
path_len++;
}
}
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;
/* malus if there is opponent on the path */
if (opp_on_path) {
- DPR("malus for opponent: 1000\r\n");
- score -= 2000;
+ DPR("malus for opponent: %d\r\n", (500 * opp_on_path));
+ score -= (500 * opp_on_path);
}
return score;
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, d;
+ int16_t x, y;
+ int32_t d, prev_d = 0;
int16_t oppx, oppy;
*score = 0x8000; /* -int_max */
/* get opponent coords */
if (get_opponent_xy(&oppx, &oppy) < 0)
oppx = I2C_OPPONENT_NOT_THERE;
+ else
+ DPR("%s() opponent: %d, %d\r\n", __FUNCTION__, oppx, oppy);
/* silent the compiler */
prev_i = 0xff;
prev_j = 0xff;
/* browse all points and calculate the score */
- for (curcircuit = start;
+ for (; /* start at starti,startj */
curcircuit != end;
curcircuit += step, len ++, prev_i = i, prev_j = j) {
i = curcircuit->i;
/* is opponent near the point ? */
ijcoord_to_xycoord(i, j, &x, &y);
if (oppx != I2C_OPPONENT_NOT_THERE) {
- d = distance_between(oppx, oppy, x, y);
- if (d < 600)
- opponent_on_path = 1;
+ d = quad_distance_between(oppx, oppy, x, y);
+ DPR("%s(): opp at %d mm (ij=%d,%d opp=%d,%d pos=%d,%d)\r\n",
+ __FUNCTION__, d, i, j, oppx, oppy, x, y);
+ if (d < (250L*250L) && d < prev_d)
+ opponent_on_path += 3;
+ else if (d < (500L*500L) && d < prev_d)
+ opponent_on_path ++;
+ prev_d = d;
}
/* don't try to look cobs/tomato for first point */
/* 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;
}
}
/* i,j starting position */
-int8_t find_best_circuit(uint8_t i, uint8_t j,
- const struct circuit **selected_circuit,
- int8_t *selected_face)
+static int8_t find_best_circuit(uint8_t i, uint8_t j,
+ const struct circuit **selected_circuit,
+ int8_t *selected_face)
{
const struct circuit **circuit;
int16_t scoreA, scoreB;
if (scoreA > selected_score) {
*selected_circuit = *circuit;
selected_score = scoreA;
- *selected_face = 0;
+ *selected_face = 1;
}
if (scoreB > selected_score) {
*selected_circuit = *circuit;
selected_score = scoreB;
- *selected_face = 1;
+ *selected_face = 0;
}
}
return found;
}
+static void init_all_circuits(void)
+{
+ const struct circuit **circuit;
+ const struct wp_coord *cur;
+ const struct wp_coord *start;
+ const struct wp_coord *end;
+ uint8_t prev_i, prev_j, i, j, dir;
+
+ for (circuit = &circuits[0]; *circuit; circuit++) {
+ start = &(*circuit)->path[0];
+ end = start + (*circuit)->len - 1;
+
+ prev_i = start->i;
+ prev_j = start->j;
+ start ++;
+
+ for (cur = start; cur != end;
+ cur ++, prev_i = i, prev_j = j) {
+
+ i = cur->i;
+ j = cur->j;
+
+ strat_db.wp_table[i][j].on_circuit = 1;
+
+ dir = get_dir(prev_i, prev_j, i, j);
+ if (dir == 0xFF)
+ printf_P("Bad circuit %s %d %d\r\n", (*circuit)->name, i, j);
+ }
+ }
+}
+
+
static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
{
int8_t i;
uint8_t dir, prev_dir;
uint8_t err;
+ strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
+ strat_want_pack = 1;
+
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);
- return END_ERROR;
+ err = END_ERROR;
+ goto fail;
}
if (find_best_circuit(i, j, &selected_circuit, &selected_face) < 0) {
DEBUG(E_USER_STRAT, "%s(): cannot find a good circuit",
__FUNCTION__);
- return END_ERROR;
+ err = END_ERROR;
+ goto fail;
}
len = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
if (len < 0) {
DEBUG(E_USER_STRAT, "%s(): cannot find a path",
__FUNCTION__);
- return END_ERROR;
+ err = END_ERROR;
+ goto fail;
}
dump_circuit_wp(circuit_wpline, len);
prev_linenum = circuit_wpline[0].line_num;
prev_dir = circuit_wpline[0].dir;
+
+ /* fix orientation first */
+ trajectory_a_abs(&mainboard.traj, linedir2angle(prev_dir));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ goto fail;
+
+ strat_want_pack = 0;
+
+ /* do all lines of circuit */
for (idx = 1; idx < len; idx ++) {
retry:
linenum = circuit_wpline[idx].line_num;
dir = circuit_wpline[idx].dir;
- /* XXX basic opponent management */
DEBUG(E_USER_STRAT, "%s(): line %d dir %d -> line %d dir %d",
__FUNCTION__, prev_linenum, prev_dir, linenum, dir);
err = line2line(prev_linenum, prev_dir, linenum, dir,
TRAJ_FLAGS_NO_NEAR);
- if (!TRAJ_SUCCESS(err)) {
- strat_hardstop();
+
+ /* 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;
prev_linenum = linenum;
prev_dir = dir;
}
+ err = END_TRAJ;
+
+ fail:
+ strat_want_pack = 0;
+ return err;
+}
+
+/* list of waypoints when we are not on a circuit */
+const struct xy_point unblock_pts[] = {
+ { .x = 375, .y = 597 }, /* 1,1 */
+ { .x = 2625, .y = 597 }, /* 11,1 */
+ { .x = 1500, .y = 722 }, /* 6,2 */
+ { .x = 375, .y = 1097 }, /* 1,3 */
+ { .x = 375, .y = 1597 }, /* 1,5 */
+ { .x = 2625, .y = 1097 }, /* 11,3 */
+ { .x = 2625, .y = 1597 }, /* 11,5 */
+ { .x = 1500, .y = 1722 }, /* 6,6 */
+};
+
+
+/* try to unblock in any situation */
+uint8_t strat_unblock(void)
+{
+ 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 = 0x7FFF, d;
+ const struct xy_point *pt;
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ strat_want_pack = 1;
+ 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);
+ posx = position_get_x_s16(&mainboard.pos);
+ posy = position_get_y_s16(&mainboard.pos);
+ x = posx;
+ y = posy;
+
+ 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) {
+
+ /* 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(posx, posy, pt->x, COLOR_Y(pt->y));
+ if (d < d_min) {
+ d_min = d;
+ x = pt->x;
+ y = COLOR_Y(pt->y);
+ }
+ }
+ }
+ DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d",
+ __FUNCTION__, x, y);
+
+ for (cpt = 0; cpt < 2; cpt++) {
+
+ /* 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;
+
+ 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 END_TRAJ; // XXX
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ strat_set_speed(old_dspeed, old_aspeed);
+ return END_TRAJ;
}
-void test_strat_avoid(void)
+void strat_avoid_init(void)
{
+ init_all_circuits();
+
#ifdef TEST_STRAT_AVOID
uint8_t i, j;
const struct circuit *selected_circuit;
git.droids-corp.org / aversive.git / blobdiff