From 3c7512d1619b2e73424790f157aec3926e39e838 Mon Sep 17 00:00:00 2001 From: Nélio Laranjeiro Date: Sat, 12 Apr 2008 02:03:38 +0200 Subject: Update the fsms. --- digital/io/src/Makefile | 2 +- digital/io/src/getsamples.c | 3 +- digital/io/src/getsamples.fsm | 12 +-- digital/io/src/getsamples.h | 4 +- digital/io/src/getsamples_cb.c | 28 +------ digital/io/src/gutter.c | 35 +++++++++ digital/io/src/gutter.h | 32 ++++++++ digital/io/src/gutter_cb.c | 5 ++ digital/io/src/top.c | 2 + digital/io/src/top.fsm | 39 ++++------ digital/io/src/top.h | 12 +++ digital/io/src/top_cb.c | 172 ++++++++++++++++++++++++----------------- 12 files changed, 217 insertions(+), 129 deletions(-) create mode 100644 digital/io/src/gutter.c create mode 100644 digital/io/src/gutter.h diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 1898742a..a251442f 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -4,7 +4,7 @@ io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c trap.c sharp.c \ simu.host.c \ fsm.c \ getsamples.c getsamples_fsm.c getsamples_cb.c \ - gutter_fsm.c gutter_cb.c \ + gutter_fsm.c gutter_cb.c gutter.c \ move.c move_fsm.c move_cb.c \ top.c top_fsm.c top_cb.c MODULES = proto uart twi utils adc diff --git a/digital/io/src/getsamples.c b/digital/io/src/getsamples.c index b80881eb..96644a59 100644 --- a/digital/io/src/getsamples.c +++ b/digital/io/src/getsamples.c @@ -32,12 +32,13 @@ struct getsamples_data_t getsamples_data; /** Start a getsamples FSM. */ void getsamples_start (uint32_t distributor_x, uint32_t distributor_y, - uint8_t samples) + uint8_t samples, uint8_t event_to_post) { /* Set parameters. */ getsamples_data.distributor_x = distributor_x; getsamples_data.distributor_y = distributor_y; getsamples_data.samples = samples; + getsamples_data.event = event_to_post; /* Start FSM. */ fsm_init (&getsamples_fsm); fsm_handle_event (&getsamples_fsm, GETSAMPLES_EVENT_ok); diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm index 3f7266d0..daf1b738 100644 --- a/digital/io/src/getsamples.fsm +++ b/digital/io/src/getsamples.fsm @@ -7,16 +7,15 @@ States: PREPARE_ARM FORWARD_CONTROL TAKE_SAMPLES - BACKWARD END Events: ok - position_reached position_failed arm_moved sample_took classifier_ready + position_reached START: ok -> GO_TO_POSITION @@ -25,8 +24,6 @@ START: GO_TO_POSITION: position_reached -> PREPARE_ARM Go to the position desired, it is very near the position of the distributor in case it is a ice distributor or sample distributor. - position_failed -> GO_TO_POSITION - Go to another point before trying to go to this one again. PREPARE_ARM: arm_moved -> FORWARD_CONTROL @@ -39,9 +36,6 @@ FORWARD_CONTROL: TAKE_SAMPLES: sample_took: more -> . Continue to take samples and classify the next sample. - sample_took: no_more -> BACKWARD - If the quantity of samples are tooked, then go backeward and conitnue classifying the samples. + sample_took: no_more -> END + If the quantity of samples are taken, then go backward and continue classifying the samples. -BACKWARD: - position_reached -> END - Ending this state machine. diff --git a/digital/io/src/getsamples.h b/digital/io/src/getsamples.h index 883441d5..6bfb3d63 100644 --- a/digital/io/src/getsamples.h +++ b/digital/io/src/getsamples.h @@ -34,6 +34,8 @@ struct getsamples_data_t uint32_t distributor_y; /* Samples to take. */ uint8_t samples; + /* event of the main fsm to post. */ + uint8_t event; }; /** getsamples global. */ @@ -42,7 +44,7 @@ extern struct getsamples_data_t getsamples_data; /** Start a getsamples FSM. */ void getsamples_start (uint32_t distributor_x, uint32_t distributor_y, - uint8_t samples); + uint8_t samples, uint8_t event_to_post); /** Configure the classifier using the bit fields in the getsamples_data * structure. diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c index 68268795..3c15fc42 100644 --- a/digital/io/src/getsamples_cb.c +++ b/digital/io/src/getsamples_cb.c @@ -75,7 +75,7 @@ getsamples__START__ok (void) /* * TAKE_SAMPLES =sample_took=> - * no_more => BACKWARD + * no_more => END * If the quantity of samples are taken, then go backward and continue classifying the samples. * more => TAKE_SAMPLES * Continue to take samples and classify the next sample. @@ -96,35 +96,11 @@ getsamples__TAKE_SAMPLES__sample_took (void) asserv_goto (getsamples_data.distributor_x - 200, getsamples_data.distributor_y - 200); + fsm_handle_event (&top_fsm, getsamples_data.event); return getsamples_next_branch (TAKE_SAMPLES, sample_took, no_more); } } -/* - * BACKWARD =position_reached=> - * => END - * Ending this state machine. - */ -fsm_branch_t -getsamples__BACKWARD__position_reached (void) -{ - asserv_move_arm (5000, 100); - - fsm_handle_event (&top_fsm, TOP_EVENT_samples_took); - return getsamples_next (BACKWARD, position_reached); -} - -/* - * GO_TO_POSITION =position_failed=> - * => GO_TO_POSITION - * Go to another point before trying to go to this one again. - */ -fsm_branch_t -getsamples__GO_TO_POSITION__position_failed (void) -{ - // TODO In this case i don't know what to do. - return getsamples_next (GO_TO_POSITION, position_failed); -} /* * GO_TO_POSITION =position_reached=> diff --git a/digital/io/src/gutter.c b/digital/io/src/gutter.c new file mode 100644 index 00000000..0337b974 --- /dev/null +++ b/digital/io/src/gutter.c @@ -0,0 +1,35 @@ +/* gutter.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2008 Nélio Laranjeiro + * + * 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 "gutter.h" +#include "fsm.h" + +void +gutter_start (void) +{ + /* Start the FSM. */ + fsm_init (&gutter_fsm); + fsm_handle_event (&gutter_fsm, GUTTER_EVENT_ok); +} diff --git a/digital/io/src/gutter.h b/digital/io/src/gutter.h new file mode 100644 index 00000000..32d0b226 --- /dev/null +++ b/digital/io/src/gutter.h @@ -0,0 +1,32 @@ +#ifndef gutter_h +#define gutter_h +/* gutter.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2008 Nélio Laranjeiro + * + * 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. + * + * }}} */ + +/* gutter start */ +void +gutter_start (void); + +#endif /* gutter_h */ diff --git a/digital/io/src/gutter_cb.c b/digital/io/src/gutter_cb.c index 336ae404..f2c01539 100644 --- a/digital/io/src/gutter_cb.c +++ b/digital/io/src/gutter_cb.c @@ -24,10 +24,13 @@ * }}} */ #include "common.h" #include "fsm.h" +#include "gutter.h" #include "gutter_cb.h" #include "asserv.h" #include "trap.h" #include "modules/utils/utils.h" +#include "top.h" +#include "topfsm_fsm.h" /* * START =ok=> @@ -51,6 +54,8 @@ 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, TOPFSM_EVENT_samples_deposed); return gutter_next (CLOSE_COLLECTOR, collector_closed); } diff --git a/digital/io/src/top.c b/digital/io/src/top.c index f358445d..c685af19 100644 --- a/digital/io/src/top.c +++ b/digital/io/src/top.c @@ -35,6 +35,8 @@ top_start (uint8_t color_team) { /* Set parameters. */ top_data.sequence = 0x15; + top_data.boxes_used = 0x0; + top_data.sequence_to_do = 0x15; /* Get the team color. * !! I supose that the 0 value is 0 and red value is 1 */ top_data.team_color = color_team; diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 1da78745..21a6923f 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -4,9 +4,10 @@ top States: START GET_SAMPLES - GET_ICE GO_TO_GOAL + GET_ICE GET_ADV_ICE + BACKWARD Events: ok @@ -14,39 +15,33 @@ Events: collector_full ice_took samples_deposed - samples_took_ice_distributor_empty_not_enough_time - samples_took_ice_distributor_empty_enough_time - samples_deposed_ice_distributor_empty - ice_took_collector_full - not_enough_time - ice_took_collector_not_full + 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 -> GET_ICE + samples_took -> BACKWARD The samples had been took and now the ice is missing. - samples_took_ice_distributor_empty_not_enough_time -> GO_TO_GOAL - Some samples had been took and there is not enough time to get some ice. - samples_took_ice_distributor_empty_enough_time -> GET_ADV_ICE - Go to take ice in the adversary distributor because the one in our part of the table is empty. - collector_full -> GO_TO_GOAL - All the room in the robot are full. GET_ICE: - ice_took -> GO_TO_GOAL - The ice had been taken and the collector is full. The robot shall go to depose it into the goal. + 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. - samples_deposed_ice_distributor_empty -> GET_ADV_ICE - Go to get some adversary ice because our distributor is empty. GET_ADV_ICE: - ice_took -> GO_TO_GOAL - The ice has been taken, now the robot shall depose it. - ice_took_collector_not_full -> GET_SAMPLES - There is enough time to get some samples and go to the goal. + 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. diff --git a/digital/io/src/top.h b/digital/io/src/top.h index 169cf278..aa05c07b 100644 --- a/digital/io/src/top.h +++ b/digital/io/src/top.h @@ -41,6 +41,14 @@ enum team_color_e RED_TEAM }; +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. @@ -50,6 +58,10 @@ struct top_data_t * 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; /** The color of the balls the robot shall take. * RED_TEAM or BLUE_TEAM. */ uint8_t team_color; diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index bf92b204..bb2f6c1d 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -7,88 +7,104 @@ */ #include "common.h" #include "fsm.h" -#include "top.h" #include "top_cb.h" +#include "top.h" #include "getsamples.h" +#include "asserv.h" +#include "gutter.h" /* - * START =ok=> - * => GET_SAMPLES - * Go to take some samples. The sequence shall be adapt to take the correct number of samples. - */ -fsm_branch_t -top__START__ok (void) -{ - // Call the get samples state machine. - - // Blue color. - if (top_data.team_color == BLUE_TEAM) - { - getsamples_start (BLUE_DISTRIBUTOR_X, BLUE_DISTRIBUTOR_Y, - top_data.sequence); - } - else - { - getsamples_start (RED_DISTRIBUTOR_X, RED_DISTRIBUTOR_Y, - top_data.sequence); - } - - return top_next (START, ok); -} - -/* - * GET_SAMPLES =samples_took_ice_distributor_empty_not_enough_time=> + * BACKWARD =collector_full=> * => GO_TO_GOAL - * Some samples had been took and there is not enough time to get some ice. + * Go the goal to depose the samples. */ fsm_branch_t -top__GET_SAMPLES__samples_took_ice_distributor_empty_not_enough_time (void) +top__BACKWARD__collector_full (void) { - return top_next (GET_SAMPLES, samples_took_ice_distributor_empty_not_enough_time); + gutter_start (); + top_data.sequence_to_do = (~top_data.sequence_to_do) & 0x1f; + return top_next (BACKWARD, collector_full); } /* - * GET_SAMPLES =samples_took=> + * BACKWARD =ended=> * => GET_ICE - * The samples had been took and now the ice is missing. + * Get ICE to end the sequence. */ fsm_branch_t -top__GET_SAMPLES__samples_took (void) +top__BACKWARD__ended (void) { - return top_next (GET_SAMPLES, samples_took); + if (top_data.team_color) + { + getsamples_start ( ICE_DISTRIBUTOR_LEFT, ICE_DISTRIBUTOR_Y, + ~top_data.boxes_used, TOP_EVENT_ice_took); + } + else + { + getsamples_start ( ICE_DISTRIBUTOR_RIGHT, ICE_DISTRIBUTOR_Y, + ~top_data.boxes_used, TOP_EVENT_ice_took); + } + return top_next (BACKWARD, ended); } /* - * GET_SAMPLES =collector_full=> - * => GO_TO_GOAL - * All the room in the robot are full. + * BACKWARD =ice_dist_empty=> + * => GET_ADV_ICE + * Go to take the ice in the adversary distributor because our is empty. */ fsm_branch_t -top__GET_SAMPLES__collector_full (void) +top__BACKWARD__ice_dist_empty (void) { - return top_next (GET_SAMPLES, collector_full); + if (top_data.team_color) + { + getsamples_start ( ICE_DISTRIBUTOR_RIGHT, ICE_DISTRIBUTOR_Y, + ~top_data.boxes_used, TOP_EVENT_ice_took); + } + else + { + getsamples_start ( ICE_DISTRIBUTOR_LEFT, ICE_DISTRIBUTOR_Y, + ~top_data.boxes_used, TOP_EVENT_ice_took); + } + + return top_next (BACKWARD, ice_dist_empty); } /* - * GET_SAMPLES =samples_took_ice_distributor_empty_enough_time=> - * => GET_ADV_ICE - * Go to take ice in the adversary distributor because the one in our part of the table is empty. + * GET_ICE =ice_took=> + * => BACKWARD + * Go backward to end the take balls. */ fsm_branch_t -top__GET_SAMPLES__samples_took_ice_distributor_empty_enough_time (void) +top__GET_ICE__ice_took (void) { - return top_next (GET_SAMPLES, samples_took_ice_distributor_empty_enough_time); + asserv_position_t pos; + + asserv_get_position (&pos); + + asserv_goto (pos.x - 200, pos.y - 200); + return top_next (GET_ICE, ice_took); } /* - * GO_TO_GOAL =samples_deposed_ice_distributor_empty=> - * => GET_ADV_ICE - * Go to get some adversary ice because our distributor is empty. + * START =ok=> + * => GET_SAMPLES + * Go to take some samples. The sequence shall be adapt to take the correct number of samples. */ fsm_branch_t -top__GO_TO_GOAL__samples_deposed_ice_distributor_empty (void) +top__START__ok (void) { - return top_next (GO_TO_GOAL, samples_deposed_ice_distributor_empty); + if (top_data.team_color == BLUE_TEAM) + { + getsamples_start ( BLUE_DISTRIBUTOR_X, BLUE_DISTRIBUTOR_Y , + ~top_data.boxes_used, TOP_EVENT_samples_took); + } + else + { + getsamples_start ( RED_DISTRIBUTOR_X, RED_DISTRIBUTOR_Y, + ~top_data.boxes_used, TOP_EVENT_samples_took); + } + + return top_next (START, ok); } /* @@ -99,40 +115,58 @@ top__GO_TO_GOAL__samples_deposed_ice_distributor_empty (void) fsm_branch_t top__GO_TO_GOAL__samples_deposed (void) { + if (top_data.team_color == BLUE_TEAM) + { + getsamples_start ( BLUE_DISTRIBUTOR_X, BLUE_DISTRIBUTOR_Y , + ~top_data.boxes_used, TOP_EVENT_samples_took); + } + else + { + getsamples_start ( RED_DISTRIBUTOR_X, RED_DISTRIBUTOR_Y, + ~top_data.boxes_used, TOP_EVENT_samples_took); + } return top_next (GO_TO_GOAL, samples_deposed); } /* - * GET_ICE =ice_took=> - * => GO_TO_GOAL - * The ice had been taken and the collector is full. The robot shall go to depose it into the goal. + * GET_ADV_ICE =ice_took=> + * => BACKWARD + * The ice has been taken. */ fsm_branch_t -top__GET_ICE__ice_took (void) +top__GET_ADV_ICE__ice_took (void) { - return top_next (GET_ICE, ice_took); -} + asserv_position_t pos; -/* - * GET_ADV_ICE =ice_took_collector_not_full=> - * => GET_SAMPLES - * There is enough time to get some samples and go to the goal. - */ -fsm_branch_t -top__GET_ADV_ICE__ice_took_collector_not_full (void) -{ - return top_next (GET_ADV_ICE, ice_took_collector_not_full); + asserv_get_position (&pos); + + asserv_goto (pos.x - 200, pos.y - 200); + return top_next (GET_ADV_ICE, ice_took); } /* - * GET_ADV_ICE =ice_took=> - * => GO_TO_GOAL - * The ice has been taken, now the robot shall depose it. + * GET_SAMPLES =samples_took=> + * => BACKWARD + * The samples had been took and now the ice is missing. */ fsm_branch_t -top__GET_ADV_ICE__ice_took (void) +top__GET_SAMPLES__samples_took (void) { - return top_next (GET_ADV_ICE, ice_took); + // Call the get samples state machine. + + // Blue color. + if (top_data.team_color == BLUE_TEAM) + { + getsamples_start (BLUE_DISTRIBUTOR_X, BLUE_DISTRIBUTOR_Y, + top_data.sequence, TOP_EVENT_samples_took); + } + else + { + getsamples_start (RED_DISTRIBUTOR_X, RED_DISTRIBUTOR_Y, + top_data.sequence, TOP_EVENT_samples_took); + } + + return top_next (START, ok); } -- cgit v1.2.3