From f3d54d8e62fe76b78bcefd707182408324d25ab5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 13 May 2010 09:15:08 +0200 Subject: digital/io/src: unblocking tuning --- digital/io/src/ai_loader_cb.c | 6 +++--- digital/io/src/ai_top_cb.c | 25 ++++++++++++++++++------- digital/io/src/loader.fsm | 4 ++-- digital/io/src/top.fsm | 16 +++++++++++----- 4 files changed, 34 insertions(+), 17 deletions(-) (limited to 'digital') diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index 5bd57620..18d60c74 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -236,13 +236,13 @@ ai__LOADER_DOWNING__elevator_succeed (void) /* * LOADER_DOWNING =elevator_failed=> - * => LOADER_UPING - * something is blocking, move it up + * => LOADER_ERROR + * post loader_errored event */ fsm_branch_t ai__LOADER_DOWNING__elevator_failed (void) { - asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); + main_post_event (AI_EVENT_loader_errored); return ai_next (LOADER_DOWNING, elevator_failed); } diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index aa18df9d..2104c5c7 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -135,9 +135,8 @@ ai__FIRST_GO_END_OF_LINE_FAST__move_fsm_failed (void) /* * FIRST_GO_END_OF_LINE_FAST =loader_errored=> - * => FIRST_GO_END_OF_LINE_UNBLOCKING - * move backward - * move loader down + * => FIRST_GO_END_OF_LINE_UNBLOCKING_UP + * same as below */ fsm_branch_t ai__FIRST_GO_END_OF_LINE_FAST__loader_errored (void) @@ -173,33 +172,46 @@ ai__FIRST_GO_END_OF_LINE_SLOW__move_fsm_failed (void) /* * FIRST_GO_END_OF_LINE_SLOW =loader_errored=> - * => FIRST_GO_END_OF_LINE_UNBLOCKING + * => FIRST_GO_END_OF_LINE_UNBLOCKING_UP * move backward - * move loader down + * loader up */ fsm_branch_t ai__FIRST_GO_END_OF_LINE_SLOW__loader_errored (void) { asserv_move_linearly (-90); - loader_down (); + loader_up (); return ai_next (FIRST_GO_END_OF_LINE_SLOW, loader_errored); } +/* + * FIRST_GO_END_OF_LINE_UNBLOCKING_UP =loader_uped=> + * => FIRST_GO_END_OF_LINE_UNBLOCKING + */ +fsm_branch_t +ai__FIRST_GO_END_OF_LINE_UNBLOCKING_UP__loader_uped (void) +{ + return ai_next (FIRST_GO_END_OF_LINE_UNBLOCKING_UP, loader_uped); +} + /* * FIRST_GO_END_OF_LINE_UNBLOCKING =bot_move_succeed=> * => FIRST_GO_END_OF_LINE_SLOW + * move loader down * retry */ fsm_branch_t ai__FIRST_GO_END_OF_LINE_UNBLOCKING__bot_move_succeed (void) { move_start_noangle (PG_VECT (2625, 253), 0); + loader_down (); 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 + * move loader down * retry */ fsm_branch_t @@ -218,7 +230,6 @@ 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); } diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index 3889dd0a..cc636a97 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -159,8 +159,8 @@ LOADER_DOWNING: elevator_succeed -> LOADER_DOWN release elevator motor post loader_downed event - elevator_failed -> LOADER_UPING - something is blocking, move it up + elevator_failed -> LOADER_ERROR + post loader_errored event LOADER_UPING: elevator_succeed -> LOADER_UP diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 7c7087f5..9a6b176c 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_UP + unblock problem with loader, move loader up FIRST_GO_END_OF_LINE_UNBLOCKING unblock problem with loader, go backward UNLOAD_LOADER_UP @@ -73,23 +75,27 @@ 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 + loader_errored -> FIRST_GO_END_OF_LINE_UNBLOCKING_UP + same as below 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 + loader_errored -> FIRST_GO_END_OF_LINE_UNBLOCKING_UP move backward - move loader down + loader up + +FIRST_GO_END_OF_LINE_UNBLOCKING_UP: + loader_uped -> FIRST_GO_END_OF_LINE_UNBLOCKING FIRST_GO_END_OF_LINE_UNBLOCKING: bot_move_succeed -> FIRST_GO_END_OF_LINE_SLOW + move loader down retry bot_move_failed -> FIRST_GO_END_OF_LINE_SLOW + move loader down retry loader_errored -> . unblock again -- cgit v1.2.3