From 19b20cc7f4f83608805b2d18cb466493174a0ca5 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Fri, 22 May 2009 20:23:26 +0200 Subject: * digital/io/src: - do not try to unload pucks when we have no puck and we failed our move. --- digital/io/src/top.fsm | 8 +++++--- digital/io/src/top_cb.c | 34 ++++++++++++++++++++++++---------- 2 files changed, 29 insertions(+), 13 deletions(-) (limited to 'digital') diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 496edadc..02fb854c 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -87,10 +87,12 @@ GET_PUCK_FROM_THE_GROUND: get the next distributor position and launch move FSM to go there. move_fsm_succeed: next_position_exists -> . go to the next position using move FSM. - move_fsm_failed -> GO_TO_UNLOAD_AREA + move_fsm_failed: some_pucks -> GO_TO_UNLOAD_AREA close cylinder. - we have failed to do a move, go the distributor (get next distributor and go - there with move FSM). + we have failed to do a move, unload pucks. + move_fsm_failed: no_puck -> GET_PUCK_FROM_DISTRIBUTOR + close cylinder. + get the next distributor position and launch move FSM to go there. bot_is_full_of_pucks -> STOP_TO_GO_TO_UNLOAD_AREA stop move FSM. state_timeout: no_puck -> STOP_TO_GET_PUCK_FROM_DISTRIBUTOR diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 481f71ec..04e34a08 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -126,23 +126,37 @@ top__GET_PUCK_FROM_THE_GROUND__move_fsm_succeed (void) /* * GET_PUCK_FROM_THE_GROUND =move_fsm_failed=> - * => GO_TO_UNLOAD_AREA + * some_pucks => GO_TO_UNLOAD_AREA + * close cylinder. + * we have failed to do a move, unload pucks. + * no_puck => GET_PUCK_FROM_DISTRIBUTOR * close cylinder. - * we have failed to do a move, go the distributor (get next distributor and go - * there with move FSM). + * get the next distributor position and launch move FSM to go there. */ fsm_branch_t top__GET_PUCK_FROM_THE_GROUND__move_fsm_failed (void) { + /* Ensure cylinder is close. */ cylinder_close_order = 1; - /* Flush cylinder. */ - cylinder_flush_order = 1; asserv_position_t position; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next (GET_PUCK_FROM_THE_GROUND, move_fsm_failed); + if (top_puck_inside_bot) + { + /* Flush cylinder. */ + cylinder_flush_order = 1; + /* Go to unload area. */ + top_get_next_position_to_unload_puck (&position); + /* Go there. */ + move_start (position, ASSERV_BACKWARD); + return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_failed, some_pucks); + } + else + { + /* 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_failed, no_puck); + } } /* -- cgit v1.2.3