From: zer0 <zer0@carbon.local>
Date: Mon, 10 May 2010 22:02:52 +0000 (+0200)
Subject: wait obstacle -- c nul en fait
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=c20f7eb8460dd04e42f95d799f17d9b60b5ee8e4;p=aversive.git

wait obstacle -- c nul en fait
---

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;