From 323ad8abc26d3fe2f89c07a36288469ad6a4139f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 14 May 2010 12:25:27 +0200 Subject: digital/io/src: handle black corn --- digital/io/src/ai_loader_cb.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 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 632b7504..b556c9eb 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -449,13 +449,16 @@ ai__LOADER_LOAD_UPING__elevator_unload_position (void) /* * LOADER_LOAD_UPING =elevator_failed=> * => LOADER_ERROR - * post loader_errored event + * post loader_errored or loader_black event * open clamp */ fsm_branch_t ai__LOADER_LOAD_UPING__elevator_failed (void) { - main_post_event (AI_EVENT_loader_errored); + if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP) + main_post_event (AI_EVENT_loader_black); + else + main_post_event (AI_EVENT_loader_errored); 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_failed); -- cgit v1.2.3