circuit strat, first version

[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 5224787..8f8503f 100644 (file)
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -66,10 +66,10 @@
 #include "sensor.h"
 #include "actuator.h"
 
 #include "sensor.h"
 #include "actuator.h"
 

-#if 0

 /* return 1 if there is a corn near, and fill the index ptr */
 /* return 1 if there is a corn near, and fill the index ptr */

-uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
+int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)

 {
 {

+       /* XXX to be checked */

 #define SENSOR_CORN_DIST  225
 #define SENSOR_CORN_ANGLE 90
        double x = position_get_x_double(&mainboard.pos);
 #define SENSOR_CORN_DIST  225
 #define SENSOR_CORN_ANGLE 90
        double x = position_get_x_double(&mainboard.pos);


@@ -91,11 +91,11 @@ uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
        y_corn_int = y_corn;
 
        wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
        y_corn_int = y_corn;
 
        wp = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);

-       if (wp->corn.idx < 0)
+       if (wp == NULL)

                return 0;
                return 0;

+       *corn_idx = wp->corn.idx;

        return 1;
 }
        return 1;
 }

-#endif

 
 /*
  * - send the correct commands to the spickles
 
 /*
  * - send the correct commands to the spickles


@@ -131,8 +131,8 @@ static uint8_t handle_spickles(void)
 #endif
 }
 
 #endif
 }
 

-uint8_t line2line(uint8_t dir1, uint8_t num1,
-                 uint8_t dir2, uint8_t num2)
+uint8_t line2line(uint8_t num1, uint8_t dir1,
+                 uint8_t num2, uint8_t dir2)

 {
        double line1_a_rad, line1_a_deg, line2_a_rad;
        double diff_a_deg, diff_a_deg_abs, beta_deg;
 {
        double line1_a_rad, line1_a_deg, line2_a_rad;
        double diff_a_deg, diff_a_deg_abs, beta_deg;


@@ -142,6 +142,7 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
        point_t p;
        uint8_t err;
        uint16_t a_speed, d_speed;
        point_t p;
        uint8_t err;
        uint16_t a_speed, d_speed;

+       int8_t ret;

 
        /* convert to 2 points */
        num2line(&l1, dir1, num1);
 
        /* convert to 2 points */
        num2line(&l1, dir1, num1);


@@ -197,10 +198,13 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
        }
 
        /* XXX check return value !! */
        }
 
        /* XXX check return value !! */

-       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,
-                                          p.x, p.y));
+       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,
+                                                p.x, p.y));
+       if (ret < 0)
+               DEBUG(E_USER_STRAT, "clitoid failed");
+

        /* disabled */
        if (0) {
                err = 0;
        /* disabled */
        if (0) {
                err = 0;


@@ -219,9 +223,11 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
        strat_get_speed(&d_speed, &a_speed);
 
        /* XXX 600 -> cste */
        strat_get_speed(&d_speed, &a_speed);
 
        /* XXX 600 -> cste */

-       if (err == 0 && d_speed < 600 &&
-           mainboard.traj.state == RUNNING_CLITOID_LINE)
-               strat_set_speed(600, SPEED_ANGLE_FAST);
+       /* 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); */
+

        err = wait_traj_end(0xFF);
 
        return err;
        err = wait_traj_end(0xFF);
 
        return err;