From bc0cafe5feed295ecae01f90ca5a4279e4896f39 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 13 May 2010 07:50:15 +0200 Subject: digital/io/src: add more blocking handling in loader FSM --- digital/io/src/ai_loader_cb.c | 104 +++++++++++++++++++++++++++++++++++++++++- digital/io/src/loader.fsm | 41 +++++++++++++++-- digital/io/src/main.c | 2 + 3 files changed, 142 insertions(+), 5 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index a51d740e..5bd57620 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -248,7 +248,7 @@ ai__LOADER_DOWNING__elevator_failed (void) /* * LOADER_ERROR =loader_down=> - * => LOADER_DOWNING + * => LOADER_ERROR_DOWNING * move down */ fsm_branch_t @@ -260,7 +260,7 @@ ai__LOADER_ERROR__loader_down (void) /* * LOADER_ERROR =loader_up=> - * => LOADER_UPING + * => LOADER_ERROR_UPING * move up */ fsm_branch_t @@ -270,6 +270,82 @@ ai__LOADER_ERROR__loader_up (void) return ai_next (LOADER_ERROR, loader_up); } +/* + * LOADER_ERROR_DOWNING =elevator_succeed=> + * => LOADER_ERROR_DOWNING_OPEN + * release elevator motor + * open clamp + */ +fsm_branch_t +ai__LOADER_ERROR_DOWNING__elevator_succeed (void) +{ + asserv_motor0_free (); + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return ai_next (LOADER_ERROR_DOWNING, elevator_succeed); +} + +/* + * LOADER_ERROR_DOWNING =elevator_failed=> + * => LOADER_ERROR + * post loader_errored event + */ +fsm_branch_t +ai__LOADER_ERROR_DOWNING__elevator_failed (void) +{ + main_post_event (AI_EVENT_loader_errored); + return ai_next (LOADER_ERROR_DOWNING, elevator_failed); +} + +/* + * LOADER_ERROR_DOWNING_OPEN =clamp_succeed=> + * => LOADER_DOWN + * post loader_downed event + */ +fsm_branch_t +ai__LOADER_ERROR_DOWNING_OPEN__clamp_succeed (void) +{ + main_post_event (AI_EVENT_loader_downed); + return ai_next (LOADER_ERROR_DOWNING_OPEN, clamp_succeed); +} + +/* + * LOADER_ERROR_UPING =elevator_succeed=> + * => LOADER_ERROR_UPING_OPEN + * open clamp + */ +fsm_branch_t +ai__LOADER_ERROR_UPING__elevator_succeed (void) +{ + mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED); + return ai_next (LOADER_ERROR_UPING, elevator_succeed); +} + +/* + * LOADER_ERROR_UPING =elevator_failed=> + * => LOADER_ERROR + * post loader_errored event + */ +fsm_branch_t +ai__LOADER_ERROR_UPING__elevator_failed (void) +{ + main_post_event (AI_EVENT_loader_errored); + return ai_next (LOADER_ERROR_UPING, elevator_failed); +} + +/* + * LOADER_ERROR_UPING_OPEN =clamp_succeed=> + * => LOADER_UP + * post loader_uped event + */ +fsm_branch_t +ai__LOADER_ERROR_UPING_OPEN__clamp_succeed (void) +{ + main_post_event (AI_EVENT_loader_uped); + return ai_next (LOADER_ERROR_UPING_OPEN, clamp_succeed); +} + /* * LOADER_LOAD_CLOSING =clamp_succeed=> * full => LOADER_LOAD_UPING @@ -311,6 +387,18 @@ ai__LOADER_LOAD_UPING__elevator_unload_position (void) return ai_next (LOADER_LOAD_UPING, elevator_unload_position); } +/* + * LOADER_LOAD_UPING =elevator_failed=> + * => LOADER_ERROR + * post loader_errored event + */ +fsm_branch_t +ai__LOADER_LOAD_UPING__elevator_failed (void) +{ + main_post_event (AI_EVENT_loader_errored); + return ai_next (LOADER_LOAD_UPING, elevator_failed); +} + /* * LOADER_LOAD_UNLOADING =elevator_succeed=> * => LOADER_LOAD_UNLOADING_OPEN @@ -321,6 +409,18 @@ ai__LOADER_LOAD_UNLOADING__elevator_succeed (void) return ai_next (LOADER_LOAD_UNLOADING, elevator_succeed); } +/* + * LOADER_LOAD_UNLOADING =elevator_failed=> + * => LOADER_ERROR + * post loader_errored event + */ +fsm_branch_t +ai__LOADER_LOAD_UNLOADING__elevator_failed (void) +{ + main_post_event (AI_EVENT_loader_errored); + return ai_next (LOADER_LOAD_UNLOADING, elevator_failed); +} + /* * LOADER_LOAD_UNLOADING_OPEN =clamp_succeed=> * => LOADER_DOWNING diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index dd0a6a9c..3889dd0a 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -33,6 +33,14 @@ States: moving to the down position LOADER_ERROR error while moving up, stop in an unknown state + LOADER_ERROR_DOWNING + error recovery moving down + LOADER_ERROR_DOWNING_OPEN + error recovery moving down, open clamp + LOADER_ERROR_UPING + error recovery moving up + LOADER_ERROR_UPING_OPEN + error recovery moving up, open clamp LOADER_LOAD_CLOSING close clamp LOADER_LOAD_UPING @@ -52,7 +60,9 @@ Events: elevator_unload_position elevator above unloading position clamp_succeed - asserv success result, clamp movement cannot fail + asserv success result + clamp_failed + asserv failure result gate_succeed asserv success result gate_failed @@ -119,11 +129,32 @@ LOADER_DOWN: clamp LOADER_ERROR: - loader_down -> LOADER_DOWNING + loader_down -> LOADER_ERROR_DOWNING move down - loader_up -> LOADER_UPING + loader_up -> LOADER_ERROR_UPING move up +LOADER_ERROR_DOWNING: + elevator_succeed -> LOADER_ERROR_DOWNING_OPEN + release elevator motor + open clamp + elevator_failed -> LOADER_ERROR + post loader_errored event + +LOADER_ERROR_DOWNING_OPEN: + clamp_succeed -> LOADER_DOWN + post loader_downed event + +LOADER_ERROR_UPING: + elevator_succeed -> LOADER_ERROR_UPING_OPEN + open clamp + elevator_failed -> LOADER_ERROR + post loader_errored event + +LOADER_ERROR_UPING_OPEN: + clamp_succeed -> LOADER_UP + post loader_uped event + LOADER_DOWNING: elevator_succeed -> LOADER_DOWN release elevator motor @@ -146,9 +177,13 @@ LOADER_LOAD_CLOSING: LOADER_LOAD_UPING: elevator_unload_position -> LOADER_LOAD_UNLOADING open clamp + elevator_failed -> LOADER_ERROR + post loader_errored event LOADER_LOAD_UNLOADING: elevator_succeed -> LOADER_LOAD_UNLOADING_OPEN + elevator_failed -> LOADER_ERROR + post loader_errored event LOADER_LOAD_UNLOADING_OPEN: clamp_succeed -> LOADER_DOWNING diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 6571e9a4..63de17b1 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -187,6 +187,8 @@ main_event_to_fsm (void) if (motorm0_status == success && motorm1_status == success) FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed); + else if (motorm0_status == failure || motorm1_status == failure) + FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_failed); /* Check positions. */ if (asserv_get_motor0_position () > BOT_ELEVATOR_UNLOAD_STEP) -- cgit v1.2.3