trajectory optimizations

diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py
index b5084072c5622d08bedae101a4840570394b2438..19ff022bd15589dab77dd2eddc8987ce2cd7f639 100644 (file)
--- a/projects/microb2010/mainboard/display.py
+++ b/projects/microb2010/mainboard/display.py
@@ -278,12 +278,14 @@ def set_robot():
     robot.axis = axis
     robot.size = (250, 320, ROBOT_HEIGHT)
 
+    robot_lspickle = 2 # XXX
     lspickle.pos = (tmp_x + (robot_lspickle*60) * math.cos((tmp_a+90)*math.pi/180),
                     tmp_y + (robot_lspickle*60) * math.sin((tmp_a+90)*math.pi/180),
                     ROBOT_HEIGHT/2)
     lspickle.axis = axis
     lspickle.size = (20, 320, 5)
 
+    robot_rspickle = 2 # XXX
     rspickle.pos = (tmp_x + (robot_rspickle*60) * math.cos((tmp_a-90)*math.pi/180),
                     tmp_y + (robot_rspickle*60) * math.sin((tmp_a-90)*math.pi/180),
                     ROBOT_HEIGHT/2)

diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c
index ce765839c9d5bd5a4a70cf05fac50c1b1a4e43a7..eb549c591cd944fa2f84f5dbd273e930047d3698 100755 (executable)
--- a/projects/microb2010/mainboard/main.c
+++ b/projects/microb2010/mainboard/main.c
@@ -298,8 +298,6 @@ int main(void)
        /* strat */
        gen.logs[0] = E_USER_STRAT;
        gen.log_level = 5;
-       strat_db_init();
-       test_strat_avoid();
 
        /* strat-related event */
        scheduler_add_periodical_event_priority(strat_event, NULL,
@@ -315,6 +313,9 @@ int main(void)
 
        sei();
 
+       strat_db_init();
+       test_strat_avoid();
+
        printf_P(PSTR("\r\n"));
        printf_P(PSTR("Respect et robustesse.\r\n"));
 #ifndef HOST_VERSION

diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 7ea7b88ab068b2465b13301ce30c92af4fdb87eb..a97ae1aaf9597e7dfc8a2d49ccd17dc8ed9ec256 100644 (file)
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -70,6 +70,8 @@
 
 static volatile uint8_t strat_running = 0;
 static volatile uint8_t want_pack = 0;
+volatile uint8_t strat_lpack60 = 0;
+volatile uint8_t strat_rpack60 = 0;
 struct strat_conf strat_conf;
 
 /*************************************************************/
@@ -209,46 +211,48 @@ void strat_event(void *dummy)
        }
 
        /* control the cobboard mode for left spickle */
-       if (lcob_near) {
-               if (get_cob_count() >= 5 || want_pack) {
-                       if (strat_db.corn_table[lidx]->corn.color != I2C_COB_WHITE &&
-                           strat_db.corn_table[lidx]->corn.color != I2C_COB_BLACK)
-                               i2c_cobboard_pack(I2C_LEFT_SIDE);
+       if (lcob_near && strat_db.corn_table[lidx]->present) {
+               if (get_cob_count() >= 5 || want_pack || strat_lpack60) {
+                       /* nothing  */
                }
                else {
-                       if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE)
-                               i2c_cobboard_autoharvest(I2C_LEFT_SIDE);
-                       else if (strat_db.corn_table[lidx]->corn.color == I2C_COB_BLACK)
-                               i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
+                       /* deploy spickle and harvest white ones */
+                       if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE) {
+                               i2c_cobboard_autoharvest_nomove(I2C_LEFT_SIDE);
+                               if (cobboard.status == I2C_COBBOARD_STATUS_LBUSY)
+                                       strat_db.corn_table[lidx]->present = 0;
+                       }
                        else
-                               i2c_cobboard_deploy(I2C_LEFT_SIDE);
+                               i2c_cobboard_deploy_nomove(I2C_LEFT_SIDE);
                }
        }
        else {
-               if (get_cob_count() >= 5 || want_pack)
+               /* no cob near us, we can pack or deploy freely */
+               if (get_cob_count() >= 5 || want_pack || strat_lpack60)
                        i2c_cobboard_pack(I2C_LEFT_SIDE);
                else
                        i2c_cobboard_deploy(I2C_LEFT_SIDE);
        }
 
        /* control the cobboard mode for right spickle */
