summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/ai_move_cb.c106
-rw-r--r--digital/io/src/move.fsm31
-rw-r--r--digital/io/src/move.h2
3 files changed, 120 insertions, 19 deletions
diff --git a/digital/io/src/ai_move_cb.c b/digital/io/src/ai_move_cb.c
index 0c54de7c..5fc70653 100644
--- a/digital/io/src/ai_move_cb.c
+++ b/digital/io/src/ai_move_cb.c
@@ -456,11 +456,13 @@ ai__MOVE_WAIT_FOR_CLEAR_PATH__state_timeout (void)
* MOVE_LOADER_UNBLOCKING_UPING =bot_move_succeed=>
* => MOVE_LOADER_UNBLOCKING_DOWNING
* loader down
+ * reset unblocking retry counter
*/
fsm_branch_t
ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_succeed (void)
{
loader_down ();
+ move_data.loader_unblocking_retry = 2;
return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_succeed);
}
@@ -468,44 +470,122 @@ ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_succeed (void)
* MOVE_LOADER_UNBLOCKING_UPING =bot_move_failed=>
* => MOVE_LOADER_UNBLOCKING_DOWNING
* loader down
+ * reset unblocking retry counter
*/
fsm_branch_t
ai__MOVE_LOADER_UNBLOCKING_UPING__bot_move_failed (void)
{
loader_down ();
+ move_data.loader_unblocking_retry = 2;
return ai_next (MOVE_LOADER_UNBLOCKING_UPING, bot_move_failed);
}
/*
* MOVE_LOADER_UNBLOCKING_DOWNING =loader_downed=>
- * rotate => MOVE_ROTATING
- * repeat last rotate
- * move => MOVE_MOVING
- * repeat last move
+ * path_found_rotate => MOVE_ROTATING
+ * rotate towards next position.
+ * path_found => MOVE_MOVING
+ * move to next position.
+ * no_path_found => MOVE_IDLE
+ * post failure.
*/
fsm_branch_t
ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_downed (void)
{
- if (move_go_or_rotate (move_data.step, move_data.step_angle,
- move_data.step_with_angle,
- move_data.step_backward) == 2)
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, rotate);
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found_rotate);
+ else
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, path_found);
+ }
else
- return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, move);
+ {
+ main_post_event (AI_EVENT_move_fsm_failed);
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_downed, no_path_found);
+ }
}
/*
* MOVE_LOADER_UNBLOCKING_DOWNING =loader_errored=>
- * => MOVE_LOADER_UNBLOCKING_UPING
+ * tryagain => MOVE_LOADER_UNBLOCKING_UPING
* move backward
* loader up
+ * tryout_path_found_rotate => MOVE_ROTATING
+ * rotate towards next position.
+ * tryout_path_found => MOVE_ROTATING
+ * move to next position.
+ * tryout_no_path_found => MOVE_IDLE
+ * post failure.
*/
fsm_branch_t
ai__MOVE_LOADER_UNBLOCKING_DOWNING__loader_errored (void)
{
- asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
- loader_up ();
- return ai_next (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored);
+ if (--move_data.loader_unblocking_retry)
+ {
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryagain);
+ }
+ else
+ {
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found_rotate);
+ else
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_path_found);
+ }
+ else
+ {
+ main_post_event (AI_EVENT_move_fsm_failed);
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, loader_errored, tryout_no_path_found);
+ }
+ }
+}
+
+/*
+ * MOVE_LOADER_UNBLOCKING_DOWNING =state_timeout=>
+ * tryagain => MOVE_LOADER_UNBLOCKING_UPING
+ * move backward
+ * loader up
+ * tryout_path_found_rotate => MOVE_ROTATING
+ * rotate towards next position.
+ * tryout_path_found => MOVE_ROTATING
+ * move to next position.
+ * tryout_no_path_found => MOVE_IDLE
+ * post failure.
+ */
+fsm_branch_t
+ai__MOVE_LOADER_UNBLOCKING_DOWNING__state_timeout (void)
+{
+ if (--move_data.loader_unblocking_retry)
+ {
+ asserv_move_linearly (-MOVE_LOADER_UNBLOCKING_DISTANCE);
+ loader_up ();
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryagain);
+ }
+ else
+ {
+ /* Try to move. */
+ uint8_t next = move_path_init ();
+ if (next)
+ {
+ if (next == 2)
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_path_found_rotate);
+ else
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_path_found);
+ }
+ else
+ {
+ main_post_event (AI_EVENT_move_fsm_failed);
+ return ai_next_branch (MOVE_LOADER_UNBLOCKING_DOWNING, state_timeout, tryout_no_path_found);
+ }
+ }
}
diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm
index abedd8dd..fad6b6b7 100644
--- a/digital/io/src/move.fsm
+++ b/digital/io/src/move.fsm
@@ -18,7 +18,7 @@ States:
waiting for obstacle to disappear.
MOVE_LOADER_UNBLOCKING_UPING
moving loader up and move backward to unblock loader.
- MOVE_LOADER_UNBLOCKING_DOWNING
+ MOVE_LOADER_UNBLOCKING_DOWNING [timeout=450]
moving loader down.
Events:
@@ -93,15 +93,34 @@ MOVE_WAIT_FOR_CLEAR_PATH:
MOVE_LOADER_UNBLOCKING_UPING:
bot_move_succeed -> MOVE_LOADER_UNBLOCKING_DOWNING
loader down
+ reset unblocking retry counter
bot_move_failed -> MOVE_LOADER_UNBLOCKING_DOWNING
loader down
+ reset unblocking retry counter
MOVE_LOADER_UNBLOCKING_DOWNING:
- loader_downed: rotate -> MOVE_ROTATING
- repeat last rotate
- loader_downed: move -> MOVE_MOVING
- repeat last move
- loader_errored -> MOVE_LOADER_UNBLOCKING_UPING
+ loader_downed: path_found_rotate -> MOVE_ROTATING
+ rotate towards next position.
+ loader_downed: path_found -> MOVE_MOVING
+ move to next position.
+ loader_downed: no_path_found -> MOVE_IDLE
+ post failure.
+ loader_errored: tryagain -> MOVE_LOADER_UNBLOCKING_UPING
+ move backward
+ loader up
+ loader_errored: tryout_path_found_rotate -> MOVE_ROTATING
+ rotate towards next position.
+ loader_errored: tryout_path_found -> MOVE_ROTATING
+ move to next position.
+ loader_errored: tryout_no_path_found -> MOVE_IDLE
+ post failure.
+ state_timeout: tryagain -> MOVE_LOADER_UNBLOCKING_UPING
move backward
loader up
+ state_timeout: tryout_path_found_rotate -> MOVE_ROTATING
+ rotate towards next position.
+ state_timeout: tryout_path_found -> MOVE_ROTATING
+ move to next position.
+ state_timeout: tryout_no_path_found -> MOVE_IDLE
+ post failure.
diff --git a/digital/io/src/move.h b/digital/io/src/move.h
index 7f860802..6078a1e3 100644
--- a/digital/io/src/move.h
+++ b/digital/io/src/move.h
@@ -68,6 +68,8 @@ struct move_data_t
uint8_t final_move;
/** Distance to remove from path. */
int16_t shorten;
+ /** Loader unblocking retry counter. */
+ uint8_t loader_unblocking_retry;
};
/**