From 81c3639247293714de5913f283b786bc73cba491 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 13 May 2010 08:09:14 +0200 Subject: digital/io/src: handle loader blocking in top FSM --- digital/io/src/ai_top_cb.c | 65 ++++++++++++++++++++++++++++++++++++++++++++++ digital/io/src/top.fsm | 16 ++++++++++++ 2 files changed, 81 insertions(+) (limited to 'digital/io') diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index 6e0a0c8d..aa18df9d 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -133,6 +133,20 @@ ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_failed (void) return ai_next (FIRST_GO_END_OF_LINE_FAST, move_fsm_failed); } +/* + * FIRST_GO_END_OF_LINE_FAST =loader_errored=> + * => FIRST_GO_END_OF_LINE_UNBLOCKING + * move backward + * move loader down + */ +fsm_branch_t +ai__FIRST_GO_END_OF_LINE_FAST__loader_errored (void) +{ + asserv_set_speed (BOT_MOVE_SLOW); + ai__FIRST_GO_END_OF_LINE_SLOW__loader_errored (); + return ai_next (FIRST_GO_END_OF_LINE_FAST, loader_errored); +} + /* * FIRST_GO_END_OF_LINE_SLOW =move_fsm_succeed=> * => UNLOAD_LOADER_UP @@ -157,6 +171,57 @@ ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed (void) return ai_next (FIRST_GO_END_OF_LINE_SLOW, move_fsm_failed); } +/* + * FIRST_GO_END_OF_LINE_SLOW =loader_errored=> + * => FIRST_GO_END_OF_LINE_UNBLOCKING + * move backward + * move loader down + */ +fsm_branch_t +ai__FIRST_GO_END_OF_LINE_SLOW__loader_errored (void) +{ + asserv_move_linearly (-90); + loader_down (); + return ai_next (FIRST_GO_END_OF_LINE_SLOW, loader_errored); +} + +/* + * FIRST_GO_END_OF_LINE_UNBLOCKING =bot_move_succeed=> + * => FIRST_GO_END_OF_LINE_SLOW + * retry + */ +fsm_branch_t +ai__FIRST_GO_END_OF_LINE_UNBLOCKING__bot_move_succeed (void) +{ + move_start_noangle (PG_VECT (2625, 253), 0); + return ai_next (FIRST_GO_END_OF_LINE_UNBLOCKING, bot_move_succeed); +} + +/* + * FIRST_GO_END_OF_LINE_UNBLOCKING =bot_move_failed=> + * => FIRST_GO_END_OF_LINE_SLOW + * retry + */ +fsm_branch_t +ai__FIRST_GO_END_OF_LINE_UNBLOCKING__bot_move_failed (void) +{ + ai__FIRST_GO_END_OF_LINE_UNBLOCKING__bot_move_succeed (); + return ai_next (FIRST_GO_END_OF_LINE_UNBLOCKING, bot_move_failed); +} + +/* + * FIRST_GO_END_OF_LINE_UNBLOCKING =loader_errored=> + * => FIRST_GO_END_OF_LINE_UNBLOCKING + * unblock again + */ +fsm_branch_t +ai__FIRST_GO_END_OF_LINE_UNBLOCKING__loader_errored (void) +{ + asserv_move_linearly (-90); + loader_down (); + return ai_next (FIRST_GO_END_OF_LINE_UNBLOCKING, loader_errored); +} + /* * UNLOAD_LOADER_UP =loader_uped=> * => UNLOAD_FACE_BIN diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 9a61d00c..7c7087f5 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -13,6 +13,8 @@ States: first sequence, go to end of line FIRST_GO_END_OF_LINE_SLOW slow down to take elements + FIRST_GO_END_OF_LINE_UNBLOCKING + unblock problem with loader, go backward UNLOAD_LOADER_UP move loader to up position UNLOAD_FACE_BIN @@ -71,12 +73,26 @@ FIRST_GO_END_OF_LINE_FAST: move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW set slow speed retry + loader_errored -> FIRST_GO_END_OF_LINE_UNBLOCKING + move backward + move loader down FIRST_GO_END_OF_LINE_SLOW: move_fsm_succeed -> UNLOAD_LOADER_UP move loader up move_fsm_failed -> FIRST_GO_END_OF_LINE_SLOW retry + loader_errored -> FIRST_GO_END_OF_LINE_UNBLOCKING + move backward + move loader down + +FIRST_GO_END_OF_LINE_UNBLOCKING: + bot_move_succeed -> FIRST_GO_END_OF_LINE_SLOW + retry + bot_move_failed -> FIRST_GO_END_OF_LINE_SLOW + retry + loader_errored -> . + unblock again UNLOAD_LOADER_UP: loader_uped -> UNLOAD_FACE_BIN -- cgit v1.2.3