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 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 102 insertions(+), 2 deletions(-) (limited to 'digital/io/src/ai_loader_cb.c') 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 -- cgit v1.2.3