From 5c206451fabc49b614b09f416b373288f3cfe39d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 10 May 2010 23:54:29 +0200 Subject: digital/io/src: handle empty clamp --- digital/io/src/ai_loader_cb.c | 35 ++++++++++++++++++++++++++++++----- digital/io/src/bot.h | 6 ++++++ digital/io/src/loader.fsm | 11 +++++++++-- 3 files changed, 45 insertions(+), 7 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index a02f8b22..fa85c4e7 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -154,7 +154,7 @@ ai__LOADER_DOWN__loader_up (void) /* * LOADER_DOWN =loader_element=> * => LOADER_LOAD_CLOSING - * close clamp + * clamp */ fsm_branch_t ai__LOADER_DOWN__loader_element (void) @@ -238,15 +238,30 @@ ai__LOADER_ERROR__loader_up (void) /* * LOADER_LOAD_CLOSING =clamp_succeed=> - * => LOADER_LOAD_UPING + * full => LOADER_LOAD_UPING * move up + * empty => LOADER_LOAD_EMPTY_OPEN + * open clamp */ fsm_branch_t ai__LOADER_LOAD_CLOSING__clamp_succeed (void) { - asserv_move_motor0_absolute (BOT_ELEVATOR_UNLOAD_STEP, - BOT_ELEVATOR_SPEED); - return ai_next (LOADER_LOAD_CLOSING, clamp_succeed); + /* Measure load using clamp position. */ + uint16_t tickness = BOT_CLAMP_WIDTH_STEP + - mimot_get_motor0_position () + - mimot_get_motor1_position (); + if (tickness > BOT_CLAMP_EMPTY_STEP) + { + asserv_move_motor0_absolute (BOT_ELEVATOR_UNLOAD_STEP, + BOT_ELEVATOR_SPEED); + return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, full); + } + else + { + 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_branch (LOADER_LOAD_CLOSING, clamp_succeed, empty); + } } /* @@ -274,3 +289,13 @@ ai__LOADER_LOAD_UNLOADING__clamp_succeed (void) return ai_next (LOADER_LOAD_UNLOADING, clamp_succeed); } +/* + * LOADER_LOAD_EMPTY_OPEN =clamp_succeed=> + * => LOADER_DOWN + */ +fsm_branch_t +ai__LOADER_LOAD_EMPTY_OPEN__clamp_succeed (void) +{ + return ai_next (LOADER_LOAD_EMPTY_OPEN, clamp_succeed); +} + diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h index f4dfc00a..fef715de 100644 --- a/digital/io/src/bot.h +++ b/digital/io/src/bot.h @@ -83,6 +83,12 @@ /** Clamp open position in steps. */ #define BOT_CLAMP_OPEN_STEP 15 +/** Clamp width in steps. */ +#define BOT_CLAMP_WIDTH_STEP 4604 + +/** Distance under which the clamp is considered empty. */ +#define BOT_CLAMP_EMPTY_STEP 800 + /** Clamp work speed. */ #define BOT_CLAMP_SPEED 0x60 diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index 747bb51e..4ffafd26 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -33,6 +33,8 @@ States: move load up to the unload position LOADER_LOAD_UNLOADING open clamp to unload + LOADER_LOAD_EMPTY_OPEN + open clamp due to no elements to pick Events: elevator_succeed @@ -89,7 +91,7 @@ LOADER_DOWN: loader_up -> LOADER_UPING move up loader_element -> LOADER_LOAD_CLOSING - close clamp + clamp LOADER_ERROR: loader_down -> LOADER_DOWNING @@ -110,8 +112,10 @@ LOADER_UPING: post loader_errored event LOADER_LOAD_CLOSING: - clamp_succeed -> LOADER_LOAD_UPING + clamp_succeed: full -> LOADER_LOAD_UPING move up + clamp_succeed: empty -> LOADER_LOAD_EMPTY_OPEN + open clamp LOADER_LOAD_UPING: elevator_succeed -> LOADER_LOAD_UNLOADING @@ -121,3 +125,6 @@ LOADER_LOAD_UNLOADING: clamp_succeed -> LOADER_DOWNING move down +LOADER_LOAD_EMPTY_OPEN: + clamp_succeed -> LOADER_DOWN + -- cgit v1.2.3