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