wait obstacle -- c nul en fait

diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c
index 4da3b47259c4232603ec5c59334cf4b348cbf8f8..54c0cccd305a631fa3e4103e05808bd08dc37f7b 100644 (file)
--- a/projects/microb2010/mainboard/commands_traj.c
+++ b/projects/microb2010/mainboard/commands_traj.c
@@ -1058,6 +1058,8 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
 
        if (!strcmp_P(res->arg1, PSTR("our_orange")))
                bit = STRAT_CONF_OUR_ORANGE;
+       else if (!strcmp_P(res->arg1, PSTR("wait_obstacle")))
+               bit = STRAT_CONF_WAIT_OBSTACLE;
 
        if (on)
                strat_conf.flags |= bit;
@@ -1070,7 +1072,7 @@ static void cmd_strat_conf2_parsed(void *parsed_result, void *data)
 
 prog_char str_strat_conf2_arg0[] = "strat_conf";
 parse_pgm_token_string_t cmd_strat_conf2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg0, str_strat_conf2_arg0);
-prog_char str_strat_conf2_arg1[] = "our_orange";
+prog_char str_strat_conf2_arg1[] = "our_orange#wait_obstacle";
 parse_pgm_token_string_t cmd_strat_conf2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg1, str_strat_conf2_arg1);
 prog_char str_strat_conf2_arg2[] = "on#off";
 parse_pgm_token_string_t cmd_strat_conf2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_strat_conf2_result, arg2, str_strat_conf2_arg2);

diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 14aaf652ae8397d3233200c802d3902221c71d86..4955bbaea0e0e6a0efc14b7bd366b5003155cea8 100644 (file)
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -101,6 +101,7 @@ void strat_preinit(void)
                DO_POS | DO_BD | DO_POWER;
 
        strat_db_init();
+       strat_conf.prev_wait_obstacle = -5;
        strat_conf_dump(__FUNCTION__);
        strat_db_dump(__FUNCTION__);
 }
@@ -111,10 +112,12 @@ void strat_conf_dump(const char *caller)
                return;
 
        printf_P(PSTR("-- conf --\r\n"));
-       printf_P(PSTR("opp_orange = %d\n"), strat_conf.opp_orange);
-       printf_P(PSTR("orphan_tomato = %d\n"), strat_conf.orphan_tomato);
        printf_P(PSTR("our_orange = %s\n"),
                 (strat_conf.flags & STRAT_CONF_OUR_ORANGE) ? "y":"n");
+       printf_P(PSTR("wait_obstacle = %s\n"),
+                (strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE) ? "y":"n");
+       printf_P(PSTR("opp_orange = %d\n"), strat_conf.opp_orange);
+       printf_P(PSTR("orphan_tomato = %d\n"), strat_conf.orphan_tomato);
 }
 
 void strat_event_enable(void)
@@ -556,11 +559,27 @@ static uint8_t strat_beginning(uint8_t do_initturn)
        strat_set_acc(ACC_DIST, ACC_ANGLE);
        strat_set_speed(SPEED_CLITOID_SLOW, SPEED_ANGLE_SLOW);
 
+ l1:
        err = line2line(0, LINE_UP, 2, LINE_R_DOWN, TRAJ_FLAGS_NO_NEAR);
+       if (err == END_OBSTACLE &&
+           strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE &&
+           time_get_s() > strat_conf.prev_wait_obstacle + 5) {
+               strat_conf.prev_wait_obstacle = time_get_s();
+               time_wait_ms(2000);
+               goto l1;
+       }
        if (!TRAJ_SUCCESS(err))
                return err;
 
+ l2:
        err = line2line(2, LINE_R_DOWN, 2, LINE_R_UP, TRAJ_FLAGS_NO_NEAR);
+       if (err == END_OBSTACLE &&
+           strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE &&
+           time_get_s() > strat_conf.prev_wait_obstacle + 5) {
+               strat_conf.prev_wait_obstacle = time_get_s();
+               time_wait_ms(2000);
+               goto l2;
+       }
        if (!TRAJ_SUCCESS(err)) {
                return err;
        }

diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index 2fa2e5bacdeebd64ce08cb3488468a5cd463e346..b4592c536d10eb89dd26af5b075322041db7fc96 100644 (file)
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -153,7 +153,10 @@ struct strat_conf {
        uint8_t opp_orange;
        uint8_t orphan_tomato;
 
-#define STRAT_CONF_OUR_ORANGE   0x01
+       int8_t prev_wait_obstacle;
+
+#define STRAT_CONF_OUR_ORANGE      0x01
+#define STRAT_CONF_WAIT_OBSTACLE   0x02
        uint8_t flags;
 };
 

diff --git a/projects/microb2010/mainboard/strat_avoid.c b/projects/microb2010/mainboard/strat_avoid.c
index 3d5218823d2f27a1e69e019bb608776aff8ee027..73f97f7f1e5de0de3398ed2c8f73a9992b22331b 100644 (file)
--- a/projects/microb2010/mainboard/strat_avoid.c
+++ b/projects/microb2010/mainboard/strat_avoid.c
@@ -1060,6 +1060,7 @@ uint8_t strat_harvest_circuit(void)
 
        /* do all lines of circuit */
        for (idx = 1; idx < len; idx ++) {
+       retry:
                linenum = circuit_wpline[idx].line_num;
                dir = circuit_wpline[idx].dir;
 
@@ -1067,6 +1068,16 @@ uint8_t strat_harvest_circuit(void)
                      __FUNCTION__, prev_linenum, prev_dir, linenum, dir);
                err = line2line(prev_linenum, prev_dir, linenum, dir,
                                TRAJ_FLAGS_NO_NEAR);
+
+               /* in some cases it is better to wait that obstacle is
+                * gone before starting to avoid it */
+               if (err == END_OBSTACLE &&
+                   strat_conf.flags & STRAT_CONF_WAIT_OBSTACLE &&
+                   time_get_s() > strat_conf.prev_wait_obstacle + 5) {
+                       strat_conf.prev_wait_obstacle = time_get_s();
+                       time_wait_ms(2000);
+                       goto retry;
+               }
                if (!TRAJ_SUCCESS(err))
                        goto fail;