summaryrefslogtreecommitdiffhomepage
path: root/digital/io
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/ai_move_cb.c39
-rw-r--r--digital/io/src/move.c4
-rw-r--r--digital/io/src/move.fsm13
3 files changed, 40 insertions, 16 deletions
diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c
index e250d010..0c54de7c 100644
--- a/digital/io/src/ai_move_cb.c
+++ b/digital/io/src/ai_move_cb.c
@@ -146,8 +146,6 @@ move_go_to_next (vect_t dst)
move_data.backward_movement_allowed);
}
TRACE (TRACE_MOVE__GO_TO, dst.x, dst.y);
- /* Reset try counter. */
- move_data.try_again_counter = 3;
/* Next time, do not use slow. */
move_data.slow = 0;
return r;
@@ -322,16 +320,25 @@ ai__MOVE_MOVING__bot_move_failed (void)
/*
* MOVE_MOVING =obstacle_in_front=>
- * => MOVE_WAIT_FOR_CLEAR_PATH
+ * tryagain => MOVE_WAIT_FOR_CLEAR_PATH
* reset final_move.
* stop the bot.
+ * tryout => MOVE_IDLE
+ * stop the bot.
+ * post failure event.
*/
fsm_branch_t
ai__MOVE_MOVING__obstacle_in_front (void)
{
move_data.final_move = 0;
asserv_stop_motor ();
- return ai_next (MOVE_MOVING, obstacle_in_front);
+ if (--move_data.try_again_counter == 0)
+ {
+ main_post_event (AI_EVENT_move_fsm_failed);
+ return ai_next_branch (MOVE_MOVING, obstacle_in_front, tryout);
+ }
+ else
+ return ai_next_branch (MOVE_MOVING, obstacle_in_front, tryagain);
}
/*
@@ -381,8 +388,10 @@ ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_succeed (void)
* rotate towards next position.
* path_found => MOVE_MOVING
* move to next position.
- * no_path_found => MOVE_WAIT_FOR_CLEAR_PATH
+ * no_path_found_tryagain => MOVE_WAIT_FOR_CLEAR_PATH
* nothing to do.
+ * no_path_found_tryout => MOVE_IDLE
+ * post failure event.
*/
fsm_branch_t
ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void)
@@ -396,7 +405,15 @@ ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void)
return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, path_found);
}
else
- return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found);
+ {
+ if (--move_data.try_again_counter == 0)
+ {
+ main_post_event (AI_EVENT_move_fsm_failed);
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryout);
+ }
+ else
+ return ai_next_branch (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, bot_move_failed, no_path_found_tryagain);
+ }
}
/*
@@ -405,9 +422,9 @@ ai__MOVE_MOVING_BACKWARD_TO_TURN_FREELY__bot_move_failed (void)
* rotate towards next position.
* path_found => MOVE_MOVING
* move to next position.
- * no_path_found_and_try_again => MOVE_WAIT_FOR_CLEAR_PATH
+ * no_path_found_tryagain => MOVE_WAIT_FOR_CLEAR_PATH
* decrement counter.
- * no_path_found_and_no_try_again => MOVE_IDLE
+ * no_path_found_tryout => MOVE_IDLE
* post failure.
*/
fsm_branch_t
@@ -428,12 +445,10 @@ ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void)
if (--move_data.try_again_counter == 0)
{
main_post_event (AI_EVENT_move_fsm_failed);
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout,
- no_path_found_and_no_try_again);
+ return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_tryout);
}
else
- return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout,
- no_path_found_and_try_again);
+ return ai_next_branch (MOVE_WAIT_FOR_CLEAR_PATH, state_timeout, no_path_found_tryagain);
}
}
diff --git a/digital/io/src/move.c b/digital/io/src/move.c
index 27fc93fb..e4f59c39 100644
--- a/digital/io/src/move.c
+++ b/digital/io/src/move.c
@@ -47,6 +47,8 @@ move_start (position_t position, uint8_t backward)
move_data.backward_movement_allowed = backward;
move_data.final_move = 0;
move_data.shorten = 0;
+ /* Reset try counter. */
+ move_data.try_again_counter = 3;
/* Start the FSM. */
fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
}
@@ -60,6 +62,8 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten)
move_data.backward_movement_allowed = backward;
move_data.final_move = 0;
move_data.shorten = shorten;
+ /* Reset try counter. */
+ move_data.try_again_counter = 3;
/* Start the FSM. */
fsm_handle_event (&ai_fsm, AI_EVENT_move_start);
}
diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm
index 2812f686..abedd8dd 100644
--- a/digital/io/src/move.fsm
+++ b/digital/io/src/move.fsm
@@ -54,9 +54,12 @@ MOVE_MOVING:
bot_move_failed -> MOVE_MOVING_BACKWARD_TO_TURN_FREELY
reset final_move.
move backward to turn freely.
- obstacle_in_front -> MOVE_WAIT_FOR_CLEAR_PATH
+ obstacle_in_front: tryagain -> MOVE_WAIT_FOR_CLEAR_PATH
reset final_move.
stop the bot.
+ obstacle_in_front: tryout -> MOVE_IDLE
+ stop the bot.
+ post failure event.
loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
move backward
loader up
@@ -72,17 +75,19 @@ MOVE_MOVING_BACKWARD_TO_TURN_FREELY:
rotate towards next position.
bot_move_failed: path_found -> MOVE_MOVING
move to next position.
- bot_move_failed: no_path_found -> MOVE_WAIT_FOR_CLEAR_PATH
+ bot_move_failed: no_path_found_tryagain -> MOVE_WAIT_FOR_CLEAR_PATH
nothing to do.
+ bot_move_failed: no_path_found_tryout -> MOVE_IDLE
+ post failure event.
MOVE_WAIT_FOR_CLEAR_PATH:
state_timeout: path_found_rotate -> MOVE_ROTATING
rotate towards next position.
state_timeout: path_found -> MOVE_MOVING
move to next position.
- state_timeout: no_path_found_and_try_again -> .
+ state_timeout: no_path_found_tryagain -> .
decrement counter.
- state_timeout: no_path_found_and_no_try_again -> MOVE_IDLE
+ state_timeout: no_path_found_tryout -> MOVE_IDLE
post failure.
MOVE_LOADER_UNBLOCKING_UPING: