From 7300878edf53acd4a9580125013606997ed652ed Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 14 May 2010 02:06:38 +0200 Subject: digital/io/src: infer blocking when bumpers activated --- digital/io/src/ai_loader_cb.c | 28 ++++++++++++++++++++++++++++ digital/io/src/loader.fsm | 6 ++++++ 2 files changed, 34 insertions(+) diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index 5e9af297..6afa73f8 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -260,6 +260,20 @@ ai__LOADER_DOWNING__elevator_failed (void) return ai_next (LOADER_DOWNING, elevator_failed); } +/* + * LOADER_DOWNING =loader_element=> + * => LOADER_ERROR + * emergency stop + * post loader_errored event + */ +fsm_branch_t +ai__LOADER_DOWNING__loader_element (void) +{ + asserv_motor0_free (); + main_post_event (AI_EVENT_loader_errored); + return ai_next (LOADER_DOWNING, loader_element); +} + /* * LOADER_DOWNING =loader_up=> * => LOADER_UPING @@ -323,6 +337,20 @@ ai__LOADER_ERROR_DOWNING__elevator_failed (void) return ai_next (LOADER_ERROR_DOWNING, elevator_failed); } +/* + * LOADER_ERROR_DOWNING =loader_element=> + * => LOADER_ERROR + * emergency stop + * post loader_errored event + */ +fsm_branch_t +ai__LOADER_ERROR_DOWNING__loader_element (void) +{ + asserv_motor0_free (); + main_post_event (AI_EVENT_loader_errored); + return ai_next (LOADER_ERROR_DOWNING, loader_element); +} + /* * LOADER_ERROR_DOWNING_OPEN =clamp_succeed=> * => LOADER_DOWN diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index e830def9..00366ed5 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -140,6 +140,9 @@ LOADER_ERROR_DOWNING: open clamp elevator_failed -> LOADER_ERROR post loader_errored event + loader_element -> LOADER_ERROR + emergency stop + post loader_errored event LOADER_ERROR_DOWNING_OPEN: clamp_succeed -> LOADER_DOWN @@ -161,6 +164,9 @@ LOADER_DOWNING: post loader_downed event elevator_failed -> LOADER_ERROR post loader_errored event + loader_element -> LOADER_ERROR + emergency stop + post loader_errored event loader_up -> LOADER_UPING move up -- cgit v1.2.3