X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_avoid.c;h=73f97f7f1e5de0de3398ed2c8f73a9992b22331b;hp=61151ff4fd1936bf9fdbe86af7e678ebe4e05dae;hb=c20f7eb8460dd04e42f95d799f17d9b60b5ee8e4;hpb=878d0fc33d940d5d02fc65dfd0e56798c97de18d diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 61151ff..73f97f7 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,73 @@ 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, +}; + +const struct wp_coord sperma_tab[] = { + { .i = 11, .j = 6, }, + { .i = 10, .j = 6, }, + { .i = 9, .j = 5, }, + { .i = 8, .j = 5, }, + { .i = 7, .j = 5, }, + { .i = 6, .j = 6, }, + { .i = 5, .j = 5, }, + { .i = 4, .j = 5, }, + { .i = 3, .j = 4, }, + { .i = 2, .j = 4, }, + { .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 = 5, }, + { .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 sperma_circuit = { + .name = "sperma", + .len = sizeof(sperma_tab)/sizeof(struct wp_coord), + .path = sperma_tab, +}; + /* list of all possible circuits */ const struct circuit *circuits[] = { &butterfly_circuit, @@ -290,6 +357,8 @@ const struct circuit *circuits[] = { &h_lambda_circuit, &asym_butterfly_circuit, &big_h_lambda_circuit, + &letter_v_circuit, + &sperma_circuit, NULL, }; @@ -399,19 +468,33 @@ int8_t wp_get_neigh(uint8_t i, uint8_t j, uint8_t *ni, uint8_t *nj, return 0; } -static uint8_t get_line_num(int8_t i, int8_t j, uint8_t dir) +static int8_t get_line_num(int8_t i, int8_t j, uint8_t dir) { + uint8_t mod; + switch (dir) { case LINE_UP: case LINE_DOWN: + if ((i & 1) == 0) + return -1; return i/2; case LINE_R_UP: case LINE_L_DOWN: + mod = i & 3; + if ((mod == 0 || mod == 1) && ((j & 1) == 0)) + return -1; + if ((mod == 2 || mod == 3) && ((j & 1) == 1)) + return -1; i &= 0xfe; j -= i/2; return (5-j)/2; case LINE_R_DOWN: case LINE_L_UP: + mod = i & 3; + if ((mod == 0 || mod == 3) && ((j & 1) == 0)) + return -1; + if ((mod == 1 || mod == 2) && ((j & 1) == 1)) + return -1; i &= 0xfe; j += i/2; return (11-j)/2; @@ -458,11 +541,34 @@ static uint8_t get_dir(uint8_t prev_i, uint8_t prev_j, return 0xFF; } +/* return approximative angle of line */ +int16_t linedir2angle(uint8_t dir) +{ + switch (dir) { + case LINE_UP: + return COLOR_A(90); + case LINE_DOWN: + return COLOR_A(-90); + case LINE_R_UP: + return COLOR_A(30); + case LINE_R_DOWN: + return COLOR_A(-90); + case LINE_L_UP: + return COLOR_A(150); + case LINE_L_DOWN: + return COLOR_A(-150); + default: + return 0; + } +} + /* 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; + int8_t ln; ln = get_line_num(i, j, dir); + if (ln == -1) + return 0; if (ln == linenum) return 1; return 0; @@ -479,10 +585,11 @@ uint8_t corn_count_neigh(uint8_t i, uint8_t j) if (wp_get_neigh(i, j, &ni, &nj, dir) < 0) continue; - /* is there a corn cob ? */ + /* is there a corn cob removed for more than 2 secs ? */ 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) + strat_db.wp_table[ni][nj].corn.color != I2C_COB_BLACK && + (strat_db.wp_table[ni][nj].present || + strat_db.wp_table[ni][nj].time_removed + 2 > time_get_s())) n ++; } @@ -516,7 +623,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 +652,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++; } } @@ -556,6 +665,7 @@ static int8_t get_path(const struct circuit *circuit, static int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved, uint16_t tomato_retrieved, + uint16_t utomato_retrieved, uint8_t len, uint8_t opp_on_path) { int16_t score = 0; @@ -619,8 +729,8 @@ static int16_t get_score(uint32_t wcorn_retrieved, /* malus if there is opponent on the path */ if (opp_on_path) { - DPR("malus for opponent: 1000\r\n"); - score -= 2000; + DPR("malus for opponent: %d\r\n", (500 * opp_on_path)); + score -= (500 * opp_on_path); } return score; @@ -654,13 +764,15 @@ static int8_t evaluate_one_face(const struct circuit *circuit, uint32_t wcorn_retrieved = 0; /* bit mask */ uint32_t ucorn_retrieved = 0; /* bit mask */ uint16_t tomato_retrieved = 0; /* bit mask */ + uint16_t utomato_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; + uint8_t dir, color, idx, visited; int8_t step = faceA ? 1 : -1; - int16_t x, y, d; + int16_t x, y; + int32_t d, prev_d = 0; int16_t oppx, oppy; *score = 0x8000; /* -int_max */ @@ -690,13 +802,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; @@ -705,9 +819,14 @@ static int8_t evaluate_one_face(const struct circuit *circuit, /* 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; + d = quad_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 < (250L*250L) && d < prev_d) + opponent_on_path += 3; + else if (d < (500L*500L) && d < prev_d) + opponent_on_path ++; + prev_d = d; } /* don't try to look cobs/tomato for first point */ @@ -723,18 +842,29 @@ static int8_t evaluate_one_face(const struct circuit *circuit, /* 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); + if (strat_db.wp_table[i][j].opp_visited) { + DPR(" TOMATO (opp visited)\n"); + utomato_retrieved |= (1UL << strat_db.wp_table[i][j].tomato.idx); + } + else { + 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) { + visited = strat_db.wp_table[ni][nj].opp_visited; + if (color == I2C_COB_WHITE && !visited) { DPR(" LEFT WCORN (%d)\n", idx); wcorn_retrieved |= (1UL << idx); } + else if (color == I2C_COB_WHITE && visited) { + DPR(" LEFT CORN visited (%d)\n", idx); + ucorn_retrieved |= (1UL << idx); + } else if (color == I2C_COB_UNKNOWN) { DPR(" LEFT UCORN (%d)\n", idx); ucorn_retrieved |= (1UL << idx); @@ -745,10 +875,15 @@ static int8_t evaluate_one_face(const struct circuit *circuit, 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) { + visited = strat_db.wp_table[ni][nj].opp_visited; + if (color == I2C_COB_WHITE && !visited) { DPR(" RIGHT WCORN (%d)\n", idx); wcorn_retrieved |= (1UL << idx); } + else if (color == I2C_COB_WHITE && visited) { + DPR(" RIGHT CORN visited (%d)\n", idx); + ucorn_retrieved |= (1UL << idx); + } else if (color == I2C_COB_UNKNOWN) { DPR(" RIGHT UCORN (%d)\n", idx); ucorn_retrieved |= (1UL << idx); @@ -761,7 +896,8 @@ static int8_t evaluate_one_face(const struct circuit *circuit, /* write score and exit */ *score = get_score(wcorn_retrieved, ucorn_retrieved, - tomato_retrieved, len, opponent_on_path); + tomato_retrieved, utomato_retrieved, + len, opponent_on_path); return 0; } @@ -785,9 +921,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 +941,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 +959,38 @@ int8_t find_best_circuit(uint8_t i, uint8_t j, return found; } +static void init_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; + + strat_db.wp_table[i][j].on_circuit = 1; + + 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,57 +1017,156 @@ uint8_t strat_harvest_circuit(void) uint8_t dir, prev_dir; uint8_t err; + strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); + strat_want_pack = 1; + x = position_get_x_s16(&mainboard.pos); y = position_get_y_s16(&mainboard.pos); - if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) { + if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) { DEBUG(E_USER_STRAT, "%s(): cannot find waypoint at %d,%d", __FUNCTION__, x, y); - return END_ERROR; + err = END_ERROR; + goto fail; } 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; + err = END_ERROR; + goto fail; } 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; + err = END_ERROR; + goto fail; } dump_circuit_wp(circuit_wpline, len); prev_linenum = circuit_wpline[0].line_num; prev_dir = circuit_wpline[0].dir; + + /* fix orientation first */ + trajectory_a_abs(&mainboard.traj, linedir2angle(prev_dir)); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + if (!TRAJ_SUCCESS(err)) + goto fail; + + strat_want_pack = 0; + + /* 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(); + + /* in some cases it is better to wait that obstacle is + * gone before starting to avoid it */ + if (err == END_OBSTACLE && + strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE && + time_get_s() > strat_conf.prev_wait_obstacle + 5) { + strat_conf.prev_wait_obstacle = time_get_s(); time_wait_ms(2000); goto retry; } + if (!TRAJ_SUCCESS(err)) + goto fail; prev_linenum = linenum; prev_dir = dir; } + err = END_TRAJ; - return END_TRAJ; // XXX + fail: + strat_want_pack = 0; + return err; } -void test_strat_avoid(void) +/* list of waypoints when we are not on a circuit */ +const struct xy_point unblock_pts[] = { + { .x = 375, .y = 597 }, /* 1,1 */ + { .x = 2625, .y = 597 }, /* 11,1 */ + { .x = 1500, .y = 722 }, /* 6,2 */ + { .x = 375, .y = 1097 }, /* 1,3 */ + { .x = 375, .y = 1597 }, /* 1,5 */ + { .x = 2625, .y = 1097 }, /* 11,3 */ + { .x = 2625, .y = 1597 }, /* 11,5 */ + { .x = 1500, .y = 1722 }, /* 6,6 */ +}; + + +/* try to unblock in any situation */ +uint8_t strat_unblock(void) +{ + int16_t x, y, posx, posy; + uint8_t i, j, k; + uint16_t old_dspeed, old_aspeed; + uint8_t err; + uint16_t d_min = 0x7FFF, d; + const struct xy_point *pt; + + 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); + posx = position_get_x_s16(&mainboard.pos); + posy = position_get_y_s16(&mainboard.pos); + x = posx; + y = posy; + + if (xycoord_to_ijcoord_not_corn(&x, &y, &i, &j) < 0) + x = -1; + else if (strat_db.wp_table[i][j].on_circuit == 0) + x = -1; + + /* find the nearest unblock point */ + if (x == -1) { + + /* browse all points and find the nearest */ + for (k = 0; k < sizeof(unblock_pts)/sizeof(*unblock_pts); k++) { + pt = &unblock_pts[k]; + d = distance_between(posx, posy, pt->x, COLOR_Y(pt->y)); + if (d < d_min) { + d_min = d; + x = pt->x; + y = COLOR_Y(pt->y); + } + } + } + DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d", + __FUNCTION__, x, 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); + return END_TRAJ; +} + +void strat_avoid_init(void) { + init_all_circuits(); + #ifdef TEST_STRAT_AVOID uint8_t i, j; const struct circuit *selected_circuit;