summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/Makefile2
-rw-r--r--digital/io/src/main.c29
-rw-r--r--digital/io/src/top.c33
-rw-r--r--digital/io/src/top.fsm97
-rw-r--r--digital/io/src/top.h52
-rw-r--r--digital/io/src/top_cb.c301
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);
}