X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_base.c;h=90762a635d72a7066f8b0011eeaa04e4517fa3f6;hp=acded681383da332ef6aa8d40410b10e9bd2575f;hb=HEAD;hpb=b699727efceb661be65fbd22e975565b800c628e diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c index acded68..90762a6 100644 --- a/projects/microb2010/mainboard/strat_base.c +++ b/projects/microb2010/mainboard/strat_base.c @@ -59,10 +59,14 @@ #include "strat.h" #include "sensor.h" #include "i2c_protocol.h" +#include "robotsim.h" /* true if we want to interrupt a trajectory */ static uint8_t traj_intr=0; +/* to debug state of strat_obstacle() */ +static uint8_t strat_obstacle_debug = 0; + /* filled when a END_OBSTACLE is returned */ struct opponent_obstacle opponent_obstacle; @@ -143,19 +147,38 @@ uint8_t strat_goto_xy_force(int16_t x, int16_t y) } /* reset position */ -void strat_reset_pos(int16_t x, int16_t y, int16_t a) +void strat_reset_pos(int16_t x, int16_t y, double a) { - int16_t posx = position_get_x_s16(&mainboard.pos); - int16_t posy = position_get_y_s16(&mainboard.pos); - int16_t posa = position_get_a_deg_s16(&mainboard.pos); + double posx = position_get_x_double(&mainboard.pos); + double posy = position_get_y_double(&mainboard.pos); + double posa = position_get_a_rad_double(&mainboard.pos); if (x == DO_NOT_SET_POS) x = posx; if (y == DO_NOT_SET_POS) y = posy; if (a == DO_NOT_SET_POS) - a = posa; - + a = DEG(posa); + + /* some issues with blocking on simulator */ +#ifdef HOST_VERSION + if (x == ROBOT_HALF_LENGTH_REAR) + x = ROBOT_HALF_LENGTH_REAR + 10; + if (x == AREA_X - ROBOT_HALF_LENGTH_REAR) + x = AREA_X - ROBOT_HALF_LENGTH_REAR - 10; + if (y == ROBOT_HALF_LENGTH_REAR) + y = ROBOT_HALF_LENGTH_REAR + 10; + if (y == AREA_Y - ROBOT_HALF_LENGTH_REAR) + y = AREA_Y - ROBOT_HALF_LENGTH_REAR - 10; + if (x == ROBOT_HALF_LENGTH_FRONT) + x = ROBOT_HALF_LENGTH_FRONT + 10; + if (x == AREA_X - ROBOT_HALF_LENGTH_FRONT) + x = AREA_X - ROBOT_HALF_LENGTH_FRONT - 10; + if (y == ROBOT_HALF_LENGTH_FRONT) + y = ROBOT_HALF_LENGTH_FRONT + 10; + if (y == AREA_Y - ROBOT_HALF_LENGTH_FRONT) + y = AREA_Y - ROBOT_HALF_LENGTH_FRONT - 10; +#endif DEBUG(E_USER_STRAT, "reset pos (%s%s%s)", x == DO_NOT_SET_POS ? "" : "x", y == DO_NOT_SET_POS ? "" : "y", @@ -176,14 +199,45 @@ uint8_t strat_calib(int16_t dist, uint8_t flags) int32_t max_in = pid_get_max_in(&mainboard.angle.pid); int32_t max_i = pid_get_max_I(&mainboard.angle.pid); int32_t max_out = pid_get_max_out(&mainboard.angle.pid); + uint32_t i_thres = mainboard.distance.bd.i_thres; + int32_t k1 = mainboard.distance.bd.k1; + int32_t k2 = mainboard.distance.bd.k2; + uint16_t cpt_thres = mainboard.distance.bd.cpt_thres; + uint8_t err; + /* go with a lower angle pid, and with a sensible blocking + * detection */ + bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 400000, 20); pid_set_maximums(&mainboard.distance.pid, 0, 20000, 1000); - pid_set_gains(&mainboard.angle.pid, 200, 0, 2000); + pid_set_gains(&mainboard.angle.pid, 500, 10, 4000); trajectory_d_rel(&mainboard.traj, dist); err = wait_traj_end(flags); + if (err != END_BLOCKING) + goto fail; + + /* we detected a blocking, reset bd, remove angle pid and + * continue */ + trajectory_d_rel(&mainboard.traj, dist); + pid_set_maximums(&mainboard.distance.pid, max_in, max_i, 4095); + pid_set_gains(&mainboard.angle.pid, 1, 0, 0); + time_wait_ms(500); + strat_hardstop(); + +#ifdef HOST_VERSION + /* issue with block on simulator */ + if (dist > 0) + trajectory_d_rel(&mainboard.traj, -10); + else + trajectory_d_rel(&mainboard.traj, 10); + wait_traj_end(END_TRAJ); +#endif + + fail: pid_set_gains(&mainboard.angle.pid, p, i, d); pid_set_maximums(&mainboard.distance.pid, max_in, max_i, max_out); + bd_set_current_thresholds(&mainboard.distance.bd, k1, k2, i_thres, cpt_thres); + return err; } @@ -332,6 +386,7 @@ void strat_start(void) strat_exit(); } + /* return true if we have to brake due to an obstacle */ uint8_t strat_obstacle(void) { @@ -339,12 +394,22 @@ uint8_t strat_obstacle(void) int16_t opp_x, opp_y, opp_d, opp_a; /* too slow */ - if (ABS(mainboard.speed_d) < 150) + if (ABS(mainboard.speed_d) < 100) { + if (strat_obstacle_debug != 1) { + DEBUG(E_USER_STRAT, "disable opp, too slow"); + strat_obstacle_debug = 1; + } return 0; + } /* no opponent detected */ - if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a)) + if (get_opponent_xyda(&opp_x, &opp_y, &opp_d, &opp_a) < 0) { + if (strat_obstacle_debug != 2) { + DEBUG(E_USER_STRAT, "no opponent found"); + strat_obstacle_debug = 2; + } return 0; + } /* save obstacle position */ opponent_obstacle.x = opp_x; @@ -352,23 +417,47 @@ uint8_t strat_obstacle(void) opponent_obstacle.d = opp_d; opponent_obstacle.a = opp_a; - if (!is_in_area(opp_x, opp_y, 250)) + if (!is_in_area(opp_x, opp_y, 250)) { + if (strat_obstacle_debug != 3) { + DEBUG(E_USER_STRAT, + "opponent not in area : d=%d, a=%d " + "x=%d y=%d (speed_d=%d)", + opp_d, opp_a, opp_x, opp_y, mainboard.speed_d); + strat_obstacle_debug = 3; + } return 0; + } /* sensor are temporarily disabled */ - if (sensor_obstacle_is_disabled()) + if (sensor_obstacle_is_disabled()) { + if (strat_obstacle_debug != 4) { + DEBUG(E_USER_STRAT, + "sensor are disabled: opponent d=%d, a=%d " + "x=%d y=%d (speed_d=%d)", + opp_d, opp_a, opp_x, opp_y, mainboard.speed_d); + strat_obstacle_debug = 4; + } return 0; + } /* relative position */ x_rel = cos(RAD(opp_a)) * (double)opp_d; y_rel = sin(RAD(opp_a)) * (double)opp_d; /* opponent too far */ - if (opp_d > 500) + if (opp_d > 650) { + if (strat_obstacle_debug != 5) { + DEBUG(E_USER_STRAT, "opponent too far d=%d, a=%d " + "x=%d y=%d (speed_d=%d)", + opp_d, opp_a, opp_x, opp_y, mainboard.speed_d); + strat_obstacle_debug = 5; + } return 0; + } /* opponent is in front of us */ - if (mainboard.speed_d > 0 && (opp_a > 325 || opp_a < 35)) { + if ((mainboard.speed_d >= 0 && opp_d < 600 && (opp_a > 315 || opp_a < 45)) || + (mainboard.speed_d >= 0 && opp_d < 800 && (opp_a > 330 || opp_a < 30))) { DEBUG(E_USER_STRAT, "opponent front d=%d, a=%d " "xrel=%d yrel=%d (speed_d=%d)", opp_d, opp_a, x_rel, y_rel, mainboard.speed_d); @@ -376,13 +465,21 @@ uint8_t strat_obstacle(void) return 1; } /* opponent is behind us */ - if (mainboard.speed_d < 0 && (opp_a < 215 && opp_a > 145)) { + if ((mainboard.speed_d < 0 && opp_d < 600 && (opp_a < 225 && opp_a > 135)) || + (mainboard.speed_d < 0 && opp_d < 800 && (opp_a < 210 && opp_a > 150))) { DEBUG(E_USER_STRAT, "opponent behind d=%d, a=%d xrel=%d yrel=%d", opp_d, opp_a, x_rel, y_rel); sensor_obstacle_disable(); return 1; } + if (strat_obstacle_debug != 6) { + DEBUG(E_USER_STRAT, "opponent not in cone d=%d, a=%d " + "x=%d y=%d (speed_d=%d)", + opp_d, opp_a, opp_x, opp_y, mainboard.speed_d); + strat_obstacle_debug = 6; + } + return 0; } @@ -450,6 +547,12 @@ uint8_t test_traj_end(uint8_t why) return END_OBSTACLE; } +#ifdef HOST_VERSION + if (robotsim_blocking) { + robotsim_blocking = 0; + return END_BLOCKING; + } +#endif return 0; }