From 0b7b203b4d6cc0e8efaadee8c1ad051276d0b3c8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 10 May 2010 23:55:18 +0200 Subject: digital/io/src: move clamp all the way up --- digital/io/src/ai_loader_cb.c | 24 +++++++++++++++++------- digital/io/src/loader.fsm | 11 +++++++++-- digital/io/src/main.c | 4 ++++ 3 files changed, 30 insertions(+), 9 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index fa85c4e7..46e9b60c 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -252,7 +252,7 @@ ai__LOADER_LOAD_CLOSING__clamp_succeed (void) - mimot_get_motor1_position (); if (tickness > BOT_CLAMP_EMPTY_STEP) { - asserv_move_motor0_absolute (BOT_ELEVATOR_UNLOAD_STEP, + asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP, BOT_ELEVATOR_SPEED); return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, full); } @@ -265,28 +265,38 @@ ai__LOADER_LOAD_CLOSING__clamp_succeed (void) } /* - * LOADER_LOAD_UPING =elevator_succeed=> + * LOADER_LOAD_UPING =elevator_unload_position=> * => LOADER_LOAD_UNLOADING * open clamp */ fsm_branch_t -ai__LOADER_LOAD_UPING__elevator_succeed (void) +ai__LOADER_LOAD_UPING__elevator_unload_position (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_LOAD_UPING, elevator_succeed); + return ai_next (LOADER_LOAD_UPING, elevator_unload_position); } /* - * LOADER_LOAD_UNLOADING =clamp_succeed=> + * LOADER_LOAD_UNLOADING =elevator_succeed=> + * => LOADER_LOAD_UNLOADING_OPEN + */ +fsm_branch_t +ai__LOADER_LOAD_UNLOADING__elevator_succeed (void) +{ + return ai_next (LOADER_LOAD_UNLOADING, elevator_succeed); +} + +/* + * LOADER_LOAD_UNLOADING_OPEN =clamp_succeed=> * => LOADER_DOWNING * move down */ fsm_branch_t -ai__LOADER_LOAD_UNLOADING__clamp_succeed (void) +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, clamp_succeed); + return ai_next (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed); } /* diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index 4ffafd26..c97941b9 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -32,7 +32,9 @@ States: LOADER_LOAD_UPING move load up to the unload position LOADER_LOAD_UNLOADING - open clamp to unload + open clamp to unload, wait until unload position + LOADER_LOAD_UNLOADING_OPEN + check clamp is open LOADER_LOAD_EMPTY_OPEN open clamp due to no elements to pick @@ -41,6 +43,8 @@ Events: asserv success result elevator_failed asserv failure result + elevator_unload_position + elevator above unloading position clamp_succeed asserv success result, clamp movement cannot fail loader_element @@ -118,10 +122,13 @@ LOADER_LOAD_CLOSING: open clamp LOADER_LOAD_UPING: - elevator_succeed -> LOADER_LOAD_UNLOADING + elevator_unload_position -> LOADER_LOAD_UNLOADING open clamp LOADER_LOAD_UNLOADING: + elevator_succeed -> LOADER_LOAD_UNLOADING_OPEN + +LOADER_LOAD_UNLOADING_OPEN: clamp_succeed -> LOADER_DOWNING move down diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 8ad7c533..fdd98355 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -153,6 +153,10 @@ main_event_to_fsm (void) if (motorm0_status == success && motorm1_status == success) FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed); + /* Check positions. */ + if (asserv_get_motor0_position () > BOT_ELEVATOR_UNLOAD_STEP) + FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_unload_position); + /* Contacts. */ if (!IO_GET (CONTACT_BUMPER0) || !IO_GET (CONTACT_BUMPER1)) FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_loader_element); -- cgit v1.2.3