From d51d7ef789a776741cb4712646c47c7f37b005c2 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 13 May 2010 14:02:55 +0200 Subject: digital/io/src: count elements to unload --- digital/io/src/ai_top_cb.c | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) (limited to 'digital/io/src/ai_top_cb.c') diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c index c1b2a264..173df388 100644 --- a/digital/io/src/ai_top_cb.c +++ b/digital/io/src/ai_top_cb.c @@ -233,6 +233,30 @@ ai__FIRST_GO_END_OF_LINE_UNBLOCKING__loader_errored (void) return ai_next (FIRST_GO_END_OF_LINE_UNBLOCKING, loader_errored); } +/* + * UNLOAD =move_fsm_succeed=> + * => UNLOAD_LOADER_UP + * move loader up + */ +fsm_branch_t +ai__UNLOAD__move_fsm_succeed (void) +{ + loader_up (); + return ai_next (UNLOAD, move_fsm_succeed); +} + +/* + * UNLOAD =move_fsm_failed=> + * => UNLOAD + * retry + */ +fsm_branch_t +ai__UNLOAD__move_fsm_failed (void) +{ + move_start_noangle (PG_VECT (2625, 253), 0); + return ai_next (UNLOAD, move_fsm_failed); +} + /* * UNLOAD_LOADER_UP =loader_uped=> * => UNLOAD_FACE_BIN @@ -291,6 +315,7 @@ ai__UNLOAD_BACK_BIN__bot_move_failed (void) fsm_branch_t ai__UNLOAD_UNLOAD__state_timeout (void) { + loader_elements = 0; asserv_move_motor1_absolute (BOT_GATE_STROKE_STEP, BOT_GATE_SPEED); loader_down (); top_collect (1); -- cgit v1.2.3