X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_avoid.c;h=cd54e6c3c2c5f04eace314654e124dd33243c819;hp=87bcf9f5ea9a63dfafd9fd5c7a4b0f837147cef3;hb=17aadc4c8c3e60c2b5e6bbba91c8542849addbd7;hpb=e442b9c066de9b55eef70fdf9993fc3f6b8259e8 diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 87bcf9f..cd54e6c 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,576 @@ #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; -}; - -/* database for dijkstra */ -static struct djpoint djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; - -/* return index from neigh pointer */ -#define PT2IDX(neigh) ( ((void *)(neigh)-(void *)(&djpoints)) / sizeof(*neigh) ) -void dump(void) -{ - int8_t i, j; - struct djpoint *pt; - struct waypoint_db *wp; - - 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")); - } -} +/* list of all possible circuits */ +const struct wp_coord *circuits[] = { + circuit1, + circuit2, + 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) +static uint8_t cc; +uint8_t xget_cob_count(void) { - struct waypoint_db *wp; - - wp = &strat_db.wp_table[i][j]; - if (wp->type == WP_TYPE_WAYPOINT) - return 1; - if (wp->type == WP_TYPE_TOMATO) - return 1; - if (wp->type == WP_TYPE_CORN && - wp->present == 0) - return 1; - return 0; + return cc; } -/* return coord of the entry in the table from the pointer */ -static void djpoint2ij(struct djpoint *pt, uint8_t *i, uint8_t *j) +static uint8_t bc; +uint8_t xget_ball_count(void) { - int8_t idx = PT2IDX(pt); - *i = idx / WAYPOINTS_NBY; - *j = idx % WAYPOINTS_NBY; + return bc; } -/* get the neighbour of the point at specified position */ -static struct djpoint *get_neigh(struct djpoint *pt, - uint8_t position) +static uint32_t ts; +uint8_t xtime_get_s(void) { - uint8_t i,j; - struct djpoint *neigh; + return ts; +} - djpoint2ij(pt, &i, &j); +/* get the neighbour of the point at specified position, 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) +{ switch (position) { - case UP: + case LINE_UP: j++; break; - case UP_RIGHT: + case LINE_R_UP: if (!(i & 1)) j++; i++; break; - case DOWN_RIGHT: + case LINE_R_DOWN: if (i & 1) j--; i++; break; - case DOWN: + case LINE_DOWN: j--; break; - case DOWN_LEFT: + case LINE_L_DOWN: if (i & 1) j--; i--; break; - case UP_LEFT: + 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; + + diff_i = i - prev_i; + diff_j = j - prev_j; + + if (diff_i == 0 && diff_j == 1) + return LINE_UP; + if (diff_i == 0 && diff_j == -1) + return LINE_DOWN; + + 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; + } - for (pos = *position + 1; pos < END; pos++) { - neigh = get_neigh(pt, pos); - if (neigh != NULL) + /* invalid value */ + return 0xFF; +} + + +int8_t get_path(const struct wp_coord *circuit, + uint8_t starti, uint8_t startj, uint8_t faceA, + struct wp_line *circuit_wpline) +{ + const struct wp_coord *curcircuit; + 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("face: %d\n", faceA); + if ( !faceA && circuit->i == 11 && circuit->j == 6) + skipfirst=1; + + /* check that the point is present in the circuit */ + for (curcircuit = circuit + skipfirst; curcircuit->end == 0; curcircuit ++) { + if (curcircuit->i == starti && curcircuit->j == startj) { + found = 1; break; + } } - *position = pos; - return neigh; -} + if ( !faceA && curcircuit->i == 11 && curcircuit->j == 6) + found = 1; + if (found == 0) + return -1; -/* browse all points */ -#define POINT_FOREACH(cur) \ - for (cur = &djpoints[0][0]; \ - cur < &djpoints[WAYPOINTS_NBX][WAYPOINTS_NBY]; \ - cur ++) + /* XXX len must be >= 1 */ + /* XXX start = 11,6 */ -/* XXX comment */ -#define NEIGH_FOREACH(neigh, pos, point) \ - for (pos = START, neigh = get_next_neigh(point, &pos); \ - neigh; \ - neigh = get_next_neigh(point, &pos)) + prev_i = starti; + prev_j = startj; -int dijkstra_init(void) -{ - return 0; + curcircuit = curcircuit; + while (1) { + if (faceA && curcircuit->end) + break; + else if (!faceA && curcircuit == circuit) + break; + 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); + /* 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 distance between p1 and p2 */ -static uint16_t dist(struct djpoint *p1, struct djpoint *p2) +int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved, + uint16_t tomato_retrieved, uint8_t len) { - int16_t x1, y1, x2, y2; - double vx, vy; - uint8_t i, j; + 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; + } - djpoint2ij(p1, &i, &j); - ijcoord_to_xycoord(i, j, &x1, &y1); + printf("get score: cob %d \n", n); + /* 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; + } - djpoint2ij(p2, &i, &j); - ijcoord_to_xycoord(i, j, &x2, &y2); + printf("get score: ball %d \n", n); + /* malus for long circuits */ + score -= (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; + extra = (len * WP_SPEED) - (MATCH_TIME - xtime_get_s()); + score -= (200 * extra); + } - vx = x2 - x1; - vy = y2 - y1; - return sqrt(vx * vx + vy * vy); + /* XXX use direction of robot */ + + return score; } -void dijkstra_process_neighs(struct djpoint *pt) +/* 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) { - 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; + 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; + + /* check that the point is present in the circuit */ + for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++) { + if (curcircuit->i == starti && curcircuit->j == startj) { + found = 1; + break; } } -} -int dijkstra(struct djpoint *start) -{ - struct djpoint *cur; - uint8_t todolist = 1; + if (found == 0) + return -1; - start->todo = 1; + for (curcircuit = circuit; curcircuit->end == 0; curcircuit ++, len ++) { + i = curcircuit->i; + j = curcircuit->j; - 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; +/* 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 there a tomato ? */ + if (strat_db.wp_table[i][j].type == WP_TYPE_TOMATO && + strat_db.wp_table[i][j].present) { + tomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx); } - /* convert updated list in todo list */ - todolist = 0; - POINT_FOREACH(cur) { - if (!cur->updated) + /* 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; - todolist = 1; - cur->todo = 1; - cur->updated = 0; + + /* 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); + } } - } - return 0; /* XXX */ -} + }; -/* init waypoints position */ -void init_djpoints(void) -{ - int8_t i, j; - struct djpoint *pt; + *scoreB = get_score(wcorn_retrieved, ucorn_retrieved, + tomato_retrieved, len); + if (circuit->i == starti && circuit->j == startj) + *scoreA = *scoreB; - for (i=0; iparent_pos = END; - pt->updated = 0; - pt->todo = 0; - pt->weight = 0; +/* i,j starting position */ +int8_t browse_circuits(uint8_t i, uint8_t j, + const struct wp_coord **selected_circuit, + int8_t *selected_face) +{ + const struct wp_coord **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 (browse_one_circuit(*circuit, i, j, &scoreA, &scoreB) < 0) + continue; + found = 0; + printf_P(PSTR("Circuit: %d %d\r\n"), scoreA, scoreB); + if (scoreA > selected_score) { + *selected_circuit = *circuit; + selected_score = scoreA; + *selected_face = 0; } + if (scoreB > selected_score) { + *selected_circuit = *circuit; + selected_score = scoreB; + *selected_face = 1; + } + } + return found; +} + +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 ++) { + printf(PSTR("linenum %d dir %d\r\n"), circuit_wpline[i].line_num, + circuit_wpline[i].dir); } + } -int get_path(struct djpoint *start, struct djpoint *end) +uint8_t strat_harvest_circuit(void) { - struct djpoint *cur; - uint8_t prev_direction = 0; - int8_t idx; + const struct wp_coord *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; + + x = position_get_x_s16(&mainboard.pos); + y = position_get_y_s16(&mainboard.pos); - 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); + 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; + } + + browse_circuits(i, j, &selected_circuit, &selected_face); + 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; + 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); + if (!TRAJ_SUCCESS(err)) { + strat_hardstop(); + time_wait_ms(2000); + goto retry; } - prev_direction = cur->parent_pos; + + prev_linenum = linenum; + prev_dir = dir; } - idx = PT2IDX(end); - corn_idx_to_xycoord(idx, &x, &y); - printf_P(PSTR("%d %d\r\n"), x, y); - return 0; /* XXX */ + return END_TRAJ; // XXX +} + +void test_strat_avoid(void) +{ + uint8_t i, j; + const struct wp_coord *selected_circuit; + int8_t selected_face; + struct wp_line circuit_wpline[MAX_CIRCUIT_WPLINE]; + int8_t ret; + + i = 1; j = 1; + printf("========= i=%d, j=%d\r\n", i, j); + + ts = 0; bc = 0; cc = 0; + printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc); + browse_circuits(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("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc); + browse_circuits(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("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc); + browse_circuits(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("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc); + browse_circuits(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("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc); + browse_circuits(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("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc); + browse_circuits(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("========= i=%d, j=%d\r\n", i, j); + + ts = 0; bc = 0; cc = 0; + printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc); + browse_circuits(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("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc); + browse_circuits(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("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc); + browse_circuits(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("========= i=%d, j=%d\r\n", i, j); + + ts = 0; bc = 0; cc = 0; + printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc); + browse_circuits(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("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc); + browse_circuits(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("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc); + browse_circuits(i, j, &selected_circuit, &selected_face); + ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline); + dump_circuit_wp(circuit_wpline, ret); + }