X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fstrat_avoid.c;h=73f97f7f1e5de0de3398ed2c8f73a9992b22331b;hp=3d5218823d2f27a1e69e019bb608776aff8ee027;hb=c20f7eb8460dd04e42f95d799f17d9b60b5ee8e4;hpb=90bef45081332f3780e26bf9f838b668df99fb26

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;