X-Git-Url: http://git.droids-corp.org/?a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_avoid.c;h=73f97f7f1e5de0de3398ed2c8f73a9992b22331b;hb=d78a81c0df6dfe318d873a6f877e53cfb957ad8e;hp=5211c4f162c0d6421b01b861ed5572b7c1877bbb;hpb=cc67fe587de07a329525c8f5c8ecfd1fabbf83b8;p=aversive.git diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 5211c4f..73f97f7 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -315,6 +315,39 @@ const struct circuit letter_v_circuit = { .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, @@ -325,6 +358,7 @@ const struct circuit *circuits[] = { &asym_butterfly_circuit, &big_h_lambda_circuit, &letter_v_circuit, + &sperma_circuit, NULL, }; @@ -434,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; @@ -493,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; @@ -514,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 ++; } @@ -593,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; @@ -656,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; @@ -691,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, prev_d = 0; + int16_t x, y; + int32_t d, prev_d = 0; int16_t oppx, oppy; *score = 0x8000; /* -int_max */ @@ -744,11 +819,13 @@ 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); + 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 < 600 && d < prev_d) - opponent_on_path = 1; + 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; } @@ -765,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); @@ -787,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); @@ -803,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; } @@ -865,7 +959,7 @@ static int8_t find_best_circuit(uint8_t i, uint8_t j, return found; } -static void test_all_circuits(void) +static void init_all_circuits(void) { const struct circuit **circuit; const struct wp_coord *cur; @@ -887,6 +981,8 @@ static void test_all_circuits(void) 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); @@ -922,27 +1018,31 @@ uint8_t strat_harvest_circuit(void) 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); @@ -950,8 +1050,17 @@ uint8_t strat_harvest_circuit(void) 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; @@ -959,23 +1068,51 @@ uint8_t strat_harvest_circuit(void) __FUNCTION__, prev_linenum, prev_dir, linenum, dir); err = line2line(prev_linenum, prev_dir, linenum, dir, TRAJ_FLAGS_NO_NEAR); + + /* 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)) - return err; + goto fail; prev_linenum = linenum; prev_dir = dir; } + err = END_TRAJ; - return END_TRAJ; + fail: + strat_want_pack = 0; + return err; } +/* 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; - uint8_t i, j; + 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__); @@ -984,14 +1121,32 @@ uint8_t strat_unblock(void) 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; + 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 ? */ @@ -1005,13 +1160,12 @@ uint8_t strat_unblock(void) return err; strat_set_speed(old_dspeed, old_aspeed); - strat_want_pack = 0; return END_TRAJ; } -void test_strat_avoid(void) +void strat_avoid_init(void) { - test_all_circuits(); + init_all_circuits(); #ifdef TEST_STRAT_AVOID uint8_t i, j;