summaryrefslogtreecommitdiff
path: root/digital/io/src/top_cb.c
diff options
context:
space:
mode:
authorNicolas Haller2009-05-08 10:13:00 +0200
committerNicolas Haller2009-05-08 10:13:00 +0200
commitc5ce79b4c6c8671e0e6062706695fc05439252f5 (patch)
treea7d48302df0b469db9342ccd1368835f558e72e7 /digital/io/src/top_cb.c
parente5955adc9c33cee7d2fdda723fb02a2af5e612be (diff)
* digital/io/src:
- first test: top FSM order basic move to the bot
Diffstat (limited to 'digital/io/src/top_cb.c')
-rw-r--r--digital/io/src/top_cb.c301
1 files changed, 41 insertions, 260 deletions
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);
}