From 5d55416cac3fa485d61fb01c95ca8067c2ca17c7 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 20 May 2009 16:34:31 +0200 Subject: * digital/io/src: - implement top FSM. --- digital/io/src/top.c | 128 ++++++++++++++++++++++++ digital/io/src/top.h | 40 ++++++++ digital/io/src/top_cb.c | 254 +++++++++++++++++++++++++++++++++++++++++++----- 3 files changed, 398 insertions(+), 24 deletions(-) (limited to 'digital') diff --git a/digital/io/src/top.c b/digital/io/src/top.c index 7201f192..40c6280b 100644 --- a/digital/io/src/top.c +++ b/digital/io/src/top.c @@ -22,4 +22,132 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "common.h" #include "top.h" + +#include "playground.h" + +/* Reset to 0. */ +uint8_t top_total_puck_taken = 0; +uint8_t top_puck_inside_bot = 0; + +uint8_t +top_get_next_position_to_get_puck_on_the_ground (asserv_position_t *position, + uint8_t restart) +{ + /* All positions, in order. */ + static const asserv_position_t ground_puck[] = + { + /* Position: 1 */ + { 600, 1525, 270 }, + /* Position: 4 */ + { 600, 925, 270 }, + /* Position: 6 */ + { 1100, 925, 0 }, + /* Position: 9 */ + { 1100, 1525, 90 }, + /* Position: 10 */ + { 850, 1525, 180 }, + /* Position: 10 */ + { 850, 925, 270 }, + }; + static uint8_t current_position = 0; + + if (current_position >= (sizeof (ground_puck) + / sizeof (asserv_position_t))) + /* No more position. */ + return 0; + /* Sanity check. */ + if (position) + { + /* Fill the position. */ + position->x = PG_X_VALUE_COMPUTING (ground_puck[current_position].x); + position->y = ground_puck[current_position].y; + position->a = PG_A_VALUE_COMPUTING (ground_puck[current_position].a * + BOT_ANGLE_DEGREE); + /* Go to next position only if we are restarting the procedure. */ + if (!restart) + current_position++; + } + return 1; +} + +void +top_get_next_position_to_get_distributor (asserv_position_t *clean_position, + asserv_position_t *front_position) +{ + /* Number of distributor. */ +#define DISTRIBUTOR_COUNT 3 + /* Table of distributor, first index is clean, second is front. */ + static const asserv_position_t distributor[DISTRIBUTOR_COUNT][2] = + { + /* First distributor. */ + { { 2500, 170, 0 }, { 2711, 170, 280 } }, + /* Second distributor. */ + { { 2830, 600, 180 }, { 2830, 800, 0 } }, + /* Third distributor. */ + { { 2830, 1100, 180 }, { 2830, 1300, 0 } }, + }; + /* Maximum number of remaining puck by distributor. */ + static uint8_t distributor_puck_count[DISTRIBUTOR_COUNT] = { 5, 5, 5 }; + /* Current distributor. */ + static uint8_t current_position = DISTRIBUTOR_COUNT; + /* Keep track of how many pucks we have get. This variable will be used to + * know how many puck were taken for each distributor. */ + static uint8_t previous_total_puck_taken = 0; + if (!previous_total_puck_taken) + previous_total_puck_taken = top_total_puck_taken; + + /* Sanity check. */ + if (clean_position && front_position) + { + /* Compute taken pucks since last time. */ + uint8_t puck_count = top_total_puck_taken - previous_total_puck_taken; + /* If some pucks were taken. */ + if (puck_count != 0) + { + /* Remove them. */ + distributor_puck_count[current_position] -= (top_total_puck_taken - + previous_total_puck_taken); + } + /* If no puck taken or distributor empty. */ + if (puck_count == 0 || distributor_puck_count[current_position] == 0) + { + /* Next distributor. */ + current_position++; + current_position %= 3; + } + /* Update taken pucks. */ + previous_total_puck_taken = top_total_puck_taken; + + /* Fill the clean position. */ + clean_position->x = PG_X_VALUE_COMPUTING + (distributor[current_position][0].x); + clean_position->y = distributor[current_position][0].y; + clean_position->a = PG_A_VALUE_COMPUTING + (distributor[current_position][0].a * BOT_ANGLE_DEGREE); + /* Fill the distributor position. */ + front_position->x = PG_X_VALUE_COMPUTING + (distributor[current_position][1].x); + front_position->y = distributor[current_position][1].y; + front_position->a = PG_A_VALUE_COMPUTING + (distributor[current_position][1].a * BOT_ANGLE_DEGREE); + } +} + +void +top_get_next_position_to_unload_puck (asserv_position_t *position) +{ + /* TODO: enahnce. */ + static const asserv_position_t unload[] = + { + { 1500, 600, 90 }, + }; + /* Sanity check. */ + if (position) + { + position->x = PG_X_VALUE_COMPUTING (unload->x); + position->y = unload->y; + position->a = PG_A_VALUE_COMPUTING (unload->a * BOT_ANGLE_DEGREE); + } +} diff --git a/digital/io/src/top.h b/digital/io/src/top.h index 3b453dc9..1492405d 100644 --- a/digital/io/src/top.h +++ b/digital/io/src/top.h @@ -25,4 +25,44 @@ * * }}} */ +#include "asserv.h" + +/** + * Get the next position to get pucks on the ground. + * About the @p restart parameter, you need to set to 1 when for example, you + * have unload some pucks and restart the procedure to get new one. + * @param position the next position computed. + * @param restart set it to 1 when you restart a procedure. + * @return 0 if there is no more position. + */ +uint8_t +top_get_next_position_to_get_puck_on_the_ground (asserv_position_t *position, + uint8_t restart); + +/** + * Get the next position to get pucks from the distributor. + * @param clean_position the position to clean the distributor. + * @param front_position the position in front of the distributor to fuck it. + */ +void +top_get_next_position_to_get_distributor (asserv_position_t *clean_position, + asserv_position_t *front_position); + +/** + * Get the next position to get an unload position. + * @param position the next unloading position. + */ +void +top_get_next_position_to_unload_puck (asserv_position_t *position); + +/** + * Count number of pucks we have taken during a match. + */ +extern uint8_t top_total_puck_taken; + +/** + * Number of pucks inside the bot. + */ +extern uint8_t top_puck_inside_bot; + #endif /* top_h */ diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 76cdb2b1..cd5d54a4 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -24,14 +24,23 @@ * }}} * Main FSM calling other FSM. */ - - #include "common.h" #include "fsm.h" #include "top_cb.h" -#include "asserv.h" +#include "top.h" +#include "move.h" +#include "chrono.h" #include "playground.h" -#include "move.h" /* move FSM */ + +/** + * Internal data. + */ +asserv_position_t front_position; + +/** + * Time limit. + */ +#define TOP_TIME_LIMIT 15000 /* * IDLE =start=> @@ -52,6 +61,11 @@ top__IDLE__start (void) 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); } @@ -65,8 +79,23 @@ top__WAIT_INIT_TO_FINISH__init_match_is_started (void) fsm_branch_t top__GET_PUCK_FROM_THE_GROUND__move_fsm_succeed (void) { - return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, already_six_pucks_or_no_next_position); - return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, next_position_exists); + /* 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) + { + /* Go to distributor. */ + top_get_next_position_to_get_distributor (&position, &front_position); + /* Go there. */ + move_start (position, 0); + return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, already_six_pucks_or_no_next_position); + } + else + { + /* Go there. */ + move_start (position, 0); + return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, next_position_exists); + } } /* @@ -78,6 +107,12 @@ top__GET_PUCK_FROM_THE_GROUND__move_fsm_succeed (void) fsm_branch_t top__GET_PUCK_FROM_THE_GROUND__move_fsm_failed (void) { + /* 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, 0); return top_next (GET_PUCK_FROM_THE_GROUND, move_fsm_failed); } @@ -89,6 +124,8 @@ top__GET_PUCK_FROM_THE_GROUND__move_fsm_failed (void) fsm_branch_t top__GET_PUCK_FROM_THE_GROUND__bot_is_full_of_pucks (void) { + /* Stop move FSM. */ + move_stop (); return top_next (GET_PUCK_FROM_THE_GROUND, bot_is_full_of_pucks); } @@ -100,6 +137,8 @@ top__GET_PUCK_FROM_THE_GROUND__bot_is_full_of_pucks (void) fsm_branch_t top__GET_PUCK_FROM_THE_GROUND__state_timeout (void) { + /* Stop move FSM. */ + move_stop (); return top_next (GET_PUCK_FROM_THE_GROUND, state_timeout); } @@ -111,6 +150,8 @@ top__GET_PUCK_FROM_THE_GROUND__state_timeout (void) fsm_branch_t top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_succeed (void) { + /* Go in the front of the distributor. */ + move_start (front_position, 1); return top_next (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_succeed); } @@ -125,8 +166,25 @@ top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_succeed (void) fsm_branch_t top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_failed (void) { - return top_next_branch (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_failed, no_puck_or_still_time); - return top_next_branch (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_failed, some_pucks_and_no_more_time); + 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, 0); + return top_next_branch (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_failed, 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, 2); + return top_next_branch (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_failed, some_pucks_and_no_more_time); + } } /* @@ -138,6 +196,11 @@ top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_failed (void) fsm_branch_t top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_stopped (void) { + asserv_position_t position; + /* Go to unload area. */ + top_get_next_position_to_unload_puck (&position); + /* Go there. */ + move_start (position, 2); return top_next (STOP_TO_GO_TO_UNLOAD_AREA, move_fsm_stopped); } @@ -149,6 +212,8 @@ top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_stopped (void) 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); } @@ -160,6 +225,8 @@ top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_succeed (void) 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); } @@ -171,6 +238,12 @@ top__STOP_TO_GO_TO_UNLOAD_AREA__bot_move_failed (void) fsm_branch_t top__STOP_TO_GET_PUCK_FROM_DISTRIBUTOR__move_fsm_stopped (void) { + /* 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, 0); return top_next (STOP_TO_GET_PUCK_FROM_DISTRIBUTOR, move_fsm_stopped); } @@ -182,6 +255,8 @@ top__STOP_TO_GET_PUCK_FROM_DISTRIBUTOR__move_fsm_stopped (void) fsm_branch_t top__GO_TO_UNLOAD_AREA__move_fsm_succeed (void) { + /* Find the unload area. */ + asserv_go_to_the_wall (1); return top_next (GO_TO_UNLOAD_AREA, move_fsm_succeed); } @@ -194,6 +269,11 @@ top__GO_TO_UNLOAD_AREA__move_fsm_succeed (void) fsm_branch_t top__GO_TO_UNLOAD_AREA__move_fsm_failed (void) { + asserv_position_t position; + /* Go to unload area. */ + top_get_next_position_to_unload_puck (&position); + /* Go there. */ + move_start (position, 2); return top_next (GO_TO_UNLOAD_AREA, move_fsm_failed); } @@ -205,6 +285,7 @@ top__GO_TO_UNLOAD_AREA__move_fsm_failed (void) fsm_branch_t top__FUCK_UNLOAD_AREA__bot_move_succeed (void) { + /* TODO: elevator order to unload. */ return top_next (FUCK_UNLOAD_AREA, bot_move_succeed); } @@ -216,6 +297,8 @@ top__FUCK_UNLOAD_AREA__bot_move_succeed (void) fsm_branch_t top__FUCK_UNLOAD_AREA__bot_move_failed (void) { + /* Move forward. */ + asserv_move_linearly (PG_BORDER_DISTANCE); return top_next (FUCK_UNLOAD_AREA, bot_move_failed); } @@ -227,6 +310,8 @@ top__FUCK_UNLOAD_AREA__bot_move_failed (void) fsm_branch_t top__UNLOAD_PUCKS__elevator_is_closed (void) { + /* Move forward. */ + asserv_move_linearly (PG_BORDER_DISTANCE); return top_next (UNLOAD_PUCKS, elevator_is_closed); } @@ -239,6 +324,7 @@ top__UNLOAD_PUCKS__elevator_is_closed (void) fsm_branch_t top__UNLOAD_PUCKS__state_timeout (void) { + /* TODO: elevator close. */ return top_next (UNLOAD_PUCKS, state_timeout); } @@ -250,6 +336,8 @@ top__UNLOAD_PUCKS__state_timeout (void) fsm_branch_t top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__elevator_is_closed (void) { + /* Move forward. */ + asserv_move_linearly (PG_BORDER_DISTANCE); return top_next (ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD, elevator_is_closed); } @@ -261,6 +349,8 @@ top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__elevator_is_closed (void) 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_is_closed (); return top_next (ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD, state_timeout); } @@ -268,11 +358,16 @@ top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__state_timeout (void) * 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. + * ask move FSM to go there using forward 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, 2); return top_next (GO_AWAY_TO_RETRY_UNLOAD, bot_move_succeed); } @@ -280,25 +375,36 @@ top__GO_AWAY_TO_RETRY_UNLOAD__bot_move_succeed (void) * GO_AWAY_TO_RETRY_UNLOAD =bot_move_failed=> * niceness => TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD * decrement niceness. - * move forward using linear move. + * move backward 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) { - return top_next_branch (GO_AWAY_TO_RETRY_UNLOAD, bot_move_failed, niceness); - return top_next_branch (GO_AWAY_TO_RETRY_UNLOAD, bot_move_failed, no_more_niceness); + /* 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. + * move forward 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); } @@ -310,6 +416,8 @@ top__TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD__bot_move_succeed (void) 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); } @@ -321,6 +429,7 @@ top__TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD__bot_move_failed (void) fsm_branch_t top__FUCK_THE_DISTRIBUTOR__bot_move_succeed (void) { + /* FIXME: nothing to do? */ return top_next (FUCK_THE_DISTRIBUTOR, bot_move_succeed); } @@ -335,8 +444,27 @@ top__FUCK_THE_DISTRIBUTOR__bot_move_succeed (void) fsm_branch_t top__FUCK_THE_DISTRIBUTOR__bot_move_failed (void) { - return top_next_branch (FUCK_THE_DISTRIBUTOR, bot_move_failed, no_puck_or_still_time); - return top_next_branch (FUCK_THE_DISTRIBUTOR, bot_move_failed, some_pucks_and_no_more_time); + 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, 0); + return top_next_branch (FUCK_THE_DISTRIBUTOR, bot_move_failed, + 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, 2); + return top_next_branch (FUCK_THE_DISTRIBUTOR, bot_move_failed, + some_pucks_and_no_more_time); + } } /* @@ -348,6 +476,11 @@ top__FUCK_THE_DISTRIBUTOR__bot_move_failed (void) 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, 2); return top_next (WAIT_FOR_PUCKS, bot_is_full_of_pucks); } @@ -361,8 +494,27 @@ top__WAIT_FOR_PUCKS__bot_is_full_of_pucks (void) fsm_branch_t top__WAIT_FOR_PUCKS__empty_distributor (void) { - return top_next_branch (WAIT_FOR_PUCKS, empty_distributor, no_puck_or_still_time); - return top_next_branch (WAIT_FOR_PUCKS, empty_distributor, some_pucks_and_no_more_time); + 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, 0); + 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, 2); + return top_next_branch (WAIT_FOR_PUCKS, empty_distributor, + some_pucks_and_no_more_time); + } } /* @@ -375,15 +527,32 @@ top__WAIT_FOR_PUCKS__empty_distributor (void) fsm_branch_t top__GO_AWAY_FROM_UNLOAD_AREA__bot_move_succeed (void) { - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_succeed, more_than_six_pucks_or_no_next_position); - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_succeed, next_position_for_pucks_on_ground_exists); + /* 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) + { + /* Go to distributor. */ + top_get_next_position_to_get_distributor (&position, &front_position); + /* Go there. */ + move_start (position, 0); + return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_succeed, + more_than_six_pucks_or_no_next_position); + } + else + { + /* 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. + * move backward 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 @@ -392,19 +561,46 @@ top__GO_AWAY_FROM_UNLOAD_AREA__bot_move_succeed (void) fsm_branch_t top__GO_AWAY_FROM_UNLOAD_AREA__bot_move_failed (void) { - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_failed, niceness); - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_failed, no_more_niceness_and_more_than_six_pucks_or_no_next_position); - return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_failed, no_more_niceness_and_next_position_for_pucks_on_ground_exists); + /* 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, 0) + || top_total_puck_taken >= 6) + { + /* Go to distributor. */ + top_get_next_position_to_get_distributor (&position, &front_position); + /* Go there. */ + move_start (position, 0); + 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 + { + /* 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. + * move forward 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); } @@ -416,6 +612,8 @@ top__TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA__bot_move_succeed (void) 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); } @@ -427,6 +625,8 @@ top__TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA__bot_move_failed (void) fsm_branch_t top__CLEAN_FRONT_OF_DISTRIBUTOR__move_fsm_succeed (void) { + /* Fuck the distributor. */ + asserv_go_to_the_wall (0); return top_next (CLEAN_FRONT_OF_DISTRIBUTOR, move_fsm_succeed); } @@ -438,6 +638,12 @@ top__CLEAN_FRONT_OF_DISTRIBUTOR__move_fsm_succeed (void) fsm_branch_t top__CLEAN_FRONT_OF_DISTRIBUTOR__move_fsm_failed (void) { + /* 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, 0); return top_next (CLEAN_FRONT_OF_DISTRIBUTOR, move_fsm_failed); } -- cgit v1.2.3