-       if (rcob_near) {
-               if (get_cob_count() >= 5 || want_pack) {
-                       if (strat_db.corn_table[ridx]->corn.color != I2C_COB_WHITE &&
-                           strat_db.corn_table[ridx]->corn.color != I2C_COB_BLACK)
-                               i2c_cobboard_pack(I2C_RIGHT_SIDE);
+       if (rcob_near && strat_db.corn_table[ridx]->present) {
+               if (get_cob_count() >= 5 || want_pack || strat_rpack60) {
+                       /* nothing */
                }
                else {
-                       if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE)
-                               i2c_cobboard_autoharvest(I2C_RIGHT_SIDE);
-                       else if (strat_db.corn_table[ridx]->corn.color == I2C_COB_BLACK)
-                               i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
+                       /* deploy spickle and harvest white ones */
+                       if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE) {
+                               i2c_cobboard_autoharvest_nomove(I2C_RIGHT_SIDE);
+                               if (cobboard.status == I2C_COBBOARD_STATUS_RBUSY)
+                                       strat_db.corn_table[ridx]->present = 0;
+                       }
                        else
-                               i2c_cobboard_deploy(I2C_RIGHT_SIDE);
+                               i2c_cobboard_deploy_nomove(I2C_RIGHT_SIDE);
                }
        }
        else {
-               if (get_cob_count() >= 5 || want_pack)
+               /* no cob near us, we can pack or deploy freely */
+               if (get_cob_count() >= 5 || want_pack || strat_rpack60)
                        i2c_cobboard_pack(I2C_RIGHT_SIDE);
                else
                        i2c_cobboard_deploy(I2C_RIGHT_SIDE);
@@ -264,16 +268,26 @@ static uint8_t strat_harvest(void)
        return 0;
 }
 
+/* must be called from a terminal line */
 static uint8_t strat_eject(void)
 {
        uint8_t err;
-
        //XXX return vals
-       strat_set_speed(600, SPEED_ANGLE_SLOW);
 
-       want_pack = 1;
+       /* go to eject point */
        trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
-       err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+       err = WAIT_COND_OR_TRAJ_END(get_cob_count() >= 5,
+                                   TRAJ_FLAGS_NO_NEAR);
+       if (err == 0) {
+               want_pack = 1;
+               strat_set_speed(600, SPEED_ANGLE_SLOW);
+               err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+       }
+
+       /* pack arms */
+       strat_event_disable();
+       i2c_cobboard_pack(I2C_LEFT_SIDE);
+       i2c_cobboard_pack(I2C_RIGHT_SIDE);
 
        /* ball ejection */
        i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
@@ -284,11 +298,7 @@ static uint8_t strat_eject(void)
        strat_hardstop();
        time_wait_ms(2000);
 
-
        /* half turn */
-       strat_event_disable();
-       i2c_cobboard_pack(I2C_LEFT_SIDE);
-       i2c_cobboard_pack(I2C_RIGHT_SIDE);
        trajectory_a_rel(&mainboard.traj, COLOR_A(180));
        err = wait_traj_end(END_INTR|END_TRAJ);
 
@@ -317,8 +327,8 @@ static uint8_t strat_beginning(void)
        strat_set_speed(250, SPEED_ANGLE_FAST);
 #endif
 
-       // strat_set_speed(600, 60); /* OK */
-       strat_set_speed(250, 28); /* OK */
+       strat_set_speed(600, 60); /* OK */
+       //strat_set_speed(250, 28); /* OK */
 
        trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20));
        err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
@@ -327,7 +337,6 @@ static uint8_t strat_beginning(void)
        strat_set_acc(ACC_DIST, ACC_ANGLE);
        strat_set_speed(250, SPEED_ANGLE_SLOW);
 
-#if 1
  l1:
        DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
        if (get_cob_count() >= 5)
@@ -352,68 +361,14 @@ static uint8_t strat_beginning(void)
                goto l2;
        }
 
