From 6ca539dd06a20f44d6a01d450668819c7c89d201 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 14 May 2010 01:44:16 +0200 Subject: digital/io/src: handle loader state change in any state --- digital/io/src/ai_loader_cb.c | 55 +++++++++++++++++++++++++++++++++++++++---- digital/io/src/loader.c | 3 +++ digital/io/src/loader.fsm | 12 ++++++++-- digital/io/src/loader.h | 3 +++ 4 files changed, 66 insertions(+), 7 deletions(-) (limited to 'digital') diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index f486bbc6..5e9af297 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -222,6 +222,18 @@ ai__LOADER_UPING__elevator_failed (void) return ai_next (LOADER_UPING, elevator_failed); } +/* + * LOADER_UPING =loader_down=> + * => LOADER_DOWNING + * move down + */ +fsm_branch_t +ai__LOADER_UPING__loader_down (void) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); + return ai_next (LOADER_UPING, loader_down); +} + /* * LOADER_DOWNING =elevator_succeed=> * => LOADER_DOWN @@ -248,6 +260,18 @@ ai__LOADER_DOWNING__elevator_failed (void) return ai_next (LOADER_DOWNING, elevator_failed); } +/* + * LOADER_DOWNING =loader_up=> + * => LOADER_UPING + * move up + */ +fsm_branch_t +ai__LOADER_DOWNING__loader_up (void) +{ + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED); + return ai_next (LOADER_DOWNING, loader_up); +} + /* * LOADER_ERROR =loader_down=> * => LOADER_ERROR_DOWNING @@ -430,23 +454,44 @@ ai__LOADER_LOAD_UNLOADING__elevator_failed (void) /* * LOADER_LOAD_UNLOADING_OPEN =clamp_succeed=> - * => LOADER_DOWNING + * down => LOADER_DOWNING * move down + * up => LOADER_UPING + * move up */ fsm_branch_t ai__LOADER_LOAD_UNLOADING_OPEN__clamp_succeed (void) { - asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED); - return ai_next (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed); + if (loader_want_up) + { + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, + BOT_ELEVATOR_SPEED); + return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, up); + } + else + { + asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, + BOT_ELEVATOR_SPEED); + return ai_next_branch (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed, down); + } } /* * LOADER_LOAD_EMPTY_OPEN =clamp_succeed=> - * => LOADER_DOWN + * down => LOADER_DOWN + * up => LOADER_UPING + * move up */ fsm_branch_t ai__LOADER_LOAD_EMPTY_OPEN__clamp_succeed (void) { - return ai_next (LOADER_LOAD_EMPTY_OPEN, clamp_succeed); + if (loader_want_up) + { + asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, + BOT_ELEVATOR_SPEED); + return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, up); + } + else + return ai_next_branch (LOADER_LOAD_EMPTY_OPEN, clamp_succeed, down); } diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c index 179cf3b8..4157023e 100644 --- a/digital/io/src/loader.c +++ b/digital/io/src/loader.c @@ -28,16 +28,19 @@ #include "fsm.h" uint8_t loader_elements; +uint8_t loader_want_up; void loader_up (void) { + loader_want_up = 1; fsm_handle_event (&ai_fsm, AI_EVENT_loader_up); } void loader_down (void) { + loader_want_up = 0; fsm_handle_event (&ai_fsm, AI_EVENT_loader_down); } diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index 92c0d839..e830def9 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -161,12 +161,16 @@ LOADER_DOWNING: post loader_downed event elevator_failed -> LOADER_ERROR post loader_errored event + loader_up -> LOADER_UPING + move up LOADER_UPING: elevator_succeed -> LOADER_UP post loader_uped event elevator_failed -> LOADER_ERROR post loader_errored event + loader_down -> LOADER_DOWNING + move down LOADER_LOAD_CLOSING: clamp_succeed: full -> LOADER_LOAD_UPING @@ -187,9 +191,13 @@ LOADER_LOAD_UNLOADING: post loader_errored event LOADER_LOAD_UNLOADING_OPEN: - clamp_succeed -> LOADER_DOWNING + clamp_succeed: down -> LOADER_DOWNING move down + clamp_succeed: up -> LOADER_UPING + move up LOADER_LOAD_EMPTY_OPEN: - clamp_succeed -> LOADER_DOWN + clamp_succeed: down -> LOADER_DOWN + clamp_succeed: up -> LOADER_UPING + move up diff --git a/digital/io/src/loader.h b/digital/io/src/loader.h index b8150891..0021a647 100644 --- a/digital/io/src/loader.h +++ b/digital/io/src/loader.h @@ -28,6 +28,9 @@ /** Number of loaded elements. */ extern uint8_t loader_elements; +/** Loader up requested. */ +extern uint8_t loader_want_up; + /** Move loader up, in a rest position. */ void loader_up (void); -- cgit v1.2.3