From: zer0 Date: Fri, 7 May 2010 23:01:13 +0000 (+0200) Subject: lots of work in opponent avoidance and strats X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=cc67fe587de07a329525c8f5c8ecfd1fabbf83b8 lots of work in opponent avoidance and strats --- diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index 8a2de56..2be7651 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -135,15 +135,15 @@ #define ACC_ANGLE 16. /* default speeds */ -#define SPEED_DIST_FAST 2500. -#define SPEED_ANGLE_FAST 2000. -#define SPEED_DIST_SLOW 1000. -#define SPEED_ANGLE_SLOW 1000. -#define SPEED_DIST_VERY_SLOW 400. -#define SPEED_ANGLE_VERY_SLOW 400. +#define SPEED_DIST_FAST 1500. +#define SPEED_ANGLE_FAST 1000. +#define SPEED_DIST_SLOW 500. +#define SPEED_ANGLE_SLOW 500. +#define SPEED_DIST_VERY_SLOW 200. +#define SPEED_ANGLE_VERY_SLOW 200. -#define SPEED_CLITOID_SLOW 100. ///250. -#define SPEED_CLITOID_FAST 100. ///500. +#define SPEED_CLITOID_SLOW 250. +#define SPEED_CLITOID_FAST 500. /* strat infos structures */ @@ -157,6 +157,7 @@ struct strat_conf { extern struct strat_conf strat_conf; extern volatile uint8_t strat_lpack60; extern volatile uint8_t strat_rpack60; +extern volatile uint8_t strat_want_pack; /* in strat.c */ void strat_conf_dump(const char *caller); diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 4f497ea..5211c4f 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -89,7 +89,7 @@ const struct wp_coord butterfly_tab[] = { { .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, }, @@ -281,6 +281,40 @@ const struct circuit big_h_lambda_circuit = { .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, @@ -290,6 +324,7 @@ const struct circuit *circuits[] = { &h_lambda_circuit, &asym_butterfly_circuit, &big_h_lambda_circuit, + &letter_v_circuit, NULL, }; @@ -516,7 +551,7 @@ static int8_t get_path(const struct circuit *circuit, 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) { @@ -545,6 +580,8 @@ static int8_t get_path(const struct circuit *circuit, 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++; } } @@ -660,7 +697,7 @@ static int8_t evaluate_one_face(const struct circuit *circuit, 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 */ @@ -690,13 +727,15 @@ static int8_t evaluate_one_face(const struct circuit *circuit, /* 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; @@ -706,8 +745,11 @@ static int8_t evaluate_one_face(const struct circuit *circuit, 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 */ @@ -785,9 +827,9 @@ static int8_t evaluate_one_circuit(const struct circuit *circuit, } /* 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; @@ -805,12 +847,12 @@ int8_t find_best_circuit(uint8_t i, uint8_t j, 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; } } @@ -823,6 +865,36 @@ int8_t find_best_circuit(uint8_t i, uint8_t j, 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; @@ -849,6 +921,8 @@ uint8_t strat_harvest_circuit(void) 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); @@ -875,36 +949,70 @@ uint8_t strat_harvest_circuit(void) 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; diff --git a/projects/microb2010/mainboard/strat_avoid.h b/projects/microb2010/mainboard/strat_avoid.h index 493aca4..56de996 100644 --- a/projects/microb2010/mainboard/strat_avoid.h +++ b/projects/microb2010/mainboard/strat_avoid.h @@ -39,6 +39,9 @@ int8_t browse_circuits(uint8_t i, uint8_t j, const struct wp_coord **selected_circuit, int8_t *selected_face); +/* try to unblock in any situation */ +uint8_t strat_unblock(void); + /* harvest on the best circuit */ uint8_t strat_harvest_circuit(void); diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index 6075271..1a89f4d 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -235,7 +235,7 @@ void strat_limit_speed_disable(void) 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; @@ -326,7 +326,6 @@ void strat_start(void) /* 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; @@ -335,8 +334,7 @@ uint8_t strat_obstacle(void) 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 */ @@ -344,28 +342,9 @@ uint8_t strat_obstacle(void) 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()) diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index 74dd8d1..c5f0523 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -315,7 +315,7 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1, } /* hard turn, 120 deg */ else { - radius = 120;//75; + radius = 75; if (diff_a_deg > 0) beta_deg = 0; else @@ -332,7 +332,6 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1, 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, @@ -350,8 +349,10 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2, 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 */ @@ -360,8 +361,13 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2, 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", @@ -379,7 +385,7 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2, 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; diff --git a/projects/microb2010/mainboard/strat_db.c b/projects/microb2010/mainboard/strat_db.c index d6af32e..9fe4329 100644 --- a/projects/microb2010/mainboard/strat_db.c +++ b/projects/microb2010/mainboard/strat_db.c @@ -149,7 +149,10 @@ int8_t ijcoord_to_xycoord(uint8_t i, uint8_t j, int16_t *x, int16_t *y) if (i >= WAYPOINTS_NBX && j >= WAYPOINTS_NBY) return -1; *x = (OFFSET_CORN_X + i*STEP_CORN_X); - *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y); + if (i&1) + *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y + STEP_CORN_Y/2); + else + *y = COLOR_Y(OFFSET_CORN_Y + j*STEP_CORN_Y); return 0; }