From c5ce79b4c6c8671e0e6062706695fc05439252f5 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Fri, 8 May 2009 10:13:00 +0200 Subject: * digital/io/src: - first test: top FSM order basic move to the bot --- digital/io/src/Makefile | 2 +- digital/io/src/main.c | 29 ++++- digital/io/src/top.c | 33 +----- digital/io/src/top.fsm | 97 +++------------- digital/io/src/top.h | 52 +-------- digital/io/src/top_cb.c | 301 +++++++----------------------------------------- 6 files changed, 83 insertions(+), 431 deletions(-) diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 28ccd626..c5465f12 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -16,6 +16,6 @@ AVR_MCU = atmega128 OPTIMIZE = -O2 # FSMs. -FSM_SOURCES := getsamples gutter move top filterbridge elevator cylinder +FSM_SOURCES := top move filterbridge elevator cylinder # Include FSM makefile. include $(BASE)/make/Makefile.fsm diff --git a/digital/io/src/main.c b/digital/io/src/main.c index fd5b720c..c623eea5 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -43,7 +43,7 @@ #include "fsm.h" /* fsm_* */ #include "giboulee.h" /* team_color */ #include "getsamples.h" /* getsamples_start */ -#include "top.h" /* top_* */ +/* #include "top.h" */ /* top_* */ #include "chrono.h" /* chrono_end_match */ #include "gutter.h" /* gutter_generate_wait_finished_event */ #include "sharp.h" /* sharp module */ @@ -147,7 +147,9 @@ main_init (void) path_init (PG_BORDER_DISTANCE, PG_BORDER_DISTANCE, PG_WIDTH - PG_BORDER_DISTANCE, PG_HEIGHT - PG_BORDER_DISTANCE); /* Start the top FSM */ - top_start (); + //top_start (); + fsm_init(&top_fsm); + fsm_handle_event (&top_fsm, TOP_EVENT_start); /* Sharp module */ sharp_init (); /* PWM module */ @@ -237,8 +239,10 @@ main_loop (void) /* Update main */ uint8_t main_asserv_arm_position_reached = asserv_arm_position_reached (); + /* uint8_t main_top_generate_settings_ack_event = top_generate_settings_ack_event (); uint8_t main_gutter_generate_wait_finished_event = gutter_generate_wait_finished_event (); + */ asserv_status_e move_status = asserv_last_cmd_ack () ? asserv_move_cmd_status () : none; @@ -246,22 +250,26 @@ main_loop (void) if (move_status == success) { /* Pass it to all the FSM that need it */ + /* FSM_HANDLE_EVENT (&getsamples_fsm, GETSAMPLES_EVENT_bot_move_succeed); FSM_HANDLE_EVENT (&gutter_fsm, GUTTER_EVENT_bot_move_succeed); + */ FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_bot_move_succeed); } else if (move_status == failure) { /* Move failed */ + /* FSM_HANDLE_EVENT (&getsamples_fsm, GETSAMPLES_EVENT_bot_move_failed); FSM_HANDLE_EVENT (&getsamples_fsm, GETSAMPLES_EVENT_bot_move_succeed); FSM_HANDLE_EVENT (&gutter_fsm, GUTTER_EVENT_bot_move_failed); + */ FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_bot_move_failed); } @@ -271,8 +279,10 @@ main_loop (void) if (arm_status == success) { /* Pass it to all the FSM that need it */ + /* FSM_HANDLE_EVENT (&getsamples_fsm, GETSAMPLES_EVENT_arm_move_succeed); + */ } /* TODO: Check if the sensor placed at the noted position has seen * an arm passed and forward this event to the getsamples FSM */ @@ -280,23 +290,34 @@ main_loop (void) { /* Reset the notifier */ asserv_arm_set_position_reached (0); + /* FSM_HANDLE_EVENT (&getsamples_fsm, GETSAMPLES_EVENT_arm_pass_noted_position); + */ } /* Jack */ FSM_HANDLE_EVENT (&top_fsm, switch_get_jack () ? TOP_EVENT_jack_removed_from_bot : TOP_EVENT_jack_inserted_into_bot); + /*FSM_HANDLE_EVENT (&top_fsm, switch_get_jack () ? + TOP_EVENT_jack_removed_from_bot : + TOP_EVENT_jack_inserted_into_bot); + */ /* Settings acknowledge */ + /* if (main_top_generate_settings_ack_event) { FSM_HANDLE_EVENT (&top_fsm, TOP_EVENT_settings_acknowledged); } + */ + FSM_HANDLE_EVENT (&top_fsm, TOP_EVENT_settings_acknowledged); /* Gutter wait_finished event */ + /* if (main_gutter_generate_wait_finished_event) { FSM_HANDLE_EVENT (&gutter_fsm, GUTTER_EVENT_wait_finished); } + */ /* Event generated at the end of the sub FSM to post to the top FSM */ if (main_post_event_for_top_fsm != 0xFF) { @@ -335,8 +356,10 @@ main_loop (void) /* Wait flag for getsamples FSM */ if (!main_getsamples_wait_cycle) { + /* FSM_HANDLE_EVENT (&getsamples_fsm, GETSAMPLES_EVENT_wait_finished); + */ } /* TODO: Check other sensors */ } @@ -537,7 +560,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - 1b: the approach angle to face the distributor ; * - 1b: how many and where to put collected samples ; */ - getsamples_start (args[0] << 8, args[1], 0); + /* getsamples_start (args[0] << 8, args[1], 0); */ break; case c ('A', 1): diff --git a/digital/io/src/top.c b/digital/io/src/top.c index dbbe4892..7201f192 100644 --- a/digital/io/src/top.c +++ b/digital/io/src/top.c @@ -1,7 +1,7 @@ /* top.c */ /* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ * - * Copyright (C) 2008 Nélio Laranjeiro + * Copyright (C) 2009 Nicolas Haller * * APBTeam: * Web: http://apbteam.org/ @@ -22,35 +22,4 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ -#include "common.h" -#include "asserv.h" #include "top.h" -#include "fsm.h" - -struct top_data_t top_data; - -/** - * When we need to tell the main loop we want to be alerted when the last - * command sent to the asserv board has been acknowledged. - */ -uint8_t top_waiting_for_settings_ack_ = 0; - -/** Start a main FSM. */ -void -top_start (void) -{ - /* Set parameters. */ - top_data.sequence = 0x15; - top_data.boxes_used = 0x0; - top_data.sequence_to_do = 0x15; - /* Start the FSM. */ - fsm_init (&top_fsm); - fsm_handle_event (&top_fsm, TOP_EVENT_start); -} - -/* Do we need to tell the top FSM the settings has been acknowledged? */ -uint8_t -top_generate_settings_ack_event (void) -{ - return top_waiting_for_settings_ack_; -} diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 06db7b8f..c3a492ec 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -5,7 +5,7 @@ # We do not manage some cases like, we have not get any balls from any # distributors. top - Top FSM that call others FSM + Top FSM with the Marcel's strategy States: IDLE @@ -16,38 +16,16 @@ States: waiting for the jack to be removed from the bot CONFIGURE_ASSERV configuring the asserv board - MOVE_AWAY_FROM_START_BORDER - move the bot away from the border of the start point to be able to turn - freely - GO_TO_SAMPLE_DISTRIBUTOR - go to our distributor of samples (using the move FSM) - GO_TO_OUR_ICE_DISTRIBUTOR - go to our ice distributor (using the move FSM) - GO_TO_ADVERSE_ICE_DISTRIBUTOR - go to the adverse ice distributor (using the move FSM) - GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR - get samples from the samples distributor (using the get samples FSM) - GET_ICE_FROM_OUR_ICE_DISTRIBUTOR - get ice from our ice distributor (using the get samples FSM) - GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR - get ice from adverse ice distributor (using the get samples FSM) - GO_TO_GUTTER - go to the gutter (using the move FSM) - DROP_OFF_BALLS_TO_GUTTER - drop all the balls contained in the bot into the gutter (using the gutter - FSM) - MOVE_AWAY_FROM_GUTTER_BORDER - move the bot away from the border of the gutter to be able to turn freely + MOVE_FORWARD + move forward + MOVE_BACKWARD + move backward Events: start initialize the FSM - get_samples_fsm_finished - when the get samples FSM returns move_fsm_finished when the move FSM returns - gutter_fsm_finished - when the gutter FSM returns jack_inserted_into_bot the jack is inserted into the bot jack_removed_from_bot @@ -70,62 +48,13 @@ WAIT_JACK_OUT: set the settings of the asserv board (especially the position) CONFIGURE_ASSERV: - settings_acknowledged -> MOVE_AWAY_FROM_START_BORDER - move the bot away from the border to be able to turn freely + settings_acknowledged -> MOVE_FORWARD + move the bot forward -MOVE_AWAY_FROM_START_BORDER: - move_fsm_finished -> GO_TO_SAMPLE_DISTRIBUTOR - order the bot to move to our samples distributors with the move FSM +MOVE_FORWARD: + move_fsm_finished -> MOVE_BACKWARD + return to initial position (yep it's useless but it's for test) -GO_TO_SAMPLE_DISTRIBUTOR: - move_fsm_finished -> GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR - we are now in front of our samples distributor, launch the get samples FSM - -GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR: - get_samples_fsm_finished: not_near_end_of_match -> GO_TO_OUR_ICE_DISTRIBUTOR - we have finished to get our samples, let's go to our ice distributor with - the move FSM - get_samples_fsm_finished: near_end_of_match -> GO_TO_GUTTER - we have finished to get our samples, but there is no time, let's go to the - gutter with the move FSM - -GO_TO_OUR_ICE_DISTRIBUTOR: - move_fsm_finished -> GET_ICE_FROM_OUR_ICE_DISTRIBUTOR - we are now in front of our ice distributor, launch the get samples FSM - -GET_ICE_FROM_OUR_ICE_DISTRIBUTOR: - get_samples_fsm_finished: full -> GO_TO_GUTTER - we have finished to get ice from our distributor and we have no more space - left, let's go the gutter with the move FSM - get_samples_fsm_finished: not full -> GO_TO_ADVERSE_ICE_DISTRIBUTOR - we have finished to get ice from our distributor and we have some space - left, let's go the adverse ice distributor with the move FSM - -GO_TO_GUTTER: - move_fsm_finished -> DROP_OFF_BALLS_TO_GUTTER - we are now at the gutter, let's drop all ours balls into it with the gutter - FSM - -DROP_OFF_BALLS_TO_GUTTER: - gutter_fsm_finished -> MOVE_AWAY_FROM_GUTTER_BORDER - we have finished to drop off all the balls, let's move away from the gutter - to move freely - -GO_TO_ADVERSE_ICE_DISTRIBUTOR: - move_fsm_finished -> GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR - we are now in front of the adverse ice distributor, launch the get samples - FSM - -GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR: - get_samples_fsm_finished -> GO_TO_GUTTER - we have finished to get ice. Even if we are not full, let's go to the gutter - with the move FSM - -MOVE_AWAY_FROM_GUTTER_BORDER: - move_fsm_finished: first_loop -> GO_TO_SAMPLE_DISTRIBUTOR - go to our sample ditributor to try the same strategy again - reset internal data - move_fsm_finished: second_loop -> GO_TO_ADVERSE_ICE_DISTRIBUTOR - there is no more sample nor ice in our distributors, let's try the one of - the opponent - reset internal data +MOVE_BACKWARD: + move_fsm_finished -> MOVE_FORWARD + move forward again diff --git a/digital/io/src/top.h b/digital/io/src/top.h index 024231ec..3b453dc9 100644 --- a/digital/io/src/top.h +++ b/digital/io/src/top.h @@ -3,7 +3,7 @@ /* top.h */ /* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ * - * Copyright (C) 2008 Nélio Laranjeiro + * Copyright (C) 2009 Nicolas Haller * * APBTeam: * Web: http://apbteam.org/ @@ -25,54 +25,4 @@ * * }}} */ -#define BLUE_DISTRIBUTOR_X 700 -#define BLUE_DISTRIBUTOR_Y 2100 - -#define RED_DISTRIBUTOR_X 2300 -#define RED_DISTRIBUTOR_Y 2100 - -#define ICE_DISTRIBUTOR_LEFT 0 -#define ICE_DISTRIBUTOR_RIGHT 3000 -#define ICE_DISTRIBUTOR_Y 1350 - -enum sequence_e -{ - /* 3 color balls, 2 ice */ - SEQUCENCE_ONE = 0x15, - /* 2 color balls, 3 ice */ - SEQUENCE_TWO = 0xA -}; - -struct top_data_t -{ - /** The sequence to get. - * Each bit corresponds to the slot in the collector (where the balls a - * stored in the robot). - * bit 0 = slot 0 - * bit 1 = slot 1 and so on. - */ - uint8_t sequence; - /** The boxes already in use. */ - uint8_t boxes_used; - /** sequence to realize. */ - uint8_t sequence_to_do; -}; - -extern struct top_data_t top_data; - -/** Start a Top FSM. */ -void -top_start (void); - -/** - * Do we need to tell the top FSM the settings has been acknowledged? - * You need to call this function in the main loop to ensure we pass a - * requested event (settings_acknowledged) to the top FSM. - * @return - * - 0 if you do not need to generate the event for the top FSM ; - * - 1 if you need to generate the event. - */ -uint8_t -top_generate_settings_ack_event (void); - #endif /* top_h */ diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 83a6be5c..8532dc19 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -1,75 +1,17 @@ -/* top_cb.c - top FSM callbacks. */ -/* {{{ - * - * Copyright (C) 2008 Jérémy Dufour - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. +/* + * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * Skeleton for top callbacks implementation. * - * }}} */ + * Top FSM with the Marcel's strategy + */ #include "common.h" #include "fsm.h" #include "top_cb.h" - +#include "chrono.h" +#include "asserv.h" +#include "playground.h" #include "move.h" /* move FSM */ -#include "playground.h" /* PG_* */ -#include "asserv.h" /* asserv_* */ -#include "chrono.h" /* chrono_init */ -#include "trap.h" /* trap_close_rear_panel */ -/* AVR include, non HOST */ -#ifndef HOST -# include "switch.h" /* switch_get_color */ -#endif /* HOST */ -#include "simu.host.h" - -#include "getsamples.h" /* getsamples_* */ -#include "gutter.h" /* gutter_start */ - -#include "io.h" - -/** - * When we need to tell the main loop we want to be alerted when the last - * command sent to the asserv board has been acknowledged. - */ -extern uint8_t top_waiting_for_settings_ack_; - -/** - * Number of loop/iteration of the top FSM - */ -static uint8_t top_fsm_loop_count_; - -/* - * DROP_OFF_BALLS_TO_GUTTER =gutter_fsm_finished=> - * => MOVE_AWAY_FROM_GUTTER_BORDER - * we have finished to drop off all the balls, let's move away from the gutter - * to move freely - */ -fsm_branch_t -top__DROP_OFF_BALLS_TO_GUTTER__gutter_fsm_finished (void) -{ - /* Get current position */ - asserv_position_t position; - asserv_get_position (&position); - /* FIXME: linear move is better? */ - /* Move away from the gutter */ - move_start (position.x, position.y + BOT_MIN_DISTANCE_TURN_FREE, 0); - return top_next (DROP_OFF_BALLS_TO_GUTTER, gutter_fsm_finished); -} /* * WAIT_JACK_OUT =jack_removed_from_bot=> @@ -82,121 +24,32 @@ fsm_branch_t top__WAIT_JACK_OUT__jack_removed_from_bot (void) { /* Set-up our color */ - bot_color = switch_get_color (); + //bot_color = switch_get_color (); /* Start the chronometer */ chrono_init (); /* Reset the position of the bot */ asserv_set_position (PG_X_START, PG_Y_START, PG_A_START); /* Tell the main loop we need to be aware when the asserv acknowledge our * settings command */ - top_waiting_for_settings_ack_ = 1; + //top_waiting_for_settings_ack_ = 1; return top_next (WAIT_JACK_OUT, jack_removed_from_bot); } /* - * MOVE_AWAY_FROM_START_BORDER =move_fsm_finished=> - * => GO_TO_SAMPLE_DISTRIBUTOR - * order the bot to move to our samples distributors with the move FSM - */ -fsm_branch_t -top__MOVE_AWAY_FROM_START_BORDER__move_fsm_finished (void) -{ - /* Start the move FSM to our samples distributor */ - move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y, 0); - return top_next (MOVE_AWAY_FROM_START_BORDER, move_fsm_finished); -} - -/* - * GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR =get_samples_fsm_finished=> - * not_near_end_of_match => GO_TO_OUR_ICE_DISTRIBUTOR - * we have finished to get our samples, let's go to our ice distributor with - * the move FSM - * near_end_of_match => GO_TO_GUTTER - * we have finished to get our samples, but there is no time, let's go to the - * gutter with the move FSM - */ -fsm_branch_t -top__GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR__get_samples_fsm_finished (void) -{ - if (!(chrono_remaining_time ()> 83000)) - { - /* Start the move FSM to our ice distributor */ - move_start (PG_DISTRIBUTOR_ICE_OUR_X, PG_DISTRIBUTOR_ICE_OUR_Y, 0); - return top_next_branch (GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR, get_samples_fsm_finished, not_near_end_of_match); - } - else - { - /* Go to gutter */ - move_start (PG_GUTTER_X, PG_GUTTER_Y, 0); - return top_next_branch (GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR, get_samples_fsm_finished, near_end_of_match); - } -} - -/* - * GO_TO_ADVERSE_ICE_DISTRIBUTOR =move_fsm_finished=> - * => GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR - * we are now in front of the adverse ice distributor, launch the get samples - * FSM + * MOVE_BACKWARD =move_fsm_finished=> + * => MOVE_FORWARD + * move forward again */ fsm_branch_t -top__GO_TO_ADVERSE_ICE_DISTRIBUTOR__move_fsm_finished (void) +top__MOVE_BACKWARD__move_fsm_finished (void) { - /* Start the get samples FSM with the correct angle to get only two - * samples. The problem is this should depend on the time we have until - * the end of match */ - uint8_t bitfield = 0x3E; - getsamples_start (PG_DISTRIBUTOR_ICE_ADVERSE_A, bitfield, 0); - return top_next (GO_TO_ADVERSE_ICE_DISTRIBUTOR, move_fsm_finished); -} - -/* - * GO_TO_OUR_ICE_DISTRIBUTOR =move_fsm_finished=> - * => GET_ICE_FROM_OUR_ICE_DISTRIBUTOR - * we are now in front of our ice distributor, launch the get samples FSM - */ -fsm_branch_t -top__GO_TO_OUR_ICE_DISTRIBUTOR__move_fsm_finished (void) -{ - /* Start the get samples FSM with the correct angle */ - uint8_t bitfield = 0; - if (top_fsm_loop_count_ == 0) - { - /* First time we try to get our ice, let's took three */ - bitfield = _BV (out_left_box) | _BV (middle_box) | _BV - (out_right_box); - } - else - { - /* Second time we try to get our ice, let's took only two */ - bitfield = _BV (middle_left_box) | _BV (middle_right_box); - } - getsamples_start (PG_DISTRIBUTOR_ICE_OUR_A, bitfield, 0); - return top_next (GO_TO_OUR_ICE_DISTRIBUTOR, move_fsm_finished); -} - -/* - * GO_TO_SAMPLE_DISTRIBUTOR =move_fsm_finished=> - * => GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR - * we are now in front of our samples distributor, launch the get samples FSM - */ -fsm_branch_t -top__GO_TO_SAMPLE_DISTRIBUTOR__move_fsm_finished (void) -{ - /* Start the get samples FSM with the correct angle */ - uint8_t bitfield = 0; - if (top_fsm_loop_count_ == 0) - { - /* First time we try to get our samples, let's took only two of them */ - bitfield = _BV (middle_left_box) | _BV (middle_right_box); - } - else - { - /* Second time we try to get our samples, let's took three of them */ - bitfield = _BV (out_left_box) | _BV (middle_box) | _BV - (out_right_box); - } - getsamples_start (PG_DISTRIBUTOR_SAMPLE_OUR_A, bitfield, 1); - return top_next (GO_TO_SAMPLE_DISTRIBUTOR, move_fsm_finished); + /* Get current position */ + asserv_position_t position; + asserv_get_position (&position); + /* FIXME: linear move is better? */ + /* Move away from the gutter */ + move_start (position.x, position.y - 1500, 0); + return top_next (MOVE_BACKWARD, move_fsm_finished); } /* @@ -207,96 +60,25 @@ top__GO_TO_SAMPLE_DISTRIBUTOR__move_fsm_finished (void) fsm_branch_t top__IDLE__start (void) { - top_fsm_loop_count_ = 0; + //top_fsm_loop_count_ = 0; return top_next (IDLE, start); } /* - * GO_TO_GUTTER =move_fsm_finished=> - * => DROP_OFF_BALLS_TO_GUTTER - * we are now at the gutter, let's drop all ours balls into it with the gutter - * FSM - */ -fsm_branch_t -top__GO_TO_GUTTER__move_fsm_finished (void) -{ - /* Start the gutter FSM */ - gutter_start (); - return top_next (GO_TO_GUTTER, move_fsm_finished); -} - -/* - * GET_ICE_FROM_OUR_ICE_DISTRIBUTOR =get_samples_fsm_finished=> - * not full => GO_TO_ADVERSE_ICE_DISTRIBUTOR - * we have finished to get ice from our distributor and we have some space - * left, let's go the adverse ice distributor with the move FSM - * full => GO_TO_GUTTER - * we have finished to get ice from our distributor and we have no more space - * left, let's go the gutter with the move FSM - */ -fsm_branch_t -top__GET_ICE_FROM_OUR_ICE_DISTRIBUTOR__get_samples_fsm_finished (void) -{ - /* TODO: how to detect we have no more space? */ - if (0) - { - /* Start the move FSM to the adverse ice distributor */ - move_start (PG_DISTRIBUTOR_ICE_ADVERSE_X, PG_DISTRIBUTOR_ICE_ADVERSE_Y, 0); - return top_next_branch (GET_ICE_FROM_OUR_ICE_DISTRIBUTOR, get_samples_fsm_finished, not_full); - } - else - { - /* Start the move FSM to go to the gutter */ - move_start (PG_GUTTER_X, PG_GUTTER_Y, 0); - return top_next_branch (GET_ICE_FROM_OUR_ICE_DISTRIBUTOR, get_samples_fsm_finished, full); - } -} - -/* - * MOVE_AWAY_FROM_GUTTER_BORDER =move_fsm_finished=> - * first_loop => GO_TO_SAMPLE_DISTRIBUTOR - * go to our sample ditributor to try the same strategy again - * reset internal data - * second_loop => GO_TO_ADVERSE_ICE_DISTRIBUTOR - * there is no more sample nor ice in our distributors, let's try the one of - * the opponent - * reset internal data - */ -fsm_branch_t -top__MOVE_AWAY_FROM_GUTTER_BORDER__move_fsm_finished (void) -{ - /* We have finished a loop */ - top_fsm_loop_count_++; - if (top_fsm_loop_count_ < 2) - { - /* Two first loops, normal behaviour (our sample distributor, them - * ice) */ - /* Start the move FSM */ - move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y, - 0); - return top_next_branch (MOVE_AWAY_FROM_GUTTER_BORDER, move_fsm_finished, first_loop); - } - else - { - /* Second loops, ice only */ - /* Start the move FSM */ - move_start (PG_DISTRIBUTOR_ICE_ADVERSE_X, PG_DISTRIBUTOR_ICE_ADVERSE_Y, 0); - return top_next_branch (MOVE_AWAY_FROM_GUTTER_BORDER, move_fsm_finished, second_loop); - } -} - -/* - * GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR =get_samples_fsm_finished=> - * => GO_TO_GUTTER - * we have finished to get ice. Even if we are not full, let's go to the gutter - * with the move FSM + * MOVE_FORWARD =move_fsm_finished=> + * => MOVE_BACKWARD + * return to initial position (yep it's useless but it's for test) */ fsm_branch_t -top__GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR__get_samples_fsm_finished (void) +top__MOVE_FORWARD__move_fsm_finished (void) { - /* Start the move FSM to go to the gutter */ - move_start (PG_GUTTER_X, PG_GUTTER_Y, 0); - return top_next (GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR, get_samples_fsm_finished); + /* Get current position */ + asserv_position_t position; + asserv_get_position (&position); + /* FIXME: linear move is better? */ + /* Move away from the gutter */ + move_start (position.x, position.y + 1500, 0); + return top_next (MOVE_FORWARD, move_fsm_finished); } /* @@ -307,24 +89,23 @@ top__GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR__get_samples_fsm_finished (void) fsm_branch_t top__WAIT_JACK_IN__jack_inserted_into_bot (void) { - /* TODO: move the arm to reset to 0 */ - /* Close the rear panel */ - trap_close_rear_panel (); return top_next (WAIT_JACK_IN, jack_inserted_into_bot); } /* * CONFIGURE_ASSERV =settings_acknowledged=> - * => MOVE_AWAY_FROM_START_BORDER - * move the bot away from the border to be able to turn freely + * => MOVE_FORWARD + * move the bot forward */ fsm_branch_t top__CONFIGURE_ASSERV__settings_acknowledged (void) { - /* Clear the flag for the setting acknowleged */ - top_waiting_for_settings_ack_ = 0; - /* Start the move FSM to move the the bot away from the border */ - move_start (PG_X_START, PG_Y_START - BOT_MIN_DISTANCE_TURN_FREE, 0); + /* Get current position */ + asserv_position_t position; + asserv_get_position (&position); + /* FIXME: linear move is better? */ + /* Move away from the gutter */ + move_start (position.x, position.y - 1500, 0); return top_next (CONFIGURE_ASSERV, settings_acknowledged); } -- cgit v1.2.3