From d3c8bf5e81f46d66ff0badcbcbf20a3d828a30eb Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Mon, 14 Apr 2008 12:06:01 +0200 Subject: * digital/io/src * build - add dependency of fsm.h for main.c (for bootstrap) ; - ensure it is building for host target. * get samples FSM - remove unneeded fields in the getsamples structures ; - rename it to be more 'private' (to prevent declaring the same one) ; * top FSM - rewrite it ; - still missing some parts to implement. * general - manage color of the bot ; - initialize the switch module in the main_init ; - add position of the gutter. --- digital/io/src/Makefile | 1 + digital/io/src/getsamples.c | 13 +- digital/io/src/getsamples.h | 19 +-- digital/io/src/getsamples_cb.c | 11 +- digital/io/src/giboulee.h | 14 ++ digital/io/src/gutter_cb.c | 2 +- digital/io/src/main.c | 8 ++ digital/io/src/playground.h | 17 +-- digital/io/src/simu.host.c | 5 + digital/io/src/simu.host.h | 4 + digital/io/src/switch.h | 7 +- digital/io/src/top.c | 2 +- digital/io/src/top.fsm | 141 +++++++++++++------ digital/io/src/top.h | 6 - digital/io/src/top_cb.c | 301 ++++++++++++++++++++++------------------- 15 files changed, 326 insertions(+), 225 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index a251442f..8e4782ed 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -49,6 +49,7 @@ EXTRA_CLEAN_FILES = getsamples_fsm.h getsamples_fsm.c getsamples_cb.h \ # Bootstrap, fsm should be generated before fsm.h is used. fsm.c: fsm.h +main.c: fsm.h fsm.h: getsamples_fsm.h gutter_fsm.h move_fsm.h top_fsm.h DFAGEN = python $(BASE)/../../tools/dfagen/dfagen.py diff --git a/digital/io/src/getsamples.c b/digital/io/src/getsamples.c index e68857a0..8cff594e 100644 --- a/digital/io/src/getsamples.c +++ b/digital/io/src/getsamples.c @@ -32,18 +32,15 @@ /** * Get samples shared data. */ -struct getsamples_data_t getsamples_data; +struct getsamples_data_t getsamples_data_; /* Start the get samples FSM. */ void getsamples_start (struct getsamples_data_t data) { /* Set parameters */ - getsamples_data.distributor_x = data.distributor_x; - getsamples_data.distributor_y = data.distributor_y; - getsamples_data.distributor_angle = data.distributor_angle; - getsamples_data.sample_bitfield = data.sample_bitfield; - getsamples_data.event = data.event; + getsamples_data_.approach_angle = data.approach_angle; + getsamples_data_.sample_bitfield = data.sample_bitfield; /* Start the get samples FSM */ fsm_init (&getsamples_fsm); @@ -59,12 +56,12 @@ getsamples_configure_classifier (void) for (trap_num = 0; trap_num < trap_count; trap_num++) { /* Is the bit set? */ - if (bit_is_set (getsamples_data.sample_bitfield, trap_num)) + if (bit_is_set (getsamples_data_.sample_bitfield, trap_num)) { /* Configure the classifier */ trap_setup_path_to_box (trap_num); /* Reset this bit */ - getsamples_data.sample_bitfield &= ~ _BV (trap_num); + getsamples_data_.sample_bitfield &= ~_BV (trap_num); /* Stop here */ return; } diff --git a/digital/io/src/getsamples.h b/digital/io/src/getsamples.h index f7dd06a1..81d5ca6a 100644 --- a/digital/io/src/getsamples.h +++ b/digital/io/src/getsamples.h @@ -30,35 +30,18 @@ */ struct getsamples_data_t { - /** - * X position of the distributor where to get samples from. - */ - uint32_t distributor_x; - /** - * Y position of the distributor where to get samples from. - */ - uint32_t distributor_y; /** * The angle to approach the distributor. */ - int16_t distributor_angle; + int16_t approach_angle; /** * Bit field to indicate where to put the sample. * If bit 0 is set to 1, a sample will be put into the out_right_box. If * set to 0, the out_right_box will not be used to store a sample. */ uint8_t sample_bitfield; - /** - * Event to post to the top FSM when this one is finished. - */ - uint8_t event; }; -/** - * Get samples shared data. - */ -extern struct getsamples_data_t getsamples_data; - /** * Start the get samples FSM. * @param data get sample data initial configuration. diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c index 85dca001..8f7abefb 100644 --- a/digital/io/src/getsamples_cb.c +++ b/digital/io/src/getsamples_cb.c @@ -33,6 +33,11 @@ #include "giboulee.h" /* BOT_ */ #include "playground.h" /* PG_* */ +/** + * Get samples shared data. + */ +extern struct getsamples_data_t getsamples_data_; + /* * FACE_DISTRIBUTOR =bot_move_succeed=> * => OPEN_INPUT_HOLE @@ -68,7 +73,7 @@ fsm_branch_t getsamples__CLOSE_INPUT_HOLE__arm_move_succeed (void) { /* Tell the top FSM we have finished */ - fsm_handle_event (&top_fsm, getsamples_data.event); + fsm_handle_event (&top_fsm, TOP_EVENT_get_samples_fsm_finished); return getsamples_next (CLOSE_INPUT_HOLE, arm_move_succeed); } @@ -84,7 +89,7 @@ fsm_branch_t getsamples__TAKE_SAMPLES__arm_pass_noted_position (void) { /* More samples? */ - if (getsamples_data.sample_bitfield) + if (getsamples_data_.sample_bitfield) { /* Compute notifier */ uint16_t arm_current_position = asserv_get_arm_position (); @@ -115,7 +120,7 @@ fsm_branch_t getsamples__IDLE__start (void) { /* Face the distributor */ - asserv_goto_angle (getsamples_data.distributor_angle); + asserv_goto_angle (getsamples_data_.approach_angle); return getsamples_next (IDLE, start); } diff --git a/digital/io/src/giboulee.h b/digital/io/src/giboulee.h index aa8309dd..eefd1e7c 100644 --- a/digital/io/src/giboulee.h +++ b/digital/io/src/giboulee.h @@ -66,4 +66,18 @@ */ #define BOT_ANGLE_DEGREE (65536 / 360) +/** + * Definition of the colors. + */ +enum team_color_e +{ + RED_TEAM = 0, + BLUE_TEAM = 1 +}; + +/** + * Our color. + */ +extern enum team_color_e bot_color; + #endif // giboulee_h diff --git a/digital/io/src/gutter_cb.c b/digital/io/src/gutter_cb.c index 2cb159a3..298a2f26 100644 --- a/digital/io/src/gutter_cb.c +++ b/digital/io/src/gutter_cb.c @@ -55,7 +55,7 @@ gutter__CLOSE_COLLECTOR__collector_closed (void) //Close the collector. trap_close_rear_panel(); // Post an event to the top fsm machine - fsm_handle_event (&top_fsm, TOP_EVENT_samples_deposed); + fsm_handle_event (&top_fsm, TOP_EVENT_gutter_fsm_finished); return gutter_next (CLOSE_COLLECTOR, collector_closed); } diff --git a/digital/io/src/main.c b/digital/io/src/main.c index f2bd5355..85987b52 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -40,6 +40,7 @@ #include "eeprom.h" /* Parameters loaded/stored in the EEPROM */ #include "trap.h" /* Trap module (trap_* functions) */ #include "fsm.h" /* fsm_* */ +#include "giboulee.h" /* team_color */ #include "io.h" @@ -53,6 +54,11 @@ static void main_init (void); */ static void main_loop (void); +/** + * Our color. + */ +enum team_color_e bot_color; + /** * Initialize the main and all its subsystems. */ @@ -69,6 +75,8 @@ main_init (void) asserv_init (); /* Trap module */ trap_init (); + /* Switch module */ + switch_init (); /* Enable interrupts */ sei (); diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h index b3e862d8..992425ce 100644 --- a/digital/io/src/playground.h +++ b/digital/io/src/playground.h @@ -46,25 +46,19 @@ */ #define PG_HEIGHT 2100 -/** - * Our color. - * XXX our_color = 1 = bleu, 0 = rouge - */ -extern uint8_t our_color; - /** * Considering there is a symmetry axis on X, this macro will compute the * value for on the X axis depending on the color. */ #define PG_X_VALUE_COMPUTING(x) \ - (our_color ? x : PG_WIDTH - x) + (bot_color ? x : PG_WIDTH - x) /** * Considering there is a symmetry axis on X, this macro will compute the * value of the angle depending on the color. */ #define PG_A_VALUE_COMPUTING(a) \ - (our_color ? a : (a + BOT_ANGLE_DEGREE * 180)) + (bot_color ? a : (a + BOT_ANGLE_DEGREE * 180)) /** * The position where to reset the bot when it starts, depending on the color. @@ -108,4 +102,11 @@ extern uint8_t our_color; #define PG_DISTRIBUTOR_SAMPLE_OUR_Y (PG_HEIGHT - PG_DISTANCE_DISTRIBUTOR) #define PG_DISTRIBUTOR_SAMPLE_OUR_A (BOT_ANGLE_DEGREE * 270) +/** + * The position of the gutter. + */ +#define PG_GUTTER_X (PG_X_VALUE_COMPUTING (2250)) +#define PG_GUTTER_Y (100) +#define PG_GUTTER_A (BOT_ANGLE_DEGREE * 90) + #endif // playground_h diff --git a/digital/io/src/simu.host.c b/digital/io/src/simu.host.c index cc9155bd..e17a001d 100644 --- a/digital/io/src/simu.host.c +++ b/digital/io/src/simu.host.c @@ -151,3 +151,8 @@ eeprom_clear_param (void) { } +void +chrono_init (void) +{ +} + diff --git a/digital/io/src/simu.host.h b/digital/io/src/simu.host.h index 7d2be4fa..00021470 100644 --- a/digital/io/src/simu.host.h +++ b/digital/io/src/simu.host.h @@ -47,6 +47,10 @@ switch_get_color (void); uint8_t switch_get_jack (void); +/** Hooked, do nothing. */ +void +chrono_init (void); + #endif /* defined (HOST) */ #endif /* simu_host_h */ diff --git a/digital/io/src/switch.h b/digital/io/src/switch.h index 29e5a681..0f334776 100644 --- a/digital/io/src/switch.h +++ b/digital/io/src/switch.h @@ -31,6 +31,7 @@ #include "io.h" /* PORT/PIN, bit_is_set */ #include "modules/utils/utils.h" /* set_bit */ +#include "giboulee.h" /* team_color_e */ /** * @defgroup SwitchConfiguration Configuration of the switch module. @@ -74,7 +75,7 @@ * Initialize the switch module. * This functions just put the pins in input direction and enable pull-ups. */ -inline void +static inline void switch_init (void) { /* By default, all pins are in input direction */ @@ -86,7 +87,7 @@ switch_init (void) /** * Get the current state of the select colors switch. */ -inline uint8_t +static inline enum team_color_e switch_get_color (void) { return bit_is_set (SWITCH_COLOR_PIN, SWITCH_COLOR_PIN_NUMBER); @@ -95,7 +96,7 @@ switch_get_color (void) /** * Get the current state of the jack switch. */ -inline uint8_t +static inline uint8_t switch_get_jack (void) { return bit_is_set (SWITCH_JACK_PIN, SWITCH_JACK_PIN_NUMBER); diff --git a/digital/io/src/top.c b/digital/io/src/top.c index c685af19..d290efe5 100644 --- a/digital/io/src/top.c +++ b/digital/io/src/top.c @@ -42,6 +42,6 @@ top_start (uint8_t color_team) top_data.team_color = color_team; /* Start the FSM. */ fsm_init (&top_fsm); - fsm_handle_event (&top_fsm, TOP_EVENT_ok); + fsm_handle_event (&top_fsm, TOP_EVENT_start); } diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 21a6923f..eed38367 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -1,47 +1,106 @@ # Top FSM. +# Some remarks on this FSM. +# Maybe we can make only one state for the all the GO_TO_*DISTRIBUTOR. It is +# more complicated, but it can be cleaner... +# We do not manage some cases like, we have not get any balls from any +# distributors. top + Top FSM that call others FSM States: - START - GET_SAMPLES - GO_TO_GOAL - GET_ICE - GET_ADV_ICE - BACKWARD + IDLE + waiting for the beginning of the top FSM + WAIT_JACK_IN + waiting for the jack to be inserted into the bot + WAIT_JACK_OUT + waiting for the jack to be removed from the bot + 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) Events: - ok - samples_took - collector_full - ice_took - samples_deposed - ice_dist_empty - ended - -START: - ok -> GET_SAMPLES - Go to take some samples. The sequence shall be adapt to take the correct number of samples. - -GET_SAMPLES: - samples_took -> BACKWARD - The samples had been took and now the ice is missing. - -GET_ICE: - ice_took -> BACKWARD - Go backward to end the take balls. - -GO_TO_GOAL: - samples_deposed -> GET_SAMPLES - The samples had been deposed, it shall try to get more samples. This state will call the getsamples FSM and the moveFSM. - -GET_ADV_ICE: - ice_took -> BACKWARD - The ice has been taken. - -BACKWARD: - ended -> GET_ICE - Get ICE to end the sequence. - collector_full -> GO_TO_GOAL - Go the goal to depose the samples. - ice_dist_empty -> GET_ADV_ICE - Go to take the ice in the adversary distributor because our is empty. + 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 + the jack is removed from the bot + +IDLE: + start -> WAIT_JACK_IN + tell the main loop we want to be informed when the jack is inserted into the + bot + +WAIT_JACK_IN: + jack_inserted_into_bot -> WAIT_JACK_OUT + tell the main loop we want to be informed when the jack is removed from the + bot + +WAIT_JACK_OUT: + jack_removed_from_bot -> GO_TO_SAMPLE_DISTRIBUTOR + the match start, start the chronometer + we should also initialize all the subsystems of IO (reset position, get our + color, ...) + order the bot to move to our samples distributors with the move FSM + +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 -> GO_TO_OUR_ICE_DISTRIBUTOR + we have finished to get our samples, let's go to our ice distributor 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 -> GO_TO_SAMPLE_DISTRIBUTOR + we have finished to drop off all the balls, let's go to our sample + ditributor to try the same strategy again + reset internal data + +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 diff --git a/digital/io/src/top.h b/digital/io/src/top.h index aa05c07b..78b83ce8 100644 --- a/digital/io/src/top.h +++ b/digital/io/src/top.h @@ -35,12 +35,6 @@ #define ICE_DISTRIBUTOR_RIGHT 3000 #define ICE_DISTRIBUTOR_Y 1350 -enum team_color_e -{ - BLUE_TEAM, - RED_TEAM -}; - enum sequence_e { /* 3 color balls, 2 ice */ diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index ff45c290..c1624569 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -1,197 +1,226 @@ -/* - * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! +/* top_cb.c - top FSM callbacks. */ +/* {{{ * - * Skeleton for top callbacks implementation. + * 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. + * + * 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. + * + * }}} */ #include "common.h" #include "fsm.h" #include "top_cb.h" -#include "top.h" -#include "getsamples.h" -#include "asserv.h" -#include "gutter.h" + +#include "move.h" /* move FSM */ +#include "playground.h" /* PG_* */ +#include "asserv.h" /* asserv_* */ +/* AVR include, non HOST */ +#ifndef HOST +# include "chrono.h" /* chrono_init */ +# include "switch.h" /* switch_get_color */ +#endif /* HOST */ +#include "simu.host.h" + +#include "getsamples.h" /* getsamples_* */ +#include "gutter.h" /* gutter_start */ /* - * BACKWARD =collector_full=> - * => GO_TO_GOAL - * Go the goal to depose the samples. + * DROP_OFF_BALLS_TO_GUTTER =gutter_fsm_finished=> + * => GO_TO_SAMPLE_DISTRIBUTOR + * we have finished to drop off all the balls, let's go to our sample + * ditributor to try the same strategy again + * reset internal data */ fsm_branch_t -top__BACKWARD__collector_full (void) +top__DROP_OFF_BALLS_TO_GUTTER__gutter_fsm_finished (void) { - gutter_start (); - top_data.sequence_to_do = (~top_data.sequence_to_do) & 0x1f; - return top_next (BACKWARD, collector_full); + /* Start the move FSM */ + move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y); + return top_next (DROP_OFF_BALLS_TO_GUTTER, gutter_fsm_finished); } /* - * BACKWARD =ended=> - * => GET_ICE - * Get ICE to end the sequence. + * WAIT_JACK_OUT =jack_removed_from_bot=> + * => GO_TO_SAMPLE_DISTRIBUTOR + * the match start, start the chronometer + * we should also initialize all the subsystems of IO (reset position, get our + * color, ...) + * order the bot to move to our samples distributors with the move FSM */ fsm_branch_t -top__BACKWARD__ended (void) +top__WAIT_JACK_OUT__jack_removed_from_bot (void) { - /* Generic get sample data */ - struct getsamples_data_t get_sample_data; - get_sample_data.event = TOP_EVENT_ice_took; - get_sample_data.sample_bitfield = ~top_data.boxes_used; // XXX - get_sample_data.distributor_y = ICE_DISTRIBUTOR_Y; - if (top_data.team_color) - { - get_sample_data.distributor_x = ICE_DISTRIBUTOR_LEFT; - getsamples_start (get_sample_data); - } - else - { - get_sample_data.distributor_x = ICE_DISTRIBUTOR_RIGHT; - getsamples_start (get_sample_data); - } - return top_next (BACKWARD, ended); + /* Set-up our color */ + bot_color = switch_get_color (); + /* Start the chronometer */ + chrono_init (); + /* Reset the position of the bot */ + asserv_set_x_position (PG_X_START); + asserv_set_y_position (PG_Y_START); + asserv_set_angle_position (PG_A_START); + /* Start the move FSM to our samples distributor */ + move_start (PG_DISTRIBUTOR_SAMPLE_OUR_X, PG_DISTRIBUTOR_SAMPLE_OUR_Y); + return top_next (WAIT_JACK_OUT, jack_removed_from_bot); } /* - * BACKWARD =ice_dist_empty=> - * => GET_ADV_ICE - * Go to take the ice in the adversary distributor because our is empty. + * GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR =get_samples_fsm_finished=> + * => GO_TO_OUR_ICE_DISTRIBUTOR + * we have finished to get our samples, let's go to our ice distributor with + * the move FSM */ fsm_branch_t -top__BACKWARD__ice_dist_empty (void) +top__GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR__get_samples_fsm_finished (void) { - /* Generic get sample data */ - struct getsamples_data_t get_sample_data; - get_sample_data.event = TOP_EVENT_ice_took; - get_sample_data.sample_bitfield = ~top_data.boxes_used; // XXX - get_sample_data.distributor_y = ICE_DISTRIBUTOR_Y; - if (top_data.team_color) - { - get_sample_data.distributor_x = ICE_DISTRIBUTOR_LEFT; - getsamples_start (get_sample_data); - } - else - { - get_sample_data.distributor_x = ICE_DISTRIBUTOR_RIGHT; - getsamples_start (get_sample_data); - } - return top_next (BACKWARD, ice_dist_empty); + /* Start the move FSM to our ice distributor */ + move_start (PG_DISTRIBUTOR_ICE_OUR_X, PG_DISTRIBUTOR_ICE_OUR_Y); + return top_next (GET_SAMPLES_FROM_SAMPLES_DISTRIBUTOR, get_samples_fsm_finished); } /* - * GET_ICE =ice_took=> - * => BACKWARD - * Go backward to end the take balls. + * 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 */ fsm_branch_t -top__GET_ICE__ice_took (void) +top__GO_TO_ADVERSE_ICE_DISTRIBUTOR__move_fsm_finished (void) { - asserv_position_t pos; - - asserv_get_position (&pos); - - asserv_goto (pos.x - 200, pos.y - 200); - return top_next (GET_ICE, ice_took); + /* Start the get samples FSM with the correct angle */ + struct getsamples_data_t data; + /* TODO: where to put the ice?! */ + data.sample_bitfield = 0; + data.approach_angle = PG_DISTRIBUTOR_ICE_ADVERSE_A; + getsamples_start (data); + return top_next (GO_TO_ADVERSE_ICE_DISTRIBUTOR, move_fsm_finished); } /* - * START =ok=> - * => GET_SAMPLES - * Go to take some samples. The sequence shall be adapt to take the correct number of samples. + * 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__START__ok (void) +top__GO_TO_OUR_ICE_DISTRIBUTOR__move_fsm_finished (void) { - /* Generic get sample data */ - struct getsamples_data_t get_sample_data; - get_sample_data.event = TOP_EVENT_samples_took; - get_sample_data.sample_bitfield = ~top_data.boxes_used; // XXX - if (top_data.team_color == BLUE_TEAM) - { - get_sample_data.distributor_x = BLUE_DISTRIBUTOR_X; - get_sample_data.distributor_y = BLUE_DISTRIBUTOR_Y; - getsamples_start (get_sample_data); - } - else - { - get_sample_data.distributor_x = RED_DISTRIBUTOR_X; - get_sample_data.distributor_y = RED_DISTRIBUTOR_Y; - getsamples_start (get_sample_data); - } - return top_next (START, ok); + /* Start the get samples FSM with the correct angle */ + struct getsamples_data_t data; + /* TODO: where to put the ice?! */ + data.sample_bitfield = 0; + data.approach_angle = PG_DISTRIBUTOR_ICE_OUR_A; + getsamples_start (data); + return top_next (GO_TO_OUR_ICE_DISTRIBUTOR, move_fsm_finished); } /* - * GO_TO_GOAL =samples_deposed=> - * => GET_SAMPLES - * The samples had been deposed, it shall try to get more samples. This state will call the getsamples FSM and the moveFSM. + * 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_GOAL__samples_deposed (void) +top__GO_TO_SAMPLE_DISTRIBUTOR__move_fsm_finished (void) { - /* Generic get sample data */ - struct getsamples_data_t get_sample_data; - get_sample_data.event = TOP_EVENT_samples_took; - get_sample_data.sample_bitfield = ~top_data.boxes_used; // XXX - if (top_data.team_color == BLUE_TEAM) - { - get_sample_data.distributor_x = BLUE_DISTRIBUTOR_X; - get_sample_data.distributor_y = BLUE_DISTRIBUTOR_Y; - getsamples_start (get_sample_data); - } - else - { - get_sample_data.distributor_x = RED_DISTRIBUTOR_X; - get_sample_data.distributor_y = RED_DISTRIBUTOR_Y; - getsamples_start (get_sample_data); - } - return top_next (GO_TO_GOAL, samples_deposed); + /* Start the get samples FSM with the correct angle */ + struct getsamples_data_t data; + /* TODO: where to put the samples?! */ + data.sample_bitfield = 0; + data.approach_angle = PG_DISTRIBUTOR_SAMPLE_OUR_A; + getsamples_start (data); + return top_next (GO_TO_SAMPLE_DISTRIBUTOR, move_fsm_finished); } /* - * GET_ADV_ICE =ice_took=> - * => BACKWARD - * The ice has been taken. + * IDLE =start=> + * => WAIT_JACK_IN + * tell the main loop we want to be informed when the jack is inserted into the + * bot */ fsm_branch_t -top__GET_ADV_ICE__ice_took (void) +top__IDLE__start (void) { - asserv_position_t pos; - - asserv_get_position (&pos); - - asserv_goto (pos.x - 200, pos.y - 200); - return top_next (GET_ADV_ICE, ice_took); + /* TODO */ + return top_next (IDLE, start); } /* - * GET_SAMPLES =samples_took=> - * => BACKWARD - * The samples had been took and now the ice is missing. + * 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__GET_SAMPLES__samples_took (void) +top__GO_TO_GUTTER__move_fsm_finished (void) { - // Call the get samples state machine. + /* Start the gutter FSM */ + gutter_start (); + return top_next (GO_TO_GUTTER, move_fsm_finished); +} - // Blue color. - /* Generic get sample data */ - struct getsamples_data_t get_sample_data; - get_sample_data.event = TOP_EVENT_samples_took; - get_sample_data.sample_bitfield = top_data.sequence; // XXX - if (top_data.team_color == BLUE_TEAM) +/* + * 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) { - get_sample_data.distributor_x = BLUE_DISTRIBUTOR_X; - get_sample_data.distributor_y = BLUE_DISTRIBUTOR_Y; - getsamples_start (get_sample_data); + /* Start the move FSM to the adverse ice distributor */ + move_start (PG_DISTRIBUTOR_ICE_ADVERSE_X, PG_DISTRIBUTOR_ICE_ADVERSE_Y); + return top_next_branch (GET_ICE_FROM_OUR_ICE_DISTRIBUTOR, get_samples_fsm_finished, not_full); } else { - get_sample_data.distributor_x = RED_DISTRIBUTOR_X; - get_sample_data.distributor_y = RED_DISTRIBUTOR_Y; - getsamples_start (get_sample_data); + /* Start the move FSM to go to the gutter */ + move_start (PG_GUTTER_X, PG_GUTTER_Y); + return top_next_branch (GET_ICE_FROM_OUR_ICE_DISTRIBUTOR, get_samples_fsm_finished, full); } - return top_next (START, ok); } +/* + * 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 + */ +fsm_branch_t +top__GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR__get_samples_fsm_finished (void) +{ + /* Start the move FSM to go to the gutter */ + move_start (PG_GUTTER_X, PG_GUTTER_Y); + return top_next (GET_ICE_FROM_ADVERSE_ICE_DISTRIBUTOR, get_samples_fsm_finished); +} +/* + * WAIT_JACK_IN =jack_inserted_into_bot=> + * => WAIT_JACK_OUT + * tell the main loop we want to be informed when the jack is removed from the + * bot + */ +fsm_branch_t +top__WAIT_JACK_IN__jack_inserted_into_bot (void) +{ + /* TODO */ + return top_next (WAIT_JACK_IN, jack_inserted_into_bot); +} -- cgit v1.2.3