X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_avoid.c;h=37483ceb2ce4eaf264560a487c36eb667f07902b;hp=de85e30972c015a6d17816467ec45866e6da09cd;hb=HEAD;hpb=28da5858ac871c626153f47566e968ecb05ff52b diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c index de85e30..37483ce 100644 --- a/projects/microb2010/mainboard/strat_avoid.c +++ b/projects/microb2010/mainboard/strat_avoid.c @@ -552,7 +552,7 @@ int16_t linedir2angle(uint8_t dir) case LINE_R_UP: return COLOR_A(30); case LINE_R_DOWN: - return COLOR_A(-90); + return COLOR_A(-30); case LINE_L_UP: return COLOR_A(150); case LINE_L_DOWN: @@ -562,6 +562,25 @@ int16_t linedir2angle(uint8_t dir) } } +int16_t get_nearest_dir_angle(int16_t a) +{ + uint8_t dir, min_dir = 0; + int16_t min_diff = 0x7FFF, diff; + + for (dir = LINE_UP; dir <= LINE_R_UP; dir++) { + diff = abs(linedir2angle(dir) - a); + if (diff > 360) + diff -= 360; + if (diff > 360) + diff -= 360; + if (diff < min_diff) { + min_diff = diff; + min_dir = dir; + } + } + return linedir2angle(min_dir); +} + /* 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) { @@ -665,6 +684,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 +783,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 +861,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 +894,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 +915,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; } @@ -1004,7 +1042,7 @@ uint8_t strat_harvest_circuit(void) 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; @@ -1041,6 +1079,7 @@ uint8_t strat_harvest_circuit(void) /* do all lines of circuit */ for (idx = 1; idx < len; idx ++) { + retry: linenum = circuit_wpline[idx].line_num; dir = circuit_wpline[idx].dir; @@ -1048,6 +1087,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; @@ -1077,8 +1126,8 @@ const struct xy_point unblock_pts[] = { /* try to unblock in any situation */ uint8_t strat_unblock(void) { - int16_t x, y, posx, posy; - uint8_t i, j, k; + int16_t x, y, posx, posy, posa; + uint8_t i, j, k, cpt; uint16_t old_dspeed, old_aspeed; uint8_t err; uint16_t d_min = 0x7FFF, d; @@ -1090,13 +1139,15 @@ uint8_t strat_unblock(void) strat_get_speed(&old_dspeed, &old_aspeed); strat_hardstop(); + posa = position_get_a_deg_s16(&mainboard.pos); + 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(&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; @@ -1118,10 +1169,32 @@ 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 ? */ + for (cpt = 0; cpt < 2; cpt++) { + + /* 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)) + break; + + if (cpt == 1) + break; + + /* aie... do a S */ + trajectory_d_a_rel(&mainboard.traj, 100, 20); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + trajectory_d_a_rel(&mainboard.traj, 100, -20); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + trajectory_d_a_rel(&mainboard.traj, -100, -20); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + trajectory_d_a_rel(&mainboard.traj, -100, 20); + err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + } - /* go to nearest waypoint */ - trajectory_goto_xy_abs(&mainboard.traj, x, y); + trajectory_a_abs(&mainboard.traj, get_nearest_dir_angle(posa)); err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); if (err == END_TIMER) return err;