From 0fb36e5ed136d4a388f5083c18990138b8ceedb5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 29 May 2010 11:25:29 +0200 Subject: digital/io/src: do not count black corns --- digital/io/src/ai_loader_cb.c | 5 +++++ digital/io/src/loader.fsm | 1 + 2 files changed, 6 insertions(+) diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c index e4461948..9b4680a8 100644 --- a/digital/io/src/ai_loader_cb.c +++ b/digital/io/src/ai_loader_cb.c @@ -518,13 +518,18 @@ ai__LOADER_LOAD_UPING__elevator_unload_position (void) * LOADER_LOAD_UPING =elevator_failed=> * => LOADER_ERROR * post loader_errored or loader_black event + * remove one element * open clamp */ fsm_branch_t ai__LOADER_LOAD_UPING__elevator_failed (void) { if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP) + { + if (loader_elements) + loader_elements--; 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); diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm index f2ee3e44..322c99f0 100644 --- a/digital/io/src/loader.fsm +++ b/digital/io/src/loader.fsm @@ -200,6 +200,7 @@ LOADER_LOAD_UPING: open clamp elevator_failed -> LOADER_ERROR post loader_errored or loader_black event + remove one element open clamp LOADER_LOAD_UNLOADING: -- cgit v1.2.3