X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_avoid.c;h=73f97f7f1e5de0de3398ed2c8f73a9992b22331b;hp=2658b46ed31bbb44bf0739c1cb5f68df4f062f7c;hb=ad466314d3aff8f661654d4701b9d12fdeb7811f;hpb=b699727efceb661be65fbd22e975565b800c628e diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index 2658b46..73f97f7 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -665,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; @@ -763,11 +764,12 @@ 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; int32_t d, prev_d = 0; @@ -840,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); @@ -862,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); @@ -878,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; } @@ -1001,12 +1020,10 @@ uint8_t strat_harvest_circuit(void) strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW); strat_want_pack = 1; - printf("PACK PACK\n"); - 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); err = END_ERROR; @@ -1040,10 +1057,10 @@ uint8_t strat_harvest_circuit(void) goto fail; strat_want_pack = 0; - printf("UNPACK UNPACK\n"); /* do all lines of circuit */ for (idx = 1; idx < len; idx ++) { + retry: linenum = circuit_wpline[idx].line_num; dir = circuit_wpline[idx].dir; @@ -1051,6 +1068,16 @@ 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)) goto fail; @@ -1080,11 +1107,11 @@ const struct xy_point unblock_pts[] = { /* try to unblock in any situation */ uint8_t strat_unblock(void) { - int16_t x, y; + int16_t x, y, posx, posy; uint8_t i, j, k; uint16_t old_dspeed, old_aspeed; uint8_t err; - uint16_t d_min = 0xFFFF, d; + uint16_t d_min = 0x7FFF, d; const struct xy_point *pt; DEBUG(E_USER_STRAT, "%s()", __FUNCTION__); @@ -1094,24 +1121,23 @@ 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); + posx = position_get_x_s16(&mainboard.pos); + posy = position_get_y_s16(&mainboard.pos); + x = posx; + y = posy; - if (xycoord_to_ijcoord(&x, &y, &i, &j) < 0) + 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) { - /* position may have been modified */ - x = position_get_x_s16(&mainboard.pos); - y = position_get_y_s16(&mainboard.pos); /* 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(x, y, pt->x, COLOR_Y(pt->y)); + d = distance_between(posx, posy, pt->x, COLOR_Y(pt->y)); if (d < d_min) { d_min = d; x = pt->x; @@ -1119,6 +1145,8 @@ uint8_t strat_unblock(void) } } } + DEBUG(E_USER_STRAT, "%s() unblock point is %d,%d", + __FUNCTION__, x, y); /* XXX if opponent is too close, go back, or wait ? */ @@ -1132,7 +1160,6 @@ uint8_t strat_unblock(void) return err; strat_set_speed(old_dspeed, old_aspeed); - strat_want_pack = 0; return END_TRAJ; }