vt100: include pgmspace.h as we use PROGMEM macro

[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 8716486..90762a6 100644 (file)
--- 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,18 +147,18 @@ 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
@@ -217,7 +221,7 @@ uint8_t strat_calib(int16_t dist, uint8_t flags)
        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(300);
+       time_wait_ms(500);
        strat_hardstop();
 
 #ifdef HOST_VERSION
@@ -382,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)
 {
@@ -389,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;
@@ -402,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);
@@ -426,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;
 }
 
@@ -500,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;
 }