{ .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 = 1, .j = 3, },
- { .i = 1, .j = 2, },
- { .i = 1, .j = 1, },
- { .i = 2, .j = 2, },
- { .i = 3, .j = 2, },
- { .i = 4, .j = 3, },
{ .i = 5, .j = 3, },
+ { .i = 4, .j = 3, },
+ { .i = 3, .j = 2, },
+ { .i = 2, .j = 2, },
+ { .i = 1, .j = 1, },
+ { .i = 1, .j = 2, },
+ { .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 = 4, },
{ .i = 6, .j = 4, },
{ .i = 7, .j = 3, },
{ .i = 8, .j = 3, },
{ .i = 11, .j = 6, },
};
-const struct circuit butterfly = {
+const struct circuit butterfly_circuit = {
.name = "butterfly",
.len = sizeof(butterfly_tab)/sizeof(struct wp_coord),
.path = butterfly_tab,
};
-const struct wp_coord small_tab[] = {
+const struct wp_coord losange_tab[] = {
{ .i = 11, .j = 6, },
{ .i = 10, .j = 6, },
{ .i = 9, .j = 5, },
{ .i = 11, .j = 6, },
};
-const struct circuit small = {
- .name = "small",
- .len = sizeof(small)/sizeof(struct wp_coord),
- .path = small_tab,
+const struct circuit losange_circuit = {
+ .name = "losange",
+ .len = sizeof(losange_tab)/sizeof(struct wp_coord),
+ .path = losange_tab,
+};
+
+const struct wp_coord triangle_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 = 7, .j = 3, },
+ { .i = 8, .j = 3, },
+ { .i = 9, .j = 2, },
+ { .i = 10, .j = 2, },
+ { .i = 11, .j = 1, },
+ { .i = 11, .j = 2, },
+ { .i = 11, .j = 3, },
+ { .i = 11, .j = 4, },
+ { .i = 11, .j = 5, },
+ { .i = 11, .j = 6, },
+};
+
+const struct circuit triangle_circuit = {
+ .name = "triangle",
+ .len = sizeof(triangle_tab)/sizeof(struct wp_coord),
+ .path = triangle_tab,
+};
+
+const struct wp_coord answer_d_tab[] = {
+ { .i = 11, .j = 6, },
+ { .i = 11, .j = 5, },
+ { .i = 11, .j = 4, },
+ { .i = 11, .j = 3, },
+ { .i = 11, .j = 2, },
+ { .i = 11, .j = 1, },
+ { .i = 10, .j = 2, },
+ { .i = 9, .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 answer_d_circuit = {
+ .name = "answer_d",
+ .len = sizeof(answer_d_tab)/sizeof(struct wp_coord),
+ .path = answer_d_tab,
+};
+
+const struct wp_coord h_lambda_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 = 3, },
+ { .i = 5, .j = 4, },
+ { .i = 5, .j = 5, },
+ { .i = 5, .j = 6, },
+ { .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 h_lambda_circuit = {
+ .name = "h_lambda",
+ .len = sizeof(h_lambda_tab)/sizeof(struct wp_coord),
+ .path = h_lambda_tab,
+};
+
+const struct wp_coord asym_butterfly_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 = 3, },
+ { .i = 4, .j = 3, },
+ { .i = 3, .j = 2, },
+ { .i = 3, .j = 3, },
+ { .i = 3, .j = 4, },
+ { .i = 3, .j = 5, },
+ { .i = 4, .j = 5, },
+ { .i = 5, .j = 4, },
+ { .i = 6, .j = 4, },
+ { .i = 7, .j = 3, },
+ { .i = 8, .j = 3, },
+ { .i = 9, .j = 2, },
+ { .i = 10, .j = 2, },
+ { .i = 11, .j = 1, },
+ { .i = 11, .j = 2, },
+ { .i = 11, .j = 3, },
+ { .i = 11, .j = 4, },
+ { .i = 11, .j = 5, },
+ { .i = 11, .j = 6, },
+};
+
+const struct circuit asym_butterfly_circuit = {
+ .name = "asym_butterfly",
+ .len = sizeof(asym_butterfly_tab)/sizeof(struct wp_coord),
+ .path = asym_butterfly_tab,
+};
+
+const struct wp_coord big_h_lambda_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 = 1, .j = 3, },
+ { .i = 1, .j = 2, },
+ { .i = 1, .j = 1, },
+ { .i = 2, .j = 2, },
+ { .i = 3, .j = 2, },
+ { .i = 4, .j = 3, },
+ { .i = 5, .j = 3, },
+ { .i = 6, .j = 4, },
+ { .i = 7, .j = 4, },
+ { .i = 8, .j = 5, },
+ { .i = 9, .j = 5, },
+ { .i = 10, .j = 6, },
+ { .i = 11, .j = 6, },
+};
+
+const struct circuit big_h_lambda_circuit = {
+ .name = "big_h_lambda",
+ .len = sizeof(big_h_lambda_tab)/sizeof(struct wp_coord),
+ .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,
};
/* list of all possible circuits */
const struct circuit *circuits[] = {
- &butterfly,
- &small,
+ &butterfly_circuit,
+ &losange_circuit,
+ &triangle_circuit,
+ &answer_d_circuit,
+ &h_lambda_circuit,
+ &asym_butterfly_circuit,
+ &big_h_lambda_circuit,
+ &letter_v_circuit,
NULL,
};
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++;
}
}
uint8_t ni = 0, nj = 0;
uint8_t dir, color, idx;
int8_t step = faceA ? 1 : -1;
- int16_t x, y, d;
+ int16_t x, y, 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;
ijcoord_to_xycoord(i, j, &x, &y);
if (oppx != I2C_OPPONENT_NOT_THERE) {
d = distance_between(oppx, oppy, x, y);
- if (d < 600)
+ 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 < 600 && d < prev_d)
opponent_on_path = 1;
+ prev_d = d;
}
/* don't try to look cobs/tomato for first point */
}
/* 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 test_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;
+
+ 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);
+
x = position_get_x_s16(&mainboard.pos);
y = position_get_y_s16(&mainboard.pos);
prev_linenum = circuit_wpline[0].line_num;
prev_dir = circuit_wpline[0].dir;
+
+ /* 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();
- time_wait_ms(2000);
- goto retry;
- }
+ if (!TRAJ_SUCCESS(err))
+ return err;
prev_linenum = linenum;
prev_dir = dir;
}
- return END_TRAJ; // XXX
+ return END_TRAJ;
+}
+
+/* try to unblock in any situation */
+uint8_t strat_unblock(void)
+{
+ int16_t x, y;
+ uint8_t i, j;
+ uint16_t old_dspeed, old_aspeed;
+ uint8_t err;
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ strat_want_pack = 1;
+ strat_get_speed(&old_dspeed, &old_aspeed);
+
+ strat_hardstop();
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
+ x = position_get_x_s16(&mainboard.pos);
+ y = position_get_y_s16(&mainboard.pos);
+ if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) {
+ /* aie... go to center... but it's really a bad
+ * idea */
+ x = CENTER_X;
+ y = CENTER_Y;
+ }
+
+ /* 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;
+
+ if (!TRAJ_SUCCESS(err))
+ return err;
+
+ strat_set_speed(old_dspeed, old_aspeed);
+ strat_want_pack = 0;
+ return END_TRAJ;
}
void test_strat_avoid(void)
{
+ test_all_circuits();
+
#ifdef TEST_STRAT_AVOID
uint8_t i, j;
const struct circuit *selected_circuit;
git.droids-corp.org / aversive.git / blobdiff