From c726a7501bd58d172a01558c2fc2183479b6147a Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Thu, 21 May 2009 05:57:45 +0200 Subject: * digital/io/src: - add support for going away from the distributor before going somewhere else, - add support to prepapre the elevator. --- digital/io/src/top.fsm | 52 ++++++++++--- digital/io/src/top_cb.c | 196 ++++++++++++++++++++++++++++++++++++------------ 2 files changed, 192 insertions(+), 56 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 2c286ee5..6447dd1a 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -37,6 +37,12 @@ States: try again to go away from the unload area. CLEAN_FRONT_OF_DISTRIBUTOR clean the front of the distributor to have no problem to fuck it. + GO_AWAY_FROM_DISTRIBUTOR + to correctly leave a distributor, we need to do a linear move. + TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR + try again to go away from the distributor. + PREPARE_ELEVATOR [timeout=2250] + prepare the elevator for unload. Events: start @@ -102,13 +108,19 @@ STOP_TO_GO_TO_UNLOAD_AREA: do the same as bot_move_stopped. GO_TO_UNLOAD_AREA: - move_fsm_succeed -> FUCK_UNLOAD_AREA - make the bot do a fuck the wall in backward until blocked. + move_fsm_succeed -> PREPARE_ELEVATOR + prepare the elevator. # It can be dangerous to loop here with no condition to cancel this loop. move_fsm_failed -> . compute a new unload area. ask move FSM to go to the position using backward only. +PREPARE_ELEVATOR: + elevator_order_done -> FUCK_UNLOAD_AREA + make the bot do a fuck the wall in backward until blocked. + state_timeout -> GO_TO_UNLOAD_AREA + compute a new unload area and retry. + FUCK_UNLOAD_AREA: bot_move_succeed -> UNLOAD_PUCKS unload all the pucks from the elevator. @@ -156,17 +168,37 @@ FUCK_THE_DISTRIBUTOR: ask move FSM to go there. WAIT_FOR_PUCKS: - bot_is_full_of_pucks -> GO_TO_UNLOAD_AREA + bot_is_full_of_pucks -> GO_AWAY_FROM_DISTRIBUTOR + remember unload area. + empty_distributor: no_puck_or_still_time -> GO_AWAY_FROM_DISTRIBUTOR + remember distributor. + empty_distributor: some_pucks_and_no_more_time -> GO_AWAY_FROM_DISTRIBUTOR + remember unload area. + state_timeout: no_puck_or_still_time -> GO_AWAY_FROM_DISTRIBUTOR + remember distributor. + state_timeout: some_pucks_and_no_more_time -> GO_AWAY_FROM_DISTRIBUTOR + remember unload area. + +GO_AWAY_FROM_DISTRIBUTOR: + bot_move_succeed: go_to_distributor -> GET_PUCK_FROM_DISTRIBUTOR + get a new distributor position and go there. + bot_move_succeed: go_to_unload -> GO_TO_UNLOAD_AREA compute an unload area. ask the move FSM to go there. - empty_distributor: no_puck_or_still_time -> GET_PUCK_FROM_DISTRIBUTOR + bot_move_failed: niceness -> TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR + decrement niceness. + move forward using linear move. + bot_move_failed: no_more_niceness_and_go_to_distributor -> GET_PUCK_FROM_DISTRIBUTOR get a new distributor position and go there. - empty_distributor: some_pucks_and_no_more_time -> GO_TO_UNLOAD_AREA - compute an unload area and go there. - state_timeout: no_puck_or_still_time -> GET_PUCK_FROM_DISTRIBUTOR - do the same as empty distributor. - state_timeout: some_pucks_and_no_more_time -> GO_TO_UNLOAD_AREA - do the same as empty distributor. + bot_move_failed: no_more_niceness_and_go_to_unload -> GO_TO_UNLOAD_AREA + compute an unload area. + ask the move FSM to go there. + +TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR: + bot_move_succeed -> GO_AWAY_FROM_DISTRIBUTOR + move backward using linear move. + bot_move_failed -> GO_AWAY_FROM_DISTRIBUTOR + do the same as succeed. UNLOAD_PUCKS: elevator_order_done -> GO_AWAY_FROM_UNLOAD_AREA diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 524385bb..d574ac1c 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -39,6 +39,7 @@ * Internal data. */ asserv_position_t front_position; +uint8_t remember_distributor = 0; /** * Time limit. @@ -139,6 +140,7 @@ 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 + * close cylinder. * too much time lost to get puck from the ground, stop move FSM. */ fsm_branch_t @@ -263,14 +265,14 @@ top__STOP_TO_GET_PUCK_FROM_DISTRIBUTOR__move_fsm_stopped (void) /* * GO_TO_UNLOAD_AREA =move_fsm_succeed=> - * => FUCK_UNLOAD_AREA - * make the bot do a fuck the wall in backward until blocked. + * => PREPARE_ELEVATOR + * prepare the elevator. */ fsm_branch_t top__GO_TO_UNLOAD_AREA__move_fsm_succeed (void) { - /* Find the unload area. */ - asserv_go_to_the_wall (1); + /* Prepare elevator. */ + elvt_order = 3; return top_next (GO_TO_UNLOAD_AREA, move_fsm_succeed); } @@ -487,81 +489,66 @@ top__FUCK_THE_DISTRIBUTOR__bot_move_failed (void) /* * WAIT_FOR_PUCKS =bot_is_full_of_pucks=> - * => GO_TO_UNLOAD_AREA - * compute an unload area. - * ask the move FSM to go there. + * => GO_AWAY_FROM_DISTRIBUTOR + * remember unload area. */ fsm_branch_t top__WAIT_FOR_PUCKS__bot_is_full_of_pucks (void) { - asserv_position_t position; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); + remember_distributor = 0; + /* Backward move. */ + asserv_move_linearly (-PG_BORDER_DISTANCE); return top_next (WAIT_FOR_PUCKS, bot_is_full_of_pucks); } /* * WAIT_FOR_PUCKS =empty_distributor=> - * no_puck_or_still_time => GET_PUCK_FROM_DISTRIBUTOR - * get a new distributor position and go there. - * some_pucks_and_no_more_time => GO_TO_UNLOAD_AREA - * compute an unload area and go there. + * no_puck_or_still_time => GO_AWAY_FROM_DISTRIBUTOR + * remember distributor. + * some_pucks_and_no_more_time => GO_AWAY_FROM_DISTRIBUTOR + * remember unload area. */ fsm_branch_t top__WAIT_FOR_PUCKS__empty_distributor (void) { if (!top_puck_inside_bot || chrono_remaining_time () > TOP_TIME_LIMIT) { - /* Get next position. */ - asserv_position_t position; - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); - return top_next_branch (WAIT_FOR_PUCKS, empty_distributor, - no_puck_or_still_time); + remember_distributor = 1; + /* Backward move. */ + asserv_move_linearly (-PG_BORDER_DISTANCE); + return top_next_branch (WAIT_FOR_PUCKS, empty_distributor, no_puck_or_still_time); } else { - 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_branch (WAIT_FOR_PUCKS, empty_distributor, - some_pucks_and_no_more_time); + remember_distributor = 0; + /* Backward move. */ + asserv_move_linearly (-PG_BORDER_DISTANCE); + return top_next_branch (WAIT_FOR_PUCKS, empty_distributor, some_pucks_and_no_more_time); } } /* * WAIT_FOR_PUCKS =state_timeout=> - * no_puck_or_still_time => GET_PUCK_FROM_DISTRIBUTOR - * do the same as empty distributor. - * some_pucks_and_no_more_time => GO_TO_UNLOAD_AREA - * do the same as empty distributor. + * no_puck_or_still_time => GO_AWAY_FROM_DISTRIBUTOR + * remember distributor. + * some_pucks_and_no_more_time => GO_AWAY_FROM_DISTRIBUTOR + * remember unload area. */ fsm_branch_t top__WAIT_FOR_PUCKS__state_timeout (void) { if (!top_puck_inside_bot || chrono_remaining_time () > TOP_TIME_LIMIT) { - /* Get next position. */ - asserv_position_t position; - /* Go to distributor. */ - top_get_next_position_to_get_distributor (&position, &front_position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); + remember_distributor = 1; + /* Backward move. */ + asserv_move_linearly (-PG_BORDER_DISTANCE); return top_next_branch (WAIT_FOR_PUCKS, state_timeout, no_puck_or_still_time); } else { - asserv_position_t position; - /* Go to unload area. */ - top_get_next_position_to_unload_puck (&position); - /* Go there. */ - move_start (position, ASSERV_BACKWARD); + remember_distributor = 0; + /* Backward move. */ + asserv_move_linearly (-PG_BORDER_DISTANCE); return top_next_branch (WAIT_FOR_PUCKS, state_timeout, some_pucks_and_no_more_time); } } @@ -699,4 +686,121 @@ top__CLEAN_FRONT_OF_DISTRIBUTOR__move_fsm_failed (void) return top_next (CLEAN_FRONT_OF_DISTRIBUTOR, move_fsm_failed); } +/* + * GO_AWAY_FROM_DISTRIBUTOR =bot_move_succeed=> + * go_to_distributor => GET_PUCK_FROM_DISTRIBUTOR + * get a new distributor position and go there. + * go_to_unload => GO_TO_UNLOAD_AREA + * compute an unload area. + * ask the move FSM to go there. + */ +fsm_branch_t +top__GO_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void) +{ + if (remember_distributor) + { + /* Get next position. */ + asserv_position_t position; + /* Go to distributor. */ + top_get_next_position_to_get_distributor (&position, &front_position); + /* Go there. */ + move_start (position, ASSERV_BACKWARD); + return top_next_branch (GO_AWAY_FROM_DISTRIBUTOR, bot_move_succeed, go_to_distributor); + } + else + { + 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_branch (GO_AWAY_FROM_DISTRIBUTOR, bot_move_succeed, go_to_unload); + } +} + +/* + * GO_AWAY_FROM_DISTRIBUTOR =bot_move_failed=> + * niceness => TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR + * decrement niceness. + * move forward using linear move. + * no_more_niceness_and_go_to_distributor => GET_PUCK_FROM_DISTRIBUTOR + * get a new distributor position and go there. + */ +fsm_branch_t +top__GO_AWAY_FROM_DISTRIBUTOR__bot_move_failed (void) +{ + /* TODO: niceness. */ + if (1) + { + /* Linear move. */ + asserv_move_linearly (PG_BORDER_DISTANCE); + return top_next_branch (GO_AWAY_FROM_DISTRIBUTOR, bot_move_failed, niceness); + } + else + { + /* Get next position. */ + asserv_position_t position; + /* Go to distributor. */ + top_get_next_position_to_get_distributor (&position, &front_position); + /* Go there. */ + move_start (position, ASSERV_BACKWARD); + return top_next_branch (GO_AWAY_FROM_DISTRIBUTOR, bot_move_failed, no_more_niceness_and_go_to_distributor); + } +} + +/* + * TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR =bot_move_succeed=> + * => GO_AWAY_FROM_DISTRIBUTOR + * move backward using linear move. + */ +fsm_branch_t +top__TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void) +{ + /* Backward move. */ + asserv_move_linearly (-PG_BORDER_DISTANCE); + return top_next (TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR, bot_move_succeed); +} + +/* + * TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR =bot_move_failed=> + * => GO_AWAY_FROM_DISTRIBUTOR + * do the same as succeed. + */ +fsm_branch_t +top__TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR__bot_move_failed (void) +{ + /* Backward move. */ + asserv_move_linearly (-PG_BORDER_DISTANCE); + return top_next (TRY_AGAIN_TO_GO_AWAY_FROM_DISTRIBUTOR, bot_move_failed); +} + +/* + * PREPARE_ELEVATOR =elevator_order_done=> + * => FUCK_UNLOAD_AREA + * make the bot do a fuck the wall in backward until blocked. + */ +fsm_branch_t +top__PREPARE_ELEVATOR__elevator_order_done (void) +{ + /* Find the unload area. */ + asserv_go_to_the_wall (1); + return top_next (PREPARE_ELEVATOR, elevator_order_done); +} + +/* + * PREPARE_ELEVATOR =state_timeout=> + * => GO_TO_UNLOAD_AREA + * compute a new unload area and retry. + */ +fsm_branch_t +top__PREPARE_ELEVATOR__state_timeout (void) +{ + /* Get next position. */ + 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 (PREPARE_ELEVATOR, state_timeout); +} -- cgit v1.2.3