X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_avoid.c;h=5211c4f162c0d6421b01b861ed5572b7c1877bbb;hp=87bcf9f5ea9a63dfafd9fd5c7a4b0f837147cef3;hb=cc67fe587de07a329525c8f5c8ecfd1fabbf83b8;hpb=e442b9c066de9b55eef70fdf9993fc3f6b8259e8 diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 87bcf9f..5211c4f 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -61,322 +62,1043 @@ #include "strat_db.h" #include "strat_base.h" #include "strat_corn.h" +#include "strat_avoid.h" #include "strat_utils.h" #include "sensor.h" #include "actuator.h" -/* XXX TODO -static -const -change x,y -> i,j to avoid confusion with coords -could be optimized in mem space: it is not needed to store the x,y coord, - we can process it from idx. however it will be less optimized for speed - -*/ - -/* XXX enum possible ? else just rename */ -#define START 0 -#define UP 1 -#define UP_RIGHT 2 -#define DOWN_RIGHT 3 -#define DOWN 4 -#define DOWN_LEFT 5 -#define UP_LEFT 6 -#define END 7 - -struct djpoint { - uint16_t weight; - struct djpoint *parent; - - uint8_t parent_pos:3; - uint8_t updated:1; - uint8_t todo:1; - uint8_t reserved:3; +//#define VERBOSE + +#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; }; -/* database for dijkstra */ -static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; +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 = 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 = 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, }, +}; -/* return index from neigh pointer */ -#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) ) +const struct circuit butterfly_circuit = { + .name = "butterfly", + .len = sizeof(butterfly_tab)/sizeof(struct wp_coord), + .path = butterfly_tab, +}; -void dump(void) -{ - int8_t i, j; - struct djpoint *pt; - struct waypoint_db *wp; +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, }, +}; - printf_P(PSTR(" ")); - for (i=0; i=0; j--) { - printf_P(PSTR("%3d "), j/2); - - if ((j&1) == 0) - printf_P(PSTR(" ")); - - for (i=0; itype == WP_TYPE_OBSTACLE) - printf_P(PSTR(" X ")); - else if (wp->dangerous) - printf_P(PSTR(" D ")); - else if (wp->type == WP_TYPE_CORN && - wp->corn.color == I2C_COB_WHITE) - printf_P(PSTR(" W ")); - else if (wp->type == WP_TYPE_CORN && - wp->corn.color == I2C_COB_BLACK) - printf_P(PSTR(" B ")); - else if (wp->type == WP_TYPE_CORN && - wp->corn.color == I2C_COB_UNKNOWN) - printf_P(PSTR(" U ")); - else if (wp->type == WP_TYPE_WAYPOINT || - wp->type == WP_TYPE_TOMATO) - printf_P(PSTR(" %5d "), pt->weight); - else - printf_P(PSTR(" ? ")); - } - printf_P(PSTR("\r\n")); - } -} +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_circuit, + &losange_circuit, + &triangle_circuit, + &answer_d_circuit, + &h_lambda_circuit, + &asym_butterfly_circuit, + &big_h_lambda_circuit, + &letter_v_circuit, + NULL, +}; + +/* symetric neighbor position */ static inline uint8_t opposite_position(uint8_t pos) { pos += 3; - if (pos > UP_LEFT) + if (pos > LINE_L_UP) pos -= 6; return pos; } -/* is point reachable by the robot ? */ -static uint8_t is_reachable(uint8_t i, uint8_t j) +#ifdef HOST_VERSION +//#define TEST_STRAT_AVOID +#endif + +#ifdef TEST_STRAT_AVOID +static uint8_t cc; +uint8_t xget_cob_count(void) { - struct waypoint_db *wp; + return cc; +} - wp = &strat_db.wp_table[i][j]; - if (wp->type == WP_TYPE_WAYPOINT) - return 1; - if (wp->type == WP_TYPE_TOMATO) +static uint8_t bc; +uint8_t xget_ball_count(void) +{ + return bc; +} + +static uint32_t ts; +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 (wp->type == WP_TYPE_CORN && - wp->present == 0) + if (turn == 5) return 1; return 0; } -/* return coord of the entry in the table from the pointer */ -static void djpoint2ij(struct djpoint *pt, uint8_t *i, uint8_t *j) +/* return true if turn is at 60 deg */ +uint8_t is_120deg(uint8_t dir1, uint8_t dir2) { - int8_t idx = PT2IDX(pt); - *i = idx / WAYPOINTS_NBY; - *j = idx % WAYPOINTS_NBY; + 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 */ -static struct djpoint *get_neigh(struct djpoint *pt, - uint8_t position) +/* get the neighbour of the point at specified dir, return -1 if + * there is no neighbor */ +int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj, + uint8_t dir) { - uint8_t i,j; - struct djpoint *neigh; - - djpoint2ij(pt, &i, &j); - - switch (position) { - case UP: + switch (dir) { + case LINE_UP: j++; break; - case UP_RIGHT: - if (!(i & 1)) j++; + case LINE_R_UP: + if ((i & 1)) j++; i++; break; - case DOWN_RIGHT: - if (i & 1) j--; + case LINE_R_DOWN: + if (!(i & 1)) j--; i++; break; - case DOWN: + case LINE_DOWN: j--; break; - case DOWN_LEFT: - if (i & 1) j--; + case LINE_L_DOWN: + if (!(i & 1)) j--; i--; break; - case UP_LEFT: - if (!(i & 1)) j++; + case LINE_L_UP: + if ((i & 1)) j++; i--; break; default: - return NULL; + return -1; } if (i >= WAYPOINTS_NBX || j >= WAYPOINTS_NBY) - return NULL; + return -1; - if (is_reachable(i, j) == 0) - return NULL; + *ni = i; + *nj = j; + return 0; +} - neigh = &djpoints[i][j]; - return neigh; +static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir) +{ + switch (dir) { + case LINE_UP: + case LINE_DOWN: + return i/2; + case LINE_R_UP: + case LINE_L_DOWN: + i &= 0xfe; + j -= i/2; + return (5-j)/2; + case LINE_R_DOWN: + case LINE_L_UP: + i &= 0xfe; + j += i/2; + return (11-j)/2; + default: + return -1; + } } -static struct djpoint *get_next_neigh(struct djpoint *pt, uint8_t *position) +static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j, + uint8_t i, uint8_t j) { - struct djpoint *neigh = NULL; - uint8_t pos = *position + 1; + int8_t diff_i, diff_j; - for (pos = *position + 1; pos < END; pos++) { - neigh = get_neigh(pt, pos); - if (neigh != NULL) - break; - } + diff_i = i - prev_i; + diff_j = j - prev_j; - *position = pos; - return neigh; -} + if (diff_i == 0 && diff_j == 1) + return LINE_UP; + if (diff_i == 0 && diff_j == -1) + return LINE_DOWN; -/* browse all points */ -#define POINT_FOREACH(cur) \ - for (cur = &djpoints[0][0]; \ - cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \ - cur ++) + if ((prev_i & 1) == 0) { + if (diff_i == 1 && diff_j == 0) + return LINE_R_UP; + if (diff_i == 1 && diff_j == -1) + return LINE_R_DOWN; + if (diff_i == -1 && diff_j == 0) + return LINE_L_UP; + if (diff_i == -1 && diff_j == -1) + return LINE_L_DOWN; + } + else { + if (diff_i == 1 && diff_j == 1) + return LINE_R_UP; + if (diff_i == 1 && diff_j == 0) + return LINE_R_DOWN; + if (diff_i == -1 && diff_j == 1) + return LINE_L_UP; + if (diff_i == -1 && diff_j == 0) + return LINE_L_DOWN; + } -/* XXX comment */ -#define NEIGH_FOREACH(neigh, pos, point) \ - for (pos = START, neigh = get_next_neigh(point, &pos); \ - neigh; \ - neigh = get_next_neigh(point, &pos)) + /* invalid value */ + return 0xFF; +} -int dijkstra_init(void) +/* 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; } -/* return distance between p1 and p2 */ -static uint16_t dist(struct djpoint *p1, struct djpoint *p2) +/* count the number of non-black corns which are neighbors of + * specified cob */ +uint8_t corn_count_neigh(uint8_t i, uint8_t j) { - int16_t x1, y1, x2, y2; - double vx, vy; - uint8_t i, j; + uint8_t dir, n = 0; + uint8_t ni, nj; - djpoint2ij(p1, &i, &j); - ijcoord_to_xycoord(i, j, &x1, &y1); + for (dir = LINE_UP; dir <= LINE_R_UP; dir++) { + if (wp_get_neigh(i, j, &ni, &nj, dir) < 0) + continue; - djpoint2ij(p2, &i, &j); - ijcoord_to_xycoord(i, j, &x2, &y2); + /* 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 ++; + } - vx = x2 - x1; - vy = y2 - y1; - return sqrt(vx * vx + vy * vy); + return n; } -void dijkstra_process_neighs(struct djpoint *pt) + +/* 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) { - struct djpoint *neigh; - uint8_t pos, parent_pos; - uint16_t weight; - uint8_t i,j; - - djpoint2ij(pt, &i, &j); - printf_P(PSTR("at %d %d:\r\n"), i, j); - - NEIGH_FOREACH(neigh, pos, pt) { - weight = pt->weight + dist(pt, neigh); - parent_pos = opposite_position(pos); - - /* bonus if we keep the same direction */ - if (parent_pos == pt->parent_pos || - pt->parent_pos == END) - weight = weight - 1; - - printf_P(PSTR(" pos=%d: ppos=%d opos=%d nw=%d w=%d\r\n"), pos, - pt->parent_pos, parent_pos, - neigh->weight, weight); - if (neigh->weight == 0 || weight < neigh->weight) { - djpoint2ij(neigh, &i, &j); - //printf_P(PSTR(" %d,%d updated\r\n"), i, j); - neigh->weight = weight; - neigh->parent_pos = parent_pos; - neigh->updated = 1; + 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 path_len = 0; + + /* 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(): %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) { + if (curcircuit->i == starti && curcircuit->j == startj) { + found = 1; + break; + } + } + if (found == 0) + return -1; + + + /* 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) { + + i = curcircuit->i; + j = curcircuit->j; + + dir = get_dir(prev_i, prev_j, i, j); + + if (prev_dir != dir) { + 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++; } } + + return path_len; } -int dijkstra(struct djpoint *start) +/* 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) { - struct djpoint *cur; - uint8_t todolist = 1; - - start->todo = 1; - - while (todolist) { - printf_P(PSTR("\r\n")); - dump(); - /* process all neighbours of todo list */ - POINT_FOREACH(cur) { - if (!cur->todo) - continue; - dijkstra_process_neighs(cur); - cur->todo = 0; + int16_t score = 0; + uint8_t i; + uint32_t mask = 1; + uint8_t n; + + /* score with corn */ + n = xget_cob_count() * 2; + for (i = 0; i= 10) + break; + if (wcorn_retrieved & mask) { + score += 250; + n += 2; + } + if (n >= 10) + break; + if (ucorn_retrieved & mask) { + score += 125; + n += 1; } + mask <<= 1UL; + } + + DPR("get score: cob %d (->%d)\r\n", n, n/2); - /* convert updated list in todo list */ - todolist = 0; - POINT_FOREACH(cur) { - if (!cur->updated) - continue; - todolist = 1; - cur->todo = 1; - cur->updated = 0; + /* score with tomato */ + n = xget_ball_count(); + mask = 1; + for (i = 0; i= 4) + break; + if (tomato_retrieved & mask) { + score += 150; + n += 1; } + mask <<= 1UL; + } + + 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())) { + int32_t extra; + extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s()); + 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); + } + + /* 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 0; /* XXX */ + return I2C_COB_NONE; } -/* init waypoints position */ -void init_djpoints(void) +/* i,j: starting position */ +static int8_t evaluate_one_face(const struct circuit *circuit, + uint8_t starti, uint8_t startj, + uint8_t faceA, int16_t *score) { - int8_t i, j; - struct djpoint *pt; + 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 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, prev_d = 0; + int16_t oppx, oppy; - for (i=0; iparent_pos = END; - pt->updated = 0; - pt->todo = 0; - pt->weight = 0; + /* 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 = start; curcircuit != end; curcircuit += step) { + if (curcircuit->i == starti && curcircuit->j == startj) { + found = 1; + break; } } + if (found == 0) + return -1; + + /* 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 (; /* start at starti,startj */ + curcircuit != end; + curcircuit += step, len ++, prev_i = i, prev_j = j) { + i = curcircuit->i; + j = curcircuit->j; + + /* 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); + 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 */ + 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); + } + + /* 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); + } + } + + /* 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; } -int get_path(struct djpoint *start, struct djpoint *end) +/* 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) { - struct djpoint *cur; - uint8_t prev_direction = 0; - int8_t idx; - int16_t x, y; + 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; +} - for (cur = start; - cur != NULL && cur->parent_pos != END && cur != end; - cur = get_neigh(cur, cur->parent_pos)) { - if (prev_direction != cur->parent_pos) { - idx = PT2IDX(cur); - corn_idx_to_xycoord(idx, &x, &y); - printf_P(PSTR("%d %d (%d)\r\n"), - x, y, cur->parent_pos); +/* i,j starting position */ +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; + int16_t selected_score = 0x8000; /* ~-int_max */ + int8_t found = -1; + + *selected_face = 0; + *selected_circuit = circuits[0] ; + for (circuit = &circuits[0]; *circuit; circuit++) { + if (evaluate_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0) + continue; + found = 0; + 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; + *selected_face = 1; + } + if (scoreB > selected_score) { + *selected_circuit = *circuit; + selected_score = scoreB; + *selected_face = 0; } - prev_direction = cur->parent_pos; } - idx = PT2IDX(end); - corn_idx_to_xycoord(idx, &x, &y); - printf_P(PSTR("%d %d\r\n"), x, y); - return 0; /* XXX */ + 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; +} + +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; + if (len <= 0) + return; + for (i = 0; i < len; i ++) { + 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 circuit *selected_circuit; + int8_t selected_face; + struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE]; + int8_t len; + uint8_t i, j, idx; + int16_t x, y; + uint8_t linenum, prev_linenum; + 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); + + if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) { + DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d", + __FUNCTION__, x, y); + return END_ERROR; + } + + 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", + __FUNCTION__); + return END_ERROR; + } + + dump_circuit_wp(circuit_wpline, len); + + prev_linenum = circuit_wpline[0].line_num; + prev_dir = circuit_wpline[0].dir; + + /* do all lines of circuit */ + for (idx = 1; idx < len; idx ++) { + linenum = circuit_wpline[idx].line_num; + dir = circuit_wpline[idx].dir; + + 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)) + return err; + + prev_linenum = linenum; + prev_dir = dir; + } + + 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; + int8_t selected_face; + struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE]; + int8_t ret; + + i = 1; j = 1; + printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j); + + ts = 0; bc = 0; cc = 0; + printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); + 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); + 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); + 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); + 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); + 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); + 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); + + i = 4; j = 3; + printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j); + + ts = 0; bc = 0; cc = 0; + printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); + 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); + 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); + 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); + + i = 11; j = 6; + printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j); + + ts = 0; bc = 0; cc = 0; + printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc); + 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); + 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); + 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 }