beacon from 2009

[aversive.git] / projects / microb2010 / mainboard / strat_base.c

diff --git a/projects/microb2010/mainboard/strat_base.c b/projects/microb2010/mainboard/strat_base.c
index acded68..023db36 100644 (file)
--- a/projects/microb2010/mainboard/strat_base.c
+++ b/projects/microb2010/mainboard/strat_base.c
@@ -143,19 +143,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 +195,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;
 }
 
@@ -364,11 +414,12 @@ uint8_t strat_obstacle(void)
        y_rel = sin(RAD(opp_a)) * (double)opp_d;
 
        /* opponent too far */
-       if (opp_d > 500)
+       if (opp_d > 650)
                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 < 500 && (opp_a > 325 || opp_a < 35)) &&
+           (mainboard.speed_d > 0 && opp_d < 650 && (opp_a > 340 || opp_a < 20))) {
                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,7 +427,8 @@ 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 < 500 && (opp_a < 215 && opp_a > 145)) &&
+           (mainboard.speed_d < 0 && opp_d < 650 && (opp_a < 200 && opp_a > 160))) {
                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();