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 +++++++++++++++++++++++++++++++++++++++---- 1 file changed, 50 insertions(+), 5 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 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); } -- cgit v1.2.3