From 80b3da91dac28a5895a8cb9f0477a7008365dd56 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 1 Apr 2010 23:57:20 +0200 Subject: digital/io: wipe-out old robot code, closes #112 --- digital/io/src/top_cb.c | 883 +----------------------------------------------- 1 file changed, 3 insertions(+), 880 deletions(-) (limited to 'digital/io/src/top_cb.c') diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 35345858..bf24c7ee 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -1,7 +1,7 @@ /* top_cb.c */ /* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ * - * Copyright (C) 2009 Dufour Jérémy + * Copyright (C) 2010 Nicolas Schodet * * APBTeam: * Web: http://apbteam.org/ @@ -32,16 +32,6 @@ #include "chrono.h" #include "playground.h" #include "asserv.h" -#include "cylinder.h" -#include "elevator.h" - -/** - * Internal data. - */ -asserv_position_t front_position; -uint8_t remember_distributor = 0; -#define TOP_UNLOAD_RETRY_COUNT 1 -uint8_t unload_retry_count = TOP_UNLOAD_RETRY_COUNT; /** * Time limit. @@ -61,879 +51,12 @@ top__IDLE__start (void) /* * WAIT_INIT_TO_FINISH =init_match_is_started=> - * => GET_PUCK_FROM_THE_GROUND - * the match start, try to get some puck from the ground. + * => WAIT_INIT_TO_FINISH + * the match start, place something interresting here */ fsm_branch_t top__WAIT_INIT_TO_FINISH__init_match_is_started (void) { - /* Get next position. */ - asserv_position_t position; - top_get_next_position_to_get_puck_on_the_ground (&position, 0); - /* Go there. */ - move_start (position, 0); return top_next (WAIT_INIT_TO_FINISH, init_match_is_started); } -/* - * GET_PUCK_FROM_THE_GROUND =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. - * already_six_pucks_or_no_next_position_and_some_puck => 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 - * go to the next position using move FSM. - */ -fsm_branch_t -top__GET_PUCK_FROM_THE_GROUND__move_fsm_succeed (void) -{ - /* Get next position. */ - asserv_position_t position; - if (!top_get_next_position_to_get_puck_on_the_ground (&position, 0) - || top_total_puck_taken >= 6) - { - /* Ensure cylinder is close. */ - cylinder_close_order = 1; - if (top_puck_inside_bot) - { - /* 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_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, already_six_pucks_or_no_next_position_and_some_puck); - } - 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_succeed, already_six_pucks_or_no_next_position_and_no_puck); - } - } - else - { - /* Go there. */ - move_start (position, 0); - return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, next_position_exists); - } -} - -/* - * GET_PUCK_FROM_THE_GROUND =move_fsm_failed=> - * 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. - * 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; - asserv_position_t position; - 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); - } -} - -/* - * GET_PUCK_FROM_THE_GROUND =bot_is_full_of_pucks=> - * => STOP_TO_GO_TO_UNLOAD_AREA - * stop move FSM. - */ -fsm_branch_t -top__GET_PUCK_FROM_THE_GROUND__bot_is_full_of_pucks (void) -{ - /* Ensure cylinder is close. */ - cylinder_close_order = 1; - /* Stop move FSM. */ - move_stop (); - return top_next (GET_PUCK_FROM_THE_GROUND, bot_is_full_of_pucks); -} - -/* - * GET_PUCK_FROM_THE_GROUND =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. - * 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) -{ - /* Ensure cylinder is close. */ - cylinder_close_order = 1; - if (top_puck_inside_bot) - { - /* Stop move FSM. */ - move_stop (); - 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); - } -} - -/* - * GET_PUCK_FROM_DISTRIBUTOR =move_fsm_succeed=> - * => CLEAN_FRONT_OF_DISTRIBUTOR - * really go to the front of the distributor (it clean front area). - */ -fsm_branch_t -top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_succeed (void) -{ - /* Go in the front of the distributor. */ - move_start (front_position, ASSERV_REVERT_OK); - return top_next (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_succeed); -} - -/* - * GET_PUCK_FROM_DISTRIBUTOR =move_fsm_failed=> - * no_puck_or_still_time => GET_PUCK_FROM_DISTRIBUTOR - * get the next distributor position and launch move FSM to go there. - * some_pucks_and_no_more_time => GO_TO_UNLOAD_AREA - * compute an unload area. - * ask move FSM to go there. - */ -fsm_branch_t -top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_failed (void) -{ - if (!top_puck_inside_bot || chrono_remaining_time () > TOP_TIME_LIMIT) - { - /* Close cylinder. */ - cylinder_close_order = 1; - /* 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 (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_failed, no_puck_or_still_time); - } - else - { - /* Ensure cylinder is closed. */ - 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_branch (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_failed, some_pucks_and_no_more_time); - } -} - -/* - * GET_PUCK_FROM_DISTRIBUTOR =state_timeout=> - * => GET_PUCK_FROM_DISTRIBUTOR - * get the next distributor position and launch move FSM to go there. - */ -fsm_branch_t -top__GET_PUCK_FROM_DISTRIBUTOR__state_timeout (void) -{ - /* Close cylinder. */ - cylinder_close_order = 1; - /* 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 (GET_PUCK_FROM_DISTRIBUTOR, state_timeout); -} - -/* - * STOP_TO_GO_TO_UNLOAD_AREA =move_fsm_stopped=> - * => GO_TO_UNLOAD_AREA - * compute an unload area. - * ask move FSM to go there using backward move only. - */ -fsm_branch_t -top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_stopped (void) -{ - /* Ensure cylinder is closed. */ - 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 (STOP_TO_GO_TO_UNLOAD_AREA, move_fsm_stopped); -} - -/* - * STOP_TO_GO_TO_UNLOAD_AREA =move_fsm_succeed=> - * => GO_TO_UNLOAD_AREA - * do the same as bot_move_stopped. - */ -fsm_branch_t -top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_succeed (void) -{ - /* Yerk. */ - top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_stopped (); - return top_next (STOP_TO_GO_TO_UNLOAD_AREA, move_fsm_succeed); -} - -/* - * STOP_TO_GO_TO_UNLOAD_AREA =bot_move_failed=> - * => GO_TO_UNLOAD_AREA - * do the same as bot_move_stopped. - */ -fsm_branch_t -top__STOP_TO_GO_TO_UNLOAD_AREA__bot_move_failed (void) -{ - /* Yerk. */ - top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_stopped (); - return top_next (STOP_TO_GO_TO_UNLOAD_AREA, bot_move_failed); -} - -/* - * STOP_TO_GET_PUCK_FROM_DISTRIBUTOR =move_fsm_stopped=> - * => GET_PUCK_FROM_DISTRIBUTOR - * get next distributor and go there. - */ -fsm_branch_t -top__STOP_TO_GET_PUCK_FROM_DISTRIBUTOR__move_fsm_stopped (void) -{ - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* 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 (STOP_TO_GET_PUCK_FROM_DISTRIBUTOR, move_fsm_stopped); -} - -/* - * GO_TO_UNLOAD_AREA =move_fsm_succeed=> - * => PREPARE_ELEVATOR - * prepare the elevator. - */ -fsm_branch_t -top__GO_TO_UNLOAD_AREA__move_fsm_succeed (void) -{ - /* Prepare elevator. */ - elvt_prepare(3); - return top_next (GO_TO_UNLOAD_AREA, move_fsm_succeed); -} - -/* - * GO_TO_UNLOAD_AREA =move_fsm_failed=> - * => GO_TO_UNLOAD_AREA - * compute a new unload area. - * ask move FSM to go to the position using backward only. - */ -fsm_branch_t -top__GO_TO_UNLOAD_AREA__move_fsm_failed (void) -{ - /* Ensure cylinder is closed. */ - 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 (GO_TO_UNLOAD_AREA, move_fsm_failed); -} - -/* - * FUCK_UNLOAD_AREA =bot_move_succeed=> - * => UNLOAD_PUCKS - * unload all the pucks from the elevator. - */ -fsm_branch_t -top__FUCK_UNLOAD_AREA__bot_move_succeed (void) -{ - /* Unload elevator. */ - elvt_open(3); - return top_next (FUCK_UNLOAD_AREA, bot_move_succeed); -} - -/* - * FUCK_UNLOAD_AREA =bot_move_failed=> - * try_again => GO_AWAY_TO_RETRY_UNLOAD - * move backward from the unload area using linear move. - * no_more_try_again => UNLOAD_PUCKS - * move backward from the unload area using linear move. - */ -fsm_branch_t -top__FUCK_UNLOAD_AREA__bot_move_failed (void) -{ - if (unload_retry_count--) - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* Move forward. */ - asserv_move_linearly (PG_BORDER_DISTANCE); - return top_next_branch (FUCK_UNLOAD_AREA, bot_move_failed, try_again); - } - else - { - elvt_open_degraded (3); - return top_next_branch (FUCK_UNLOAD_AREA, bot_move_failed, no_more_try_again); - } -} - -/* - * UNLOAD_PUCKS =elevator_order_done=> - * => GO_AWAY_FROM_UNLOAD_AREA - * linear move. - */ -fsm_branch_t -top__UNLOAD_PUCKS__elevator_order_done (void) -{ - /* Close elevator. */ - elvt_close(); - /* Reset counter. */ - unload_retry_count = TOP_UNLOAD_RETRY_COUNT; - /* Move forward. */ - asserv_move_linearly (PG_BORDER_DISTANCE); - return top_next (UNLOAD_PUCKS, elevator_order_done); -} - -/* - * UNLOAD_PUCKS =state_timeout=> - * => ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD - * we have failed, ask the elevator to put itself in a state where bot movement - * will not damage something. - */ -fsm_branch_t -top__UNLOAD_PUCKS__state_timeout (void) -{ - /* Elevator close. */ - elvt_close(); - return top_next (UNLOAD_PUCKS, state_timeout); -} - -/* - * ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD =elevator_order_done=> - * => GO_AWAY_TO_RETRY_UNLOAD - * move backward from the unload area using linear move. - */ -fsm_branch_t -top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__elevator_order_done (void) -{ - /* Move forward. */ - asserv_move_linearly (PG_BORDER_DISTANCE); - return top_next (ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD, elevator_order_done); -} - -/* - * ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD =state_timeout=> - * => GO_AWAY_TO_RETRY_UNLOAD - * do the same as elevator_ready. - */ -fsm_branch_t -top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__state_timeout (void) -{ - /* Yerk. */ - top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__elevator_order_done (); - return top_next (ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD, state_timeout); -} - -/* - * GO_AWAY_TO_RETRY_UNLOAD =bot_move_succeed=> - * => GO_TO_UNLOAD_AREA - * compute a new unload area. - * ask move FSM to go there using backward move only. - */ -fsm_branch_t -top__GO_AWAY_TO_RETRY_UNLOAD__bot_move_succeed (void) -{ - 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 (GO_AWAY_TO_RETRY_UNLOAD, bot_move_succeed); -} - -/* - * GO_AWAY_TO_RETRY_UNLOAD =bot_move_failed=> - * niceness => TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD - * decrement niceness. - * move forward using linear move. - * no_more_niceness => GO_TO_UNLOAD_AREA - * do the same as move succeed. - */ -fsm_branch_t -top__GO_AWAY_TO_RETRY_UNLOAD__bot_move_failed (void) -{ - /* TODO: niceness. */ - if (1) - { - /* Move backward. */ - asserv_move_linearly (-PG_BORDER_DISTANCE); - return top_next_branch (GO_AWAY_TO_RETRY_UNLOAD, bot_move_failed, niceness); - } - else - { - return top_next_branch (GO_AWAY_TO_RETRY_UNLOAD, bot_move_failed, no_more_niceness); - } -} - -/* - * TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD =bot_move_succeed=> - * => GO_AWAY_TO_RETRY_UNLOAD - * move backward from the unload area using linear move. - */ -fsm_branch_t -top__TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD__bot_move_succeed (void) -{ - /* Move forward. */ - asserv_move_linearly (PG_BORDER_DISTANCE); - return top_next (TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD, bot_move_succeed); -} - -/* - * TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD =bot_move_failed=> - * => GO_AWAY_TO_RETRY_UNLOAD - * do the same as succeed. - */ -fsm_branch_t -top__TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD__bot_move_failed (void) -{ - /* Yerk. */ - top__TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD__bot_move_succeed (); - return top_next (TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD, bot_move_failed); -} - -/* - * FUCK_THE_DISTRIBUTOR =bot_move_succeed=> - * => WAIT_FOR_PUCKS - * let's cylinder FSM works until finished. - */ -fsm_branch_t -top__FUCK_THE_DISTRIBUTOR__bot_move_succeed (void) -{ - /* TODO: tell it to cylinder. */ - cylinder_distributor_fucked = 1; - return top_next (FUCK_THE_DISTRIBUTOR, bot_move_succeed); -} - -/* - * FUCK_THE_DISTRIBUTOR =bot_move_failed=> - * no_puck_or_still_time => GET_PUCK_FROM_DISTRIBUTOR - * get a new distributor and go there. - * some_pucks_and_no_more_time => GO_TO_UNLOAD_AREA - * compute an unload area. - * ask move FSM to go there. - */ -fsm_branch_t -top__FUCK_THE_DISTRIBUTOR__bot_move_failed (void) -{ - /* Close cylinder. */ - cylinder_close_order = 1; - if (!top_puck_inside_bot || chrono_remaining_time () > TOP_TIME_LIMIT) - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* 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 (FUCK_THE_DISTRIBUTOR, bot_move_failed, - no_puck_or_still_time); - } - else - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - 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_branch (FUCK_THE_DISTRIBUTOR, bot_move_failed, - some_pucks_and_no_more_time); - } -} - -/* - * WAIT_FOR_PUCKS =bot_is_full_of_pucks=> - * => GO_AWAY_FROM_DISTRIBUTOR - * remember unload area. - */ -fsm_branch_t -top__WAIT_FOR_PUCKS__bot_is_full_of_pucks (void) -{ - 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 => 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) -{ - /* Rest empty distributor. */ - cylinder_distributor_empty = 0; - if (!top_puck_inside_bot || chrono_remaining_time () > TOP_TIME_LIMIT) - { - 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 - { - 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 => 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) - { - 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 - { - 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); - } -} - -/* - * GO_AWAY_FROM_UNLOAD_AREA =bot_move_succeed=> - * more_than_six_pucks_or_no_next_position => GET_PUCK_FROM_DISTRIBUTOR - * get the next distributor position and launch move FSM to go there. - * next_position_for_pucks_on_ground_exists => GET_PUCK_FROM_THE_GROUND - * go to the next position using move FSM. - */ -fsm_branch_t -top__GO_AWAY_FROM_UNLOAD_AREA__bot_move_succeed (void) -{ - /* Get next position. */ - asserv_position_t position; - if (1 || !top_get_next_position_to_get_puck_on_the_ground (&position, 1) - || top_total_puck_taken >= 6) - { - /* Ensure cylinder is closed. */ - 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 (GO_AWAY_FROM_UNLOAD_AREA, bot_move_succeed, - more_than_six_pucks_or_no_next_position); - } - else - { - cylinder_close_order = 0; - /* Go there. */ - move_start (position, 0); - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_succeed, - next_position_for_pucks_on_ground_exists); - } -} - -/* - * GO_AWAY_FROM_UNLOAD_AREA =bot_move_failed=> - * niceness => TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA - * decrement niceness. - * move forward using linear move. - * no_more_niceness_and_more_than_six_pucks_or_no_next_position => GET_PUCK_FROM_DISTRIBUTOR - * go to the next position using move FSM. - * no_more_niceness_and_next_position_for_pucks_on_ground_exists => GET_PUCK_FROM_THE_GROUND - * go to the next position using move FSM. - */ -fsm_branch_t -top__GO_AWAY_FROM_UNLOAD_AREA__bot_move_failed (void) -{ - /* TODO: niceness. */ - if (1) - { - /* Move backward. */ - asserv_move_linearly (-PG_BORDER_DISTANCE); - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_failed, niceness); - } - else - { - asserv_position_t position; - if (!top_get_next_position_to_get_puck_on_the_ground (&position, 1) - || top_total_puck_taken >= 6) - { - /* 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_UNLOAD_AREA, bot_move_failed, - no_more_niceness_and_more_than_six_pucks_or_no_next_position); - } - else - { - cylinder_close_order = 0; - /* Go there. */ - move_start (position, 0); - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_failed, - no_more_niceness_and_next_position_for_pucks_on_ground_exists); - } - } -} - -/* - * TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA =bot_move_succeed=> - * => GO_AWAY_FROM_UNLOAD_AREA - * move backward using linear move. - */ -fsm_branch_t -top__TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA__bot_move_succeed (void) -{ - /* Move forward. */ - asserv_move_linearly (PG_BORDER_DISTANCE); - return top_next (TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA, bot_move_succeed); -} - -/* - * TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA =bot_move_failed=> - * => GO_AWAY_FROM_UNLOAD_AREA - * do the same as succeed. - */ -fsm_branch_t -top__TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA__bot_move_failed (void) -{ - /* Yerk. */ - top__TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA__bot_move_succeed (); - return top_next (TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA, bot_move_failed); -} - -/* - * CLEAN_FRONT_OF_DISTRIBUTOR =move_fsm_succeed=> - * => FUCK_THE_DISTRIBUTOR - * we are in front of the distributor: fuck it. - */ -fsm_branch_t -top__CLEAN_FRONT_OF_DISTRIBUTOR__move_fsm_succeed (void) -{ - /* Open cylinder. */ - cylinder_close_order = 0; - /* Fuck the distributor. */ - asserv_go_to_distributor (); - return top_next (CLEAN_FRONT_OF_DISTRIBUTOR, move_fsm_succeed); -} - -/* - * CLEAN_FRONT_OF_DISTRIBUTOR =move_fsm_failed=> - * => GET_PUCK_FROM_DISTRIBUTOR - * get a new distributor and go there. - */ -fsm_branch_t -top__CLEAN_FRONT_OF_DISTRIBUTOR__move_fsm_failed (void) -{ - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* 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 (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) - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* 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 - { - - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - 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_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. - * no_more_niceness_and_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_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 - { - if (remember_distributor) - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - /* 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); - } - else - { - /* Ensure cylinder is closed. */ - cylinder_close_order = 1; - 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_branch (GO_AWAY_FROM_DISTRIBUTOR, bot_move_failed, no_more_niceness_and_go_to_unload); - } - } -} - -/* - * 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