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/top_cb.c | 301 +++++++----------------------------------------- 1 file changed, 41 insertions(+), 260 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 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