vt100: include pgmspace.h as we use PROGMEM macro

[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 74dd8d1..8abef5b 100644 (file)
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -70,7 +70,8 @@
 static volatile uint8_t clitoid_slow = 0;
 
 /* return 1 if there is a corn near, and fill the index ptr */
-int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
+int8_t corn_is_near(uint8_t *corn_idx, uint8_t side,
+                   int16_t *xspickle, int16_t *yspickle)
 {
        /* XXX to be checked */
 #define SENSOR_CORN_DIST  225
@@ -92,6 +93,8 @@ int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
        }
        x_corn_int = x_corn;
        y_corn_int = y_corn;
+       *xspickle = x_corn_int;
+       *yspickle = y_corn_int;
 
        wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
        if (wp == NULL)
@@ -162,23 +165,21 @@ static uint8_t clitoid_select_speed(uint8_t num1, uint8_t dir1,
        if (get_cob_count() >= 5)
                return 0; /* fast */
 
-       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);
                return 1;
        }
 
-/*     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 */
+
+       /* we are on intersection, keep the same speed... 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;
+               return clitoid_slow;
 
        /* we can ge fast if it's a 60deg angle and if we checked the
         * current point */
@@ -302,7 +303,7 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
                }
                else {
                        beta_deg = 0;
-                       *pack_spickles = I2C_RIGHT_SIDE;
+                       *pack_spickles = I2C_LEFT_SIDE;
                }
        }
        /* double 90 deg for half turn -- not used */
@@ -315,7 +316,7 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
        }
        /* hard turn, 120 deg */
        else {
-               radius = 120;//75;
+               radius = 75;
                if (diff_a_deg > 0)
                        beta_deg = 0;
                else
@@ -332,7 +333,6 @@ static int8_t strat_calc_clitoid(uint8_t num1, uint8_t dir1,
                strat_set_speed(SPEED_CLITOID_FAST, SPEED_ANGLE_SLOW);
        }
 
-       /* XXX check return value !! */
        ret = trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y,
                                 line1_a_deg, 150., diff_a_deg, beta_deg,
                                 radius, xy_norm(l1.p1.x, l1.p1.y,
@@ -350,8 +350,10 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2,
 
  reprocess:
        ret = strat_calc_clitoid(num1, dir1, num2, dir2, &pack_spickles);
-       if (ret < 0)
+       if (ret < 0) {
                DEBUG(E_USER_STRAT, "clitoid failed");
+               return END_ERROR;
+       }
 
        /* XXX what to do if cobboard is stucked */
 
@@ -360,8 +362,13 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2,
                                                     num2, dir2),
                                    flags);
 
+       /* error during traj, or traj finished */
+       if (err != 0)
+               return err;
+
        /* the speed has to change */
-       if (trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE)
+       if (err == 0 &&
+           trajectory_get_state(&mainboard.traj) != RUNNING_CLITOID_CURVE)
                goto reprocess;
 
        DEBUG(E_USER_STRAT, "clitoid started err=%d pack_spickles=%d",
@@ -379,7 +386,7 @@ uint8_t line2line(uint8_t num1, uint8_t dir1, uint8_t num2,
                err = wait_traj_end(flags);
        }
 
-       DEBUG(E_USER_STRAT, "clitoid finished");
+       DEBUG(E_USER_STRAT, "clitoid finished, err=%d", err);
 
        strat_rpack60 = 0;
        strat_lpack60 = 0;