From 58afde96c6e2c1d94a61575fd28a896291e8ab3f Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Fri, 22 May 2009 02:33:28 +0200 Subject: * digital/io/src: - when we have finished with the puck from the ground and we go to the distributor, we unload if there is something to unload. --- digital/io/src/top.fsm | 5 ++++- digital/io/src/top_cb.c | 29 +++++++++++++++++++++++------ 2 files changed, 27 insertions(+), 7 deletions(-) diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index bf6ad983..f7693300 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -79,7 +79,10 @@ WAIT_INIT_TO_FINISH: the match start, try to get some puck from the ground. GET_PUCK_FROM_THE_GROUND: - move_fsm_succeed: already_six_pucks_or_no_next_position -> GET_PUCK_FROM_DISTRIBUTOR + move_fsm_succeed: already_six_pucks_or_no_next_position_and_no_puck -> GET_PUCK_FROM_DISTRIBUTOR + close cylinder. + get the next distributor position and launch move FSM to go there. + move_fsm_succeed: already_six_pucks_or_no_next_position_and_some_puck -> STOP_TO_GO_TO_UNLOAD_AREA close cylinder. get the next distributor position and launch move FSM to go there. move_fsm_succeed: next_position_exists -> . diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 6f52d532..c4225837 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -75,7 +75,10 @@ top__WAIT_INIT_TO_FINISH__init_match_is_started (void) /* * GET_PUCK_FROM_THE_GROUND =move_fsm_succeed=> - * already_six_pucks_or_no_next_position => GET_PUCK_FROM_DISTRIBUTOR + * already_six_pucks_or_no_next_position_and_no_puck => GET_PUCK_FROM_DISTRIBUTOR + * close cylinder. + * get the next distributor position and launch move FSM to go there. + * already_six_pucks_or_no_next_position_and_some_puck => STOP_TO_GO_TO_UNLOAD_AREA * close cylinder. * get the next distributor position and launch move FSM to go there. * next_position_exists => GET_PUCK_FROM_THE_GROUND @@ -91,11 +94,25 @@ top__GET_PUCK_FROM_THE_GROUND__move_fsm_succeed (void) { /* Ensure cylinder is close. */ cylinder_close_order = 1; - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, already_six_pucks_or_no_next_position); + 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, move_fsm_succeed, already_six_pucks_or_no_next_position_and_some_puck); + } + else + { + /* Go there. */ + move_start (position, ASSERV_BACKWARD); + /* Go to distributor. */ + top_get_next_position_to_get_distributor (&position, &front_position); + /* Go there. */ + move_start (position, ASSERV_BACKWARD); + return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, already_six_pucks_or_no_next_position_and_no_puck); + } } else { -- cgit v1.2.3