From c20f7eb8460dd04e42f95d799f17d9b60b5ee8e4 Mon Sep 17 00:00:00 2001 From: zer0 Date: Tue, 11 May 2010 00:02:52 +0200 Subject: [PATCH] wait obstacle -- c nul en fait --- projects/microb2010/mainboard/commands_traj.c | 4 +++- projects/microb2010/mainboard/strat.c | 23 +++++++++++++++++-- projects/microb2010/mainboard/strat.h | 5 +++- projects/microb2010/mainboard/strat_avoid.c | 11 +++++++++ 4 files changed, 39 insertions(+), 4 deletions(-) diff --git a/projects/microb2010/mainboard/commands_traj.c b/projects/microb2010/mainboard/commands_traj.c index 4da3b47..54c0ccc 100644 --- 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 14aaf65..4955bba 100644 --- 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 2fa2e5b..b4592c5 100644 --- 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 3d52188..73f97f7 100644 --- 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; -- 2.20.1