{ .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,
+};
+
/* list of all possible circuits */
const struct circuit *circuits[] = {
&butterfly_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;
strat_limit_speed_enabled = 0;
}
-/* called periodically */
+/* called periodically (note: disabled in 2010) */
void strat_limit_speed(void)
{
uint16_t lim_d = 0, lim_a = 0;
/* return true if we have to brake due to an obstacle */
uint8_t strat_obstacle(void)
{
-#if 0
int16_t x_rel, y_rel;
int16_t opp_x, opp_y, opp_d, opp_a;
return 0;
/* no opponent detected */
- if (get_opponent_xyda(&opp_x, &opp_y,
- &opp_d, &opp_a))
+ if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a))
return 0;
/* save obstacle position */
opponent_obstacle.y = opp_y;
opponent_obstacle.d = opp_d;
opponent_obstacle.a = opp_a;
-#else /* belgium cup only */
- int16_t x_rel, y_rel;
- int16_t opp_d, opp_a;
- double opp_x, opp_y;
-
-#ifdef HOST_VERSION
- return 0;
- if (time_get_s() >= 12 && time_get_s() <= 30)
- return 1;
-#endif
- return 0; /* XXX disabled */
-
- if (!sensor_get(S_RCOB_WHITE))
- return 0;
- opp_a = 0;
- opp_d = 300;
-
- rel_da_to_abs_xy(opp_d, RAD(opp_a), &opp_x, &opp_y);
if (!is_in_area(opp_x, opp_y, 250))
return 0;
-#endif
/* sensor are temporarily disabled */
if (sensor_obstacle_is_disabled())
}
/* hard turn, 120 deg */
else {
- radius = 120;//75;
+ radius = 75;
if (diff_a_deg > 0)
beta_deg = 0;
else
strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
}
- /* XXX check return value !! */
ret = trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
line1_a_deg, 150., diff_a_deg, beta_deg,
radius, xy_norm(l1.p1.x, l1.p1.y,
reprocess:
ret = strat_calc_clitoid(num1, dir1, num2, dir2, &pack_spickles);
- if (ret < 0)
+ if (ret < 0) {
DEBUG(E_USER_STRAT, "clitoid failed");
+ return END_ERROR;
+ }
/* XXX what to do if cobboard is stucked */
num2, dir2),
flags);
+ /* error during traj, or traj finished */
+ if (err != 0)
+ return err;
+
/* the speed has to change */
- if (trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE)
+ if (err == 0 &&
+ trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE)
goto reprocess;
DEBUG(E_USER_STRAT, "clitoid started err=%d pack_spickles=%d",
err = wait_traj_end(flags);
}
- DEBUG(E_USER_STRAT, "clitoid finished");
+ DEBUG(E_USER_STRAT, "clitoid finished, err=%d", err);
strat_rpack60 = 0;
strat_lpack60 = 0;
git.droids-corp.org / aversive.git / commitdiff