From c81bdfbebbe4804ebafe603d50c854465c31cf08 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Fri, 22 May 2009 02:45:43 +0200 Subject: * digital/io/src: - always unload pucks before going to the distributor. --- digital/io/src/top.fsm | 5 ++++- digital/io/src/top_cb.c | 25 ++++++++++++++++++++----- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index f7693300..8cee0840 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -93,7 +93,10 @@ GET_PUCK_FROM_THE_GROUND: there with move FSM). bot_is_full_of_pucks -> STOP_TO_GO_TO_UNLOAD_AREA stop move FSM. - state_timeout -> STOP_TO_GET_PUCK_FROM_DISTRIBUTOR + state_timeout: no_puck -> STOP_TO_GET_PUCK_FROM_DISTRIBUTOR + close cylinder. + too much time lost to get puck from the ground, stop move FSM. + state_timeout: some_pucks -> STOP_TO_GO_TO_UNLOAD_AREA close cylinder. too much time lost to get puck from the ground, stop move FSM. diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index c4225837..f349dfa7 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -162,18 +162,33 @@ top__GET_PUCK_FROM_THE_GROUND__bot_is_full_of_pucks (void) /* * GET_PUCK_FROM_THE_GROUND =state_timeout=> - * => STOP_TO_GET_PUCK_FROM_DISTRIBUTOR + * no_puck => STOP_TO_GET_PUCK_FROM_DISTRIBUTOR + * close cylinder. + * too much time lost to get puck from the ground, stop move FSM. + * some_pucks => STOP_TO_GO_TO_UNLOAD_AREA * close cylinder. * too much time lost to get puck from the ground, stop move FSM. */ fsm_branch_t top__GET_PUCK_FROM_THE_GROUND__state_timeout (void) { - /* Stop move FSM. */ - move_stop (); - /* Close cylinder. */ + /* Ensure cylinder is close. */ cylinder_close_order = 1; - return top_next (GET_PUCK_FROM_THE_GROUND, state_timeout); + if (top_total_puck_taken) + { + /* Flush cylinder. */ + cylinder_flush_order = 1; + asserv_position_t position; + /* Go to unload area. */ + top_get_next_position_to_unload_puck (&position); + return top_next_branch (GET_PUCK_FROM_THE_GROUND, state_timeout, some_pucks); + } + else + { + /* Stop move FSM. */ + move_stop (); + return top_next_branch (GET_PUCK_FROM_THE_GROUND, state_timeout, no_puck); + } } /* -- cgit v1.2.3