fix display and support beacon in robotsim

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

diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index 1f5e1d3..74dd8d1 100644 (file)
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -168,15 +168,23 @@ static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
                return 1;
        }
 
-/*     if (time_get_s() > 39) */
-/*             DEBUG(E_USER_STRAT, "i,j = (%d %d), count=%d", i, j, corn_count_neigh(i, j)); */
+/*     if (time_get_s() > 32) */
+/*             DEBUG(E_USER_STRAT, "i,j = (%d %d), count=%d", i, j, */
+/*                   corn_count_neigh(i, j)); */
 
        if (corn_count_neigh(i, j) == 2)
                return 1;
 
+       /* we are on intersection, let's go slow... but as we enter in
+        * the curve-part of the clitoid, we should not go there */
        if (wp_belongs_to_line(i, j, num2, dir2))
                return 0;
 
+       /* we can ge fast if it's a 60deg angle and if we checked the
+        * current point */
+       if (is_60deg(dir1, dir2))
+               return 0;
+
        /* get next point */
        if (wp_get_neigh(i, j, &i2, &j2, dir1) < 0) {
                DEBUG(E_USER_STRAT, "%s(): cannot get neigh1",
@@ -184,7 +192,7 @@ static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
                return 1;
        }
 
-       /* if (i2, j2) belongs to next line, check corn in opposition */
+       /* if (i2, j2) belongs to next line, check corns */
        if (wp_belongs_to_line(i2, j2, num2, dir2)) {
                if (corn_count_neigh(i2, j2) > 0)
                        return 1;
@@ -199,7 +207,7 @@ static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
                return 1;
        }
 
-       /* if (i3, j3) belongs to next line, check corn in opposition */
+       /* if (i3, j3) belongs to next line, check corns */
        if (wp_belongs_to_line(i3, j3, num2, dir2)) {
                if (corn_count_neigh(i2, j2) > 0 ||
                    corn_count_neigh(i3, j3) > 0)
@@ -307,7 +315,7 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
        }
        /* hard turn, 120 deg */
        else {
-               radius = 75;
+               radius = 120;//75;
                if (diff_a_deg > 0)
                        beta_deg = 0;
                else
@@ -316,11 +324,11 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
 
        clitoid_slow = clitoid_select_speed(num1, dir1, num2, dir2);
        if (clitoid_slow) {
-               DEBUG(E_USER_STRAT, "slow clito\n");
+               DEBUG(E_USER_STRAT, "slow clito");
                strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
        }
        else {
-               DEBUG(E_USER_STRAT, "fast clito\n");
+               DEBUG(E_USER_STRAT, "fast clito");
                strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
        }
 
@@ -373,12 +381,6 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2,
 
        DEBUG(E_USER_STRAT, "clitoid finished");
 
-       /* XXX 600 -> cste */
-       /* XXX does not work, do better */
-/*     if (err == 0 && d_speed < 600 && */
-/*         mainboard.traj.state == RUNNING_CLITOID_LINE) */
-/*             strat_set_speed(600, SPEED_ANGLE_FAST); */
-
        strat_rpack60 = 0;
        strat_lpack60 = 0;
        return err;