-#else
-/*     strat_set_speed(600, SPEED_ANGLE_FAST); */
-/*     err = line2line(0, LINE_UP, 3, LINE_R_DOWN); */
-/*     err = line2line(3, LINE_R_DOWN, 2, LINE_R_UP); */
-/*     err = line2line(2, LINE_R_UP, 2, LINE_R_DOWN); */
-/*     err = line2line(2, LINE_R_DOWN, 3, LINE_R_UP); */
-/*     err = line2line(3, LINE_R_UP, 5, LINE_UP); */
-/*     err = line2line(5, LINE_UP, 2, LINE_L_DOWN); */
-/*     err = line2line(2, LINE_L_DOWN, 1, LINE_L_UP); */
-/*     err = line2line(1, LINE_L_UP, 1, LINE_L_DOWN); */
-/*     err = line2line(1, LINE_L_DOWN, 0, LINE_DOWN); */
-
-       strat_set_speed(300, SPEED_ANGLE_FAST);
-       err = line2line(0, LINE_UP, 0, LINE_R_UP);
-       err = line2line(0, LINE_R_UP, 1, LINE_DOWN);
-       err = line2line(1, LINE_DOWN, 1, LINE_L_DOWN);
-       err = line2line(1, LINE_L_DOWN, 0, LINE_DOWN);
-       wait_ms(500);
-       strat_hardstop();
-       return END_TRAJ;
-#endif
-
        strat_eject();
 
-       strat_set_speed(250, SPEED_ANGLE_FAST);
-
-#if 0
- l4:
-       DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
-       if (get_cob_count() >= 5)
-               strat_set_speed(600, SPEED_ANGLE_FAST);
-
-       err = line2line(5, LINE_DOWN, 2, LINE_L_UP);
-       if (!TRAJ_SUCCESS(err)) {
-               strat_hardstop();
-               time_wait_ms(2000);
-               goto l4;
-       }
-
- l5:
-       DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
-       if (get_cob_count() >= 5)
-               strat_set_speed(600, SPEED_ANGLE_FAST);
-
-       err = line2line(2, LINE_L_UP, 2, LINE_R_UP);
-       if (!TRAJ_SUCCESS(err)) {
-               strat_hardstop();
-               time_wait_ms(2000);
-               goto l5;
+       while (1) {
+               strat_set_speed(250, SPEED_ANGLE_FAST);
+               strat_harvest_circuit();
+               strat_eject();
        }
 
-       DEBUG(E_USER_STRAT, "%s():%d count=%d", __FUNCTION__, __LINE__, get_cob_count());
-       if (get_cob_count() >= 5)
-               strat_set_speed(600, SPEED_ANGLE_FAST);
-#else
-       strat_harvest_circuit();
-#endif
-
-       WAIT_COND_OR_TRAJ_END(distance_from_robot(2625, COLOR_Y(1847)) < 100,
-                             TRAJ_FLAGS_STD);
-       strat_eject();
-
        return END_TRAJ;
 }
 

diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index daf607c5ee4af767b19e5f3f57e2558ebe02277b..3ad10bb5af16b930dc99a1992c019ded00342a0d 100644 (file)
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -151,6 +151,8 @@ struct strat_conf {
 };
 
 extern struct strat_conf strat_conf;
+extern volatile uint8_t strat_lpack60;
+extern volatile uint8_t strat_rpack60;
 
 /* in strat.c */
 void strat_conf_dump(const char *caller);

diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c
index cd54e6c3c2c5f04eace314654e124dd33243c819..ff5dac358e6c4faa2dae367727bc2ef79bb0dd7c 100644 (file)
--- a/projects/microb2010/mainboard/strat_avoid.c
+++ b/projects/microb2010/mainboard/strat_avoid.c
@@ -262,7 +262,7 @@ int8_t get_path(const struct wp_coord *circuit,
        int8_t skipfirst=0;
        int8_t path_len = 0;
 
-       printf("face: %d\n", faceA);
+       printf_P(PSTR("face: %d\r\n"), faceA);
        if ( !faceA && circuit->i == 11 && circuit->j == 6)
                skipfirst=1;
 
@@ -341,7 +341,7 @@ int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved,
                mask <<= 1UL;
        }
 
-       printf("get score: cob %d \n", n);
+       printf_P(PSTR("get score: cob %d \r\n"), n);
        /* score with tomato */
        n = xget_ball_count();
        mask = 1;
@@ -355,7 +355,7 @@ int16_t get_score(uint32_t wcorn_retrieved, uint32_t ucorn_retrieved,
                mask <<= 1UL;
        }
 
-       printf("get score: ball %d \n", n);
+       printf_P(PSTR("get score: ball %d \r\n"), n);
        /* malus for long circuits */
        score -= (len * 20);
 
@@ -481,7 +481,7 @@ static void dump_circuit_wp(struct wp_line *circuit_wpline, int8_t len)
        if (len <= 0)
                return;
        for (i = 0; i < len; i ++) {
-               printf(PSTR("linenum %d dir %d\r\n"), circuit_wpline[i].line_num,
+               printf_P(PSTR("linenum %d dir %d\r\n"), circuit_wpline[i].line_num,
                       circuit_wpline[i].dir);
        }
 
@@ -547,6 +547,7 @@ uint8_t strat_harvest_circuit(void)
 
 void test_strat_avoid(void)
 {
+#if 0
        uint8_t i, j;
        const struct wp_coord *selected_circuit;
        int8_t selected_face;
@@ -554,84 +555,84 @@ void test_strat_avoid(void)
        int8_t ret;
 
        i = 1; j = 1;
-       printf("========= i=%d, j=%d\r\n", i, j);
+       printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
 
        ts = 0; bc = 0; cc = 0;
-       printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+       printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
        browse_circuits(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 0; bc = 3; cc = 0;
-       printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+       printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
        browse_circuits(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 0; bc = 4; cc = 0;
-       printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+       printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
        browse_circuits(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 0; bc = 3; cc = 5;
-       printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+       printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
        browse_circuits(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 0; bc = 4; cc = 5;
-       printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+       printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
        browse_circuits(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 80; bc = 0; cc = 0;
-       printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+       printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
        browse_circuits(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        i = 4; j = 3;
-       printf("========= i=%d, j=%d\r\n", i, j);
+       printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
 
        ts = 0; bc = 0; cc = 0;
-       printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+       printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
        browse_circuits(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 0; bc = 3; cc = 0;
-       printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+       printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
        browse_circuits(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 80; bc = 0; cc = 0;
-       printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+       printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
        browse_circuits(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        i = 11; j = 6;
-       printf("========= i=%d, j=%d\r\n", i, j);
+       printf_P(PSTR("========= i=%d, j=%d\r\n"), i, j);
 
        ts = 0; bc = 0; cc = 0;
-       printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+       printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
        browse_circuits(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 0; bc = 3; cc = 0;
-       printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+       printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
        browse_circuits(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
 
        ts = 80; bc = 0; cc = 0;
-       printf("=== time=%"PRIu32", ball=%d, corn=%d\r\n", ts, bc, cc);
+       printf_P(PSTR("=== time=%"PRIu32", ball=%d, corn=%d\r\n"), ts, bc, cc);
        browse_circuits(i, j, &selected_circuit, &selected_face);
        ret = get_path(selected_circuit, i, j, selected_face, circuit_wpline);
        dump_circuit_wp(circuit_wpline, ret);
-
+#endif
 }

diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index 8f8503ff0d8c2fcedec4be3dc358a4b1864d7f2f..3ec6a2c5aef56a444c71219334e57754267f6b03 100644 (file)
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -98,39 +98,14 @@ int8_t corn_is_near(uint8_t *corn_idx, uint8_t side)
 }
 
 /*
- * - send the correct commands to the spickles
- * - return 1 if we need to stop (cobboard is stucked)
-*/
-static uint8_t handle_spickles(void)
+ * return true if clitoid started
+ */
+static uint8_t clitoid_started(void)
 {
-       return 0;
-#if 0
-       int8_t corn_idx;
-
-       if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE))
-               i2c_cobboard_mode_deploy(I2C_LEFT_SIDE);
-       else {
-               if (corn_table[corn_idx] == TYPE_WHITE_CORN)
-                       i2c_cobboard_mode_harvest(I2C_LEFT_SIDE);
-               else
-                       i2c_cobboard_mode_pack(I2C_LEFT_SIDE);
-       }
-/*     printf("%d %d\n", corn_idx, corn_table[corn_idx]); */
-/*     time_wait_ms(100); */
-
-       if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE))
-               i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE);
-       else {
-               if (corn_table[corn_idx] == TYPE_WHITE_CORN)
-                       i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE);
-               else
-                       i2c_cobboard_mode_pack(I2C_RIGHT_SIDE);
-       }
-
-       return 0;
-#endif
+       return trajectory_get_state(&mainboard.traj) == RUNNING_CLITOID_CURVE;
 }
 
+/* XXX passer les flags de traj */
 uint8_t line2line(uint8_t num1, uint8_t dir1,
                  uint8_t num2, uint8_t dir2)
 {
@@ -141,7 +116,6 @@ uint8_t line2line(uint8_t num1, uint8_t dir1,
        line_t ll1, ll2;
        point_t p;
        uint8_t err;
-       uint16_t a_speed, d_speed;
        int8_t ret;
 
        /* convert to 2 points */
@@ -175,13 +149,15 @@ uint8_t line2line(uint8_t num1, uint8_t dir1,
 /*     printf_P(PSTR("diff_a_deg=%2.2f\r\n"), diff_a_deg_abs); */
 /*     printf_P(PSTR("inter=%2.2f,%2.2f\r\n"), p.x, p.y); */
 
+       /* small angle, 60 deg */
        if (diff_a_deg_abs < 70.) {
-               radius = 200;
+               radius = 150;
                if (diff_a_deg > 0)
-                       beta_deg = 40;
+                       beta_deg = 0;
                else
-                       beta_deg = -40;
+                       beta_deg = 0;
        }
+       /* double 90 deg for half turn -- not used */
        else if (diff_a_deg_abs < 100.) {
                radius = 100;
                if (diff_a_deg > 0)
@@ -189,12 +165,13 @@ uint8_t line2line(uint8_t num1, uint8_t dir1,
                else
                        beta_deg = -40;
        }
+       /* hard turn, 120 deg */
        else {
-               radius = 120;
+               radius = 75;
                if (diff_a_deg > 0)
-                       beta_deg = 60;
+                       beta_deg = 0;
                else
-                       beta_deg = -60;
+                       beta_deg = 0;
        }
 
        /* XXX check return value !! */
@@ -205,22 +182,24 @@ uint8_t line2line(uint8_t num1, uint8_t dir1,
        if (ret < 0)
                DEBUG(E_USER_STRAT, "clitoid failed");
 
-       /* disabled */
-       if (0) {
-               err = 0;
-               while (err == 0) {
-                       err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF);
-                       if (err == 0) {
-                               /* cobboard is stucked */
-                               trajectory_hardstop(&mainboard.traj);
-                               return err; /* XXX do something */
-                       }
-                       err = test_traj_end(0xFF);
-               }
+       /* XXX what to do if cobboard is stucked */
+
+       err = WAIT_COND_OR_TRAJ_END(clitoid_started(), 0xFF);
+       DEBUG(E_USER_STRAT, "clitoid started err=%d diff_a_deg_abs=%2.2f diff_a_deg=%2.2f",
+             err, diff_a_deg_abs, diff_a_deg);
+
+       /* when clitoid starts and angle is 60 deg, pack external
+        * spickle */
+       if (diff_a_deg_abs < 70. && err == 0) {
+               if (diff_a_deg > 0)
+                       strat_rpack60 = 1;
+               else
+                       strat_lpack60 = 1;
        }
+       if (err == 0)
+               err = wait_traj_end(0xFF);
 
-       err = WAIT_COND_OR_TRAJ_END(get_cob_count() == 5, 0xFF);
-       strat_get_speed(&d_speed, &a_speed);
+       DEBUG(E_USER_STRAT, "clitoid finished");
 
        /* XXX 600 -> cste */
        /* XXX does not work, do better */
@@ -228,8 +207,8 @@ uint8_t line2line(uint8_t num1, uint8_t dir1,
 /*         mainboard.traj.state == RUNNING_CLITOID_LINE) */
 /*             strat_set_speed(600, SPEED_ANGLE_FAST); */
 
-       err = wait_traj_end(0xFF);
-
+       strat_rpack60 = 0;
+       strat_lpack60 = 0;
        return err;
 }
 

diff --git a/projects/microb2010/mainboard/strat_utils.h b/projects/microb2010/mainboard/strat_utils.h
index d6bea8f5ba1f35e7dc4065d38a1b6233667f63a8..7bb43a747cbd173ce91d79f3980f84449e3d4227 100644 (file)
--- a/projects/microb2010/mainboard/strat_utils.h
+++ b/projects/microb2010/mainboard/strat_utils.h
@@ -31,7 +31,7 @@ struct xy_point {
        ({                                                              \
                uint8_t __err = 0;                                      \
                while ( (! (cond)) && (__err == 0)) {                   \
-                       __err = test_traj_end(TRAJ_FLAGS_NO_NEAR);      \
+                       __err = test_traj_end(mask);                    \
                }                                                       \
                __err;                                                  \
        })                                                              \