X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_avoid.c;h=61151ff4fd1936bf9fdbe86af7e678ebe4e05dae;hp=ff5dac358e6c4faa2dae367727bc2ef79bb0dd7c;hb=878d0fc33d940d5d02fc65dfd0e56798c97de18d;hpb=5894a1a70f98504fee52e58f45f181aa684d99bf diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index ff5dac3..61151ff 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -67,57 +67,229 @@ #include "sensor.h" #include "actuator.h" +//#define VERBOSE -const struct wp_coord circuit1[] = { - { .i = 11, .j = 6, .end = 0, }, - { .i = 10, .j = 6, .end = 0, }, - { .i = 9, .j = 5, .end = 0, }, - { .i = 8, .j = 5, .end = 0, }, - { .i = 7, .j = 4, .end = 0, }, - { .i = 6, .j = 4, .end = 0, }, - { .i = 5, .j = 4, .end = 0, }, - { .i = 4, .j = 5, .end = 0, }, - { .i = 3, .j = 5, .end = 0, }, - { .i = 2, .j = 6, .end = 0, }, - { .i = 1, .j = 6, .end = 0, }, - { .i = 1, .j = 5, .end = 0, }, - { .i = 1, .j = 4, .end = 0, }, - { .i = 1, .j = 3, .end = 0, }, - { .i = 1, .j = 2, .end = 0, }, - { .i = 1, .j = 1, .end = 0, }, - { .i = 2, .j = 2, .end = 0, }, - { .i = 3, .j = 2, .end = 0, }, - { .i = 4, .j = 3, .end = 0, }, - { .i = 5, .j = 3, .end = 0, }, - { .i = 6, .j = 4, .end = 0, }, - { .i = 7, .j = 3, .end = 0, }, - { .i = 8, .j = 3, .end = 0, }, - { .i = 9, .j = 2, .end = 0, }, - { .i = 10, .j = 2, .end = 0, }, - { .i = 11, .j = 1, .end = 0, }, - { .i = 11, .j = 2, .end = 0, }, - { .i = 11, .j = 3, .end = 0, }, - { .i = 11, .j = 4, .end = 0, }, - { .i = 11, .j = 5, .end = 0, }, - { .i = 11, .j = 6, .end = 1, }, +#ifdef VERBOSE +#define DPR(fmt, ...) printf_P(PSTR(fmt), ##__VA_ARGS__) +#else +#define DPR(args...) do {} while (0) +#endif + +struct circuit { + const char *name; + uint8_t len; + const struct wp_coord *path; +}; + +const struct wp_coord 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 = 4, }, + { .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 = 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 butterfly_circuit = { + .name = "butterfly", + .len = sizeof(butterfly_tab)/sizeof(struct wp_coord), + .path = butterfly_tab, +}; + +const struct wp_coord losange_tab[] = { + { .i = 11, .j = 6, }, + { .i = 10, .j = 6, }, + { .i = 9, .j = 5, }, + { .i = 9, .j = 4, }, + { .i = 9, .j = 3, }, + { .i = 10, .j = 4, }, + { .i = 11, .j = 4, }, + { .i = 11, .j = 5, }, + { .i = 11, .j = 6, }, +}; + +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 circuit2[] = { - { .i = 11, .j = 6, .end = 0, }, - { .i = 10, .j = 6, .end = 0, }, - { .i = 9, .j = 5, .end = 0, }, - { .i = 9, .j = 4, .end = 0, }, - { .i = 9, .j = 3, .end = 0, }, - { .i = 10, .j = 4, .end = 0, }, - { .i = 11, .j = 4, .end = 0, }, - { .i = 11, .j = 5, .end = 0, }, - { .i = 11, .j = 6, .end = 1, }, +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, }; /* list of all possible circuits */ -const struct wp_coord *circuits[] = { - circuit1, - circuit2, +const struct circuit *circuits[] = { + &butterfly_circuit, + &losange_circuit, + &triangle_circuit, + &answer_d_circuit, + &h_lambda_circuit, + &asym_butterfly_circuit, + &big_h_lambda_circuit, NULL, }; @@ -130,6 +302,11 @@ static inline uint8_t opposite_position(uint8_t pos) return pos; } +#ifdef HOST_VERSION +//#define TEST_STRAT_AVOID +#endif + +#ifdef TEST_STRAT_AVOID static uint8_t cc; uint8_t xget_cob_count(void) { @@ -147,35 +324,68 @@ uint8_t xtime_get_s(void) { return ts; } +#else +#define xget_cob_count() get_cob_count() +#define xget_ball_count() get_ball_count() +#define xtime_get_s() time_get_s() +#endif +/* return true if turn is at 60 deg */ +uint8_t is_60deg(uint8_t dir1, uint8_t dir2) +{ + int8_t turn; + + turn = dir2-dir1; + if (turn < 0) + turn += 6; + if (turn == 1) + return 1; + if (turn == 5) + return 1; + return 0; +} + +/* return true if turn is at 60 deg */ +uint8_t is_120deg(uint8_t dir1, uint8_t dir2) +{ + int8_t turn; + + turn = dir2-dir1; + if (turn < 0) + turn += 6; + if (turn == 2) + return 1; + if (turn == 4) + return 1; + return 0; +} -/* get the neighbour of the point at specified position, return -1 if +/* get the neighbour of the point at specified dir, return -1 if * there is no neighbor */ -static int8_t get_neigh(uint8_t i, uint8_t j, - uint8_t *ni, uint8_t *nj, - uint8_t position) +int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj, + uint8_t dir) { - switch (position) { + switch (dir) { case LINE_UP: j++; break; case LINE_R_UP: - if (!(i & 1)) j++; + if ((i & 1)) j++; i++; break; case LINE_R_DOWN: - if (i & 1) j--; + if (!(i & 1)) j--; i++; break; case LINE_DOWN: j--; break; case LINE_L_DOWN: - if (i & 1) j--; + if (!(i & 1)) j--; i--; break; case LINE_L_UP: - if (!(i & 1)) j++; + if ((i & 1)) j++; i--; break; default: @@ -248,49 +458,84 @@ static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j, return 0xFF; } +/* 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; + ln = get_line_num(i, j, dir); + if (ln == linenum) + return 1; + return 0; +} -int8_t get_path(const struct wp_coord *circuit, - uint8_t starti, uint8_t startj, uint8_t faceA, - struct wp_line *circuit_wpline) +/* count the number of non-black corns which are neighbors of + * specified cob */ +uint8_t corn_count_neigh(uint8_t i, uint8_t j) +{ + uint8_t dir, n = 0; + uint8_t ni, nj; + + for (dir = LINE_UP; dir <= LINE_R_UP; dir++) { + if (wp_get_neigh(i, j, &ni, &nj, dir) < 0) + continue; + + /* is there a corn cob ? */ + 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) + n ++; + } + + return n; +} + + +/* fill circuit_wpline table with waypoints from circuit starting at + * i,j and using selected face */ +static int8_t get_path(const struct circuit *circuit, + uint8_t starti, uint8_t startj, uint8_t faceA, + struct wp_line *circuit_wpline) { const struct wp_coord *curcircuit; + const struct wp_coord *start; + const struct wp_coord *end; uint8_t prev_i, prev_j; uint8_t dir, prev_dir = 0xFF; uint8_t found = 0, i = 0, j = 0; uint8_t linenum; int8_t step = faceA ? 1 : -1; - int8_t skipfirst=0; int8_t path_len = 0; - printf_P(PSTR("face: %d\r\n"), faceA); - if ( !faceA && circuit->i == 11 && circuit->j == 6) - skipfirst=1; + /* start and end of circuit */ + if (faceA) { + start = &circuit->path[0]; + end = start + circuit->len - 1; + } + else { + end = &circuit->path[0]; + start = end + circuit->len - 1; + } + + DPR("face: %s %d\r\n", circuit->name, faceA); /* check that the point is present in the circuit */ - for (curcircuit = circuit + skipfirst; curcircuit->end == 0; curcircuit ++) { + for (curcircuit = start; curcircuit != end; curcircuit += step) { if (curcircuit->i == starti && curcircuit->j == startj) { found = 1; break; } } - - if ( !faceA && curcircuit->i == 11 && curcircuit->j == 6) - found = 1; if (found == 0) return -1; - /* XXX len must be >= 1 */ - /* XXX start = 11,6 */ + /* browse the circuit from starti, startj in the specified + * direction, and fill the table when direction changes */ prev_i = starti; prev_j = startj; + for ( ; curcircuit != end; + curcircuit += step, prev_dir = dir, prev_i = i, prev_j = j) { - curcircuit = curcircuit; - while (1) { - if (faceA && curcircuit->end) - break; - else if (!faceA && curcircuit == circuit) - break; i = curcircuit->i; j = curcircuit->j; @@ -298,25 +543,20 @@ int8_t get_path(const struct wp_coord *circuit, if (prev_dir != dir) { linenum = get_line_num(prev_i, prev_j, dir); - /* printf_P(PSTR("COIN %d, %d, dir=%d linenum=%d\r\n"), */ - /* prev_i, prev_j, dir, linenum); */ circuit_wpline[path_len].line_num = linenum; circuit_wpline[path_len].dir = dir; path_len++; } - prev_dir = dir; - prev_i = i; - prev_j = j; - curcircuit += step; } -/* printf_P(PSTR("COIN %d, %d\r\n"), curcircuit->i, curcircuit->j); */ - - return path_len; /* XXX */ + return path_len; } -int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved, - uint16_t tomato_retrieved, uint8_t len) +/* process score from retrieved objects number, and circuit len */ +static int16_t get_score(uint32_t wcorn_retrieved, + uint32_t ucorn_retrieved, + uint16_t tomato_retrieved, + uint8_t len, uint8_t opp_on_path) { int16_t score = 0; uint8_t i; @@ -341,7 +581,8 @@ int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved, mask <<= 1UL; } - printf_P(PSTR("get score: cob %d \r\n"), n); + DPR("get score: cob %d (->%d)\r\n", n, n/2); + /* score with tomato */ n = xget_ball_count(); mask = 1; @@ -355,101 +596,200 @@ int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved, mask <<= 1UL; } - printf_P(PSTR("get score: ball %d \r\n"), n); + DPR("get score: ball %d\r\n", n); + /* malus for long circuits */ score -= (len * 20); + DPR("malus for length: %d\r\n", len * 20); /* double malus for long circuits if we don't have much * time */ #define WP_SPEED 1 if (len * WP_SPEED > (MATCH_TIME - xtime_get_s())) { - uint16_t extra; + int32_t extra; extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s()); - score -= (200 * extra); + extra = (200 * extra); + if (extra < 0) /* should not happen */ + extra = 0; + if (extra > 10000) + extra = 10000; + score -= extra; + DPR("malus for length + time: %d\r\n", extra); } - /* XXX use direction of robot */ + /* malus if there is opponent on the path */ + if (opp_on_path) { + DPR("malus for opponent: 1000\r\n"); + score -= 2000; + } return score; } +/* return the corn type of specified coords: I2C_COB_WHITE, + * I2C_COB_UNKNOWN, or I2C_COB_NONE if it is black or not present */ +static uint8_t get_corn_type(uint8_t i, uint8_t j) +{ + uint8_t color; + /* is there a corn cob ? */ + if (strat_db.wp_table[i][j].type == WP_TYPE_CORN && + strat_db.wp_table[i][j].present) { + color = strat_db.wp_table[i][j].corn.color; + if (color == I2C_COB_WHITE) + return I2C_COB_WHITE; + else if (color == I2C_COB_UNKNOWN) + return I2C_COB_UNKNOWN; + } + return I2C_COB_NONE; +} + /* i,j: starting position */ -int8_t browse_one_circuit(const struct wp_coord *circuit, - uint8_t starti, uint8_t startj, - int16_t *scoreA, int16_t *scoreB) +static int8_t evaluate_one_face(const struct circuit *circuit, + uint8_t starti, uint8_t startj, + uint8_t faceA, int16_t *score) { const struct wp_coord *curcircuit; + const struct wp_coord *start; + const struct wp_coord *end; uint32_t wcorn_retrieved = 0; /* bit mask */ uint32_t ucorn_retrieved = 0; /* bit mask */ uint16_t tomato_retrieved = 0; /* bit mask */ - uint8_t len = 0, found = 0, i, j; - uint8_t ni = 0, nj = 0, pos, color; + 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; + int8_t step = faceA ? 1 : -1; + int16_t x, y, d; + int16_t oppx, oppy; + + *score = 0x8000; /* -int_max */ + + /* start and end of circuit */ + if (faceA) { + start = &circuit->path[0]; + end = start + circuit->len - 1; + } + else { + end = &circuit->path[0]; + start = end + circuit->len - 1; + } + + DPR("%s() face: %s %d\r\n", __FUNCTION__, circuit->name, faceA); /* check that the point is present in the circuit */ - for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++) { + for (curcircuit = start; curcircuit != end; curcircuit += step) { if (curcircuit->i == starti && curcircuit->j == startj) { found = 1; break; } } - if (found == 0) return -1; - for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++, len ++) { + /* get opponent coords */ + if (get_opponent_xy(&oppx, &oppy) < 0) + oppx = I2C_OPPONENT_NOT_THERE; + + /* silent the compiler */ + prev_i = 0xff; + prev_j = 0xff; + + /* browse all points and calculate the score */ + for (curcircuit = start; + curcircuit != end; + curcircuit += step, len ++, prev_i = i, prev_j = j) { i = curcircuit->i; j = curcircuit->j; -/* printf("cur:%d,%d %x %x %x %d\n", i, j, */ -/* wcorn_retrieved, ucorn_retrieved, tomato_retrieved, len); */ - - /* face A completed */ - if (i == starti && j == startj) { - *scoreA = get_score(wcorn_retrieved, ucorn_retrieved, - tomato_retrieved, len); - wcorn_retrieved = 0; /* bit mask */ - ucorn_retrieved = 0; /* bit mask */ - tomato_retrieved = 0; /* bit mask */ - len = 0; + /* 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; } + /* don't try to look cobs/tomato for first point */ + if (curcircuit == start) + continue; + + /* get current direction, we wil check cobs behind us + * on left and right */ + dir = get_dir(prev_i, prev_j, i, j); + + DPR("%d %d -> %d %d (%d)\n", prev_i, prev_j, i, j, dir); + /* 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); } - /* browse all neighbours to see if there is cobs */ - for (pos = LINE_UP; pos <= LINE_R_DOWN; pos++) { - if (get_neigh(i, j, &ni, &nj, pos) < 0) - continue; - - /* is there a corn cob ? */ - if (strat_db.wp_table[ni][nj].type == WP_TYPE_CORN && - strat_db.wp_table[ni][nj].present) { - color = strat_db.wp_table[ni][nj].corn.color; - if (color == I2C_COB_WHITE) - wcorn_retrieved |= (1UL << strat_db.wp_table[ni][nj].corn.idx); - else if (color == I2C_COB_UNKNOWN) - ucorn_retrieved |= (1UL << strat_db.wp_table[ni][nj].corn.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) { + DPR(" LEFT WCORN (%d)\n", idx); + wcorn_retrieved |= (1UL << idx); + } + else if (color == I2C_COB_UNKNOWN) { + DPR(" LEFT UCORN (%d)\n", idx); + ucorn_retrieved |= (1UL << idx); } } - }; - *scoreB = get_score(wcorn_retrieved, ucorn_retrieved, - tomato_retrieved, len); - if (circuit->i == starti && circuit->j == startj) - *scoreA = *scoreB; + /* behind right */ + 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) { + DPR(" RIGHT WCORN (%d)\n", idx); + wcorn_retrieved |= (1UL << idx); + } + else if (color == I2C_COB_UNKNOWN) { + DPR(" RIGHT UCORN (%d)\n", idx); + ucorn_retrieved |= (1UL << idx); + } + } + + /* prev_i, prev_j, len and curcircuit are updated in + * for (;;) */ + } + + /* write score and exit */ + *score = get_score(wcorn_retrieved, ucorn_retrieved, + tomato_retrieved, len, opponent_on_path); + return 0; +} + +/* i,j: starting position */ +static int8_t evaluate_one_circuit(const struct circuit *circuit, + uint8_t starti, uint8_t startj, + int16_t *scoreA, int16_t *scoreB) +{ + if (evaluate_one_face(circuit, starti, startj, 1, scoreA) < 0) + return -1; + /* we are on eject point, scoreB is the same */ + if (starti == 11 && startj == 6) { + *scoreB = *scoreA; + return 0; + } + + if (evaluate_one_face(circuit, starti, startj, 0, scoreB) < 0) + return -1; return 0; } /* i,j starting position */ -int8_t browse_circuits(uint8_t i, uint8_t j, - const struct wp_coord **selected_circuit, +int8_t find_best_circuit(uint8_t i, uint8_t j, + const struct circuit **selected_circuit, int8_t *selected_face) { - const struct wp_coord **circuit; + const struct circuit **circuit; int16_t scoreA, scoreB; int16_t selected_score = 0x8000; /* ~-int_max */ int8_t found = -1; @@ -457,10 +797,11 @@ int8_t browse_circuits(uint8_t i, uint8_t j, *selected_face = 0; *selected_circuit = circuits[0] ; for (circuit = &circuits[0]; *circuit; circuit++) { - if (browse_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0) + if (evaluate_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0) continue; found = 0; - printf_P(PSTR("Circuit: %d %d\r\n"), scoreA, scoreB); + DEBUG(E_USER_STRAT, "Scores for %s are: faceA=%d, faceB=%d", + (*circuit)->name, scoreA, scoreB); if (scoreA > selected_score) { *selected_circuit = *circuit; selected_score = scoreA; @@ -472,6 +813,13 @@ int8_t browse_circuits(uint8_t i, uint8_t j, *selected_face = 1; } } + + if (found == -1) + DEBUG(E_USER_STRAT, "no circuit found"); + else + DEBUG(E_USER_STRAT, "circuit found: %s, %s", + (*selected_circuit)->name, + (*selected_face) ? "faceA":"faceB"); return found; } @@ -481,15 +829,17 @@ static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len) if (len <= 0) return; for (i = 0; i < len; i ++) { - printf_P(PSTR("linenum %d dir %d\r\n"), circuit_wpline[i].line_num, + DEBUG(E_USER_STRAT, "linenum %d dir %d", + circuit_wpline[i].line_num, circuit_wpline[i].dir); } } +/* choose a circuit, then harvest on this circuit */ uint8_t strat_harvest_circuit(void) { - const struct wp_coord *selected_circuit; + const struct circuit *selected_circuit; int8_t selected_face; struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE]; int8_t len; @@ -508,7 +858,12 @@ uint8_t strat_harvest_circuit(void) return END_ERROR; } - browse_circuits(i, j, &selected_circuit, &selected_face); + 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; + } + len = get_path(selected_circuit, i, j, selected_face, circuit_wpline); if (len < 0) { DEBUG(E_USER_STRAT, "%s(): cannot find a path", @@ -522,16 +877,14 @@ uint8_t strat_harvest_circuit(void) prev_dir = circuit_wpline[0].dir; for (idx = 1; idx < len; idx ++) { retry: - if (get_cob_count() >= 5) - strat_set_speed(600, SPEED_ANGLE_FAST); - 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); + err = line2line(prev_linenum, prev_dir, linenum, dir, + TRAJ_FLAGS_NO_NEAR); if (!TRAJ_SUCCESS(err)) { strat_hardstop(); time_wait_ms(2000); @@ -547,9 +900,9 @@ uint8_t strat_harvest_circuit(void) void test_strat_avoid(void) { -#if 0 +#ifdef TEST_STRAT_AVOID uint8_t i, j; - const struct wp_coord *selected_circuit; + const struct circuit *selected_circuit; int8_t selected_face; struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE]; int8_t ret; @@ -559,37 +912,37 @@ void test_strat_avoid(void) ts = 0; bc = 0; cc = 0; printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); - browse_circuits(i, j, &selected_circuit, &selected_face); + find_best_circuit(i, j, &selected_circuit, &selected_face); ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); dump_circuit_wp(circuit_wpline, ret); ts = 0; bc = 3; cc = 0; printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); - browse_circuits(i, j, &selected_circuit, &selected_face); + find_best_circuit(i, j, &selected_circuit, &selected_face); ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); dump_circuit_wp(circuit_wpline, ret); ts = 0; bc = 4; cc = 0; printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); - browse_circuits(i, j, &selected_circuit, &selected_face); + find_best_circuit(i, j, &selected_circuit, &selected_face); ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); dump_circuit_wp(circuit_wpline, ret); ts = 0; bc = 3; cc = 5; printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); - browse_circuits(i, j, &selected_circuit, &selected_face); + find_best_circuit(i, j, &selected_circuit, &selected_face); ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); dump_circuit_wp(circuit_wpline, ret); ts = 0; bc = 4; cc = 5; printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); - browse_circuits(i, j, &selected_circuit, &selected_face); + find_best_circuit(i, j, &selected_circuit, &selected_face); ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); dump_circuit_wp(circuit_wpline, ret); ts = 80; bc = 0; cc = 0; printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); - browse_circuits(i, j, &selected_circuit, &selected_face); + find_best_circuit(i, j, &selected_circuit, &selected_face); ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); dump_circuit_wp(circuit_wpline, ret); @@ -598,19 +951,19 @@ void test_strat_avoid(void) ts = 0; bc = 0; cc = 0; printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); - browse_circuits(i, j, &selected_circuit, &selected_face); + find_best_circuit(i, j, &selected_circuit, &selected_face); ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); dump_circuit_wp(circuit_wpline, ret); ts = 0; bc = 3; cc = 0; printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); - browse_circuits(i, j, &selected_circuit, &selected_face); + find_best_circuit(i, j, &selected_circuit, &selected_face); ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); dump_circuit_wp(circuit_wpline, ret); ts = 80; bc = 0; cc = 0; printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); - browse_circuits(i, j, &selected_circuit, &selected_face); + find_best_circuit(i, j, &selected_circuit, &selected_face); ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); dump_circuit_wp(circuit_wpline, ret); @@ -619,19 +972,19 @@ void test_strat_avoid(void) ts = 0; bc = 0; cc = 0; printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); - browse_circuits(i, j, &selected_circuit, &selected_face); + find_best_circuit(i, j, &selected_circuit, &selected_face); ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); dump_circuit_wp(circuit_wpline, ret); ts = 0; bc = 3; cc = 0; printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); - browse_circuits(i, j, &selected_circuit, &selected_face); + find_best_circuit(i, j, &selected_circuit, &selected_face); ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); dump_circuit_wp(circuit_wpline, ret); ts = 80; bc = 0; cc = 0; printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); - browse_circuits(i, j, &selected_circuit, &selected_face); + find_best_circuit(i, j, &selected_circuit, &selected_face); ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); dump_circuit_wp(circuit_wpline, ret); #endif