From 689d78b5d4a57456152420b0e3f490480ad32b8f Mon Sep 17 00:00:00 2001 From: Nélio Laranjeiro Date: Mon, 24 Mar 2008 22:29:22 +0100 Subject: Update the FSM for the getsamples. --- digital/io/src/getsamples/Makefile | 13 ++-- digital/io/src/getsamples/getSamplesFsm.conf | 2 +- digital/io/src/getsamples/getsamples_robo.h | 33 ++++++++ digital/io/src/getsamples/test/Makefile | 7 ++ digital/io/src/getsamples/test/main.c | 81 ++++++++++++++++++++ digital/io/src/ia.c | 110 +-------------------------- digital/io/src/ia.h | 54 +++---------- 7 files changed, 139 insertions(+), 161 deletions(-) create mode 100644 digital/io/src/getsamples/getsamples_robo.h create mode 100644 digital/io/src/getsamples/test/Makefile create mode 100644 digital/io/src/getsamples/test/main.c diff --git a/digital/io/src/getsamples/Makefile b/digital/io/src/getsamples/Makefile index 3cf5a03a..389adc71 100644 --- a/digital/io/src/getsamples/Makefile +++ b/digital/io/src/getsamples/Makefile @@ -1,10 +1,6 @@ CFLAGS = -O2 -Wall -all: getSamplesFsm.fsm getSamplesFsm.conf - make prgm - make exe - make dot - make png +all: prgm exe dot png prgm: getSamplesFsm.fsm getSamplesFsm.conf python ../../../../tools/dfagen/dfagen.py -o c -d $< -c getSamplesFsm.conf -p getsamples @@ -20,4 +16,9 @@ png: getsamples.dot dot -Tpng -o getsamples.png getsamples.dot clean: - rm -f getsamples* + rm -f getsamples.h + rm -f getsamples_cb.c + rm -f getsamples_cb.h + rm -f getsamples.dot + rm -f getsamples.png + rm -f getsamples.c diff --git a/digital/io/src/getsamples/getSamplesFsm.conf b/digital/io/src/getsamples/getSamplesFsm.conf index bae8f698..238c0f6f 100644 --- a/digital/io/src/getsamples/getSamplesFsm.conf +++ b/digital/io/src/getsamples/getSamplesFsm.conf @@ -1,5 +1,5 @@ [user] type = getsamples_t type-forward-decl = typedef struct getsamples_t getsamples_t; -type-decl = struct getsamples_t { getsamples_state_t fsm; }; +type-decl = #include "getsamples_robo.h" field = fsm diff --git a/digital/io/src/getsamples/getsamples_robo.h b/digital/io/src/getsamples/getsamples_robo.h new file mode 100644 index 00000000..d5ee6903 --- /dev/null +++ b/digital/io/src/getsamples/getsamples_robo.h @@ -0,0 +1,33 @@ +#ifndef getsamples_robo_h +#define getsamples_robo_h +/* getsamples_robo.h */ +/* {{{ + * + * 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. + * + * }}} */ + +struct getsamples_t +{ + getsamples_state_t fsm; +}; + +#endif /* getsamples_robo_h */ diff --git a/digital/io/src/getsamples/test/Makefile b/digital/io/src/getsamples/test/Makefile new file mode 100644 index 00000000..ad659fdd --- /dev/null +++ b/digital/io/src/getsamples/test/Makefile @@ -0,0 +1,7 @@ +BASE= .. + +all: main.c $(BASE)/getsamples.c $(BASE)/getsamples_cb.c + gcc -g -O2 -Wall main.c $(BASE)/getsamples.c $(BASE)/getsamples_cb.c -o fsm + +clean: fsm + rm -f fsm diff --git a/digital/io/src/getsamples/test/main.c b/digital/io/src/getsamples/test/main.c new file mode 100644 index 00000000..2bbfe6e6 --- /dev/null +++ b/digital/io/src/getsamples/test/main.c @@ -0,0 +1,81 @@ +/* main.c */ +/* {{{ + * + * 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 "../getsamples.h" +#include "../getsamples_robo.h" + +#include + +void +getsamples_print_test (getsamples_t *getsamples) +{ + printf ("Machine state "); + + switch (getsamples->fsm) + { + case GETSAMPLES_STATE_PREPARE_ARM: + printf ("PREPARE_ARM"); + break; + case GETSAMPLES_STATE_END: + printf ("END"); + break; + case GETSAMPLES_STATE_FORWARD_CONTROL: + printf ("FORWARD CONTROL"); + break; + case GETSAMPLES_STATE_TAKE_SAMPLES: + printf ("TAKE_SAMPLES"); + break; + case GETSAMPLES_STATE_BACKWARD: + printf ("BACKWARD"); + break; + case GETSAMPLES_STATE_GO_TO_POSITION: + printf ("GO TO POSITION"); + break; + case GETSAMPLES_STATE_NB: + printf ("NB"); + } + printf ("\n"); +} + +int +main (void) +{ + getsamples_t getsamples_fsm; + + getsamples_init (&getsamples_fsm); + getsamples_print_test (&getsamples_fsm); + + getsamples_handle_event (&getsamples_fsm, + GETSAMPLES_EVENT_position_failed); + + getsamples_print_test (&getsamples_fsm); + + getsamples_handle_event (&getsamples_fsm, + GETSAMPLES_EVENT_position_reached); + + + getsamples_print_test (&getsamples_fsm); + + return 0; +} diff --git a/digital/io/src/ia.c b/digital/io/src/ia.c index 7ede1888..954459bc 100644 --- a/digital/io/src/ia.c +++ b/digital/io/src/ia.c @@ -35,114 +35,6 @@ ia_init (void) { ia_global.sequence = false; ia_global.ice_status_our = false; + ia_global.team_color = TEAM_COLOR_RED; } -/** Load balls procedure from a distributor. - * - * 1. Rotate the arm to the desired position to allow the robot to load x - * balls. - * 2. Go backward, this will allow the robot to continue rotating the arm and - * load the balls. - * 3. Stop the arm and put it to its initial position to disallow the robot - * to take undesired balls. - * - * See trunk/digital/io/doc/loadballs.png (use the make command before) - * - * \param balls_nb the quantity of ball to load. - */ -void -ia_load_samples (uint8_t balls_nb) -{ - /* Start the rotation of the arm */ - asserv_move_arm (ASSERV_ARM_ROTATION_FULL , ASSERV_ARM_SPEED_HALF); - - /* Activate the classifier. */ - // TODO write the code to use the classifier. How to know when the - // quantity of desired samples are loaded ? - - /* At this moment the samples shall be loaded. */ - /* Go backaward */ - asserv_move_linearly (-10); - - /* Set the arm to the initial position. */ - asserv_move_arm (ASSERV_ARM_ROTATION_INIT, ASSERV_ARM_SPEED_FULL); -} - -/** Get samples procedure. Request the robot to go and get some sample of the - * team color or ice samples. - * - * 1. Go to the position in front of the distributor. - * 2. Prepare the arm to get samples. - * 3. Go forward with a controled speed to get the samples. - * 4. Prepare the classifier to classify the samples. - * 5. loadballs with the quantity of samples desired. - * 6. Continue classifier - * - * See trunk/digital/io/doc/getSamples.png and - * trunk/digital/io/doc/loadballs.png (use make to get the png files). - * - * \param blue the team color true if the robot is in the blue team. - */ -void -ia_get_samples (bool blue, bool ice) -{ - /* Set the ARM to the init position to avoid the robot to take samples on - * the distributor path. */ - asserv_move_arm (ASSERV_ARM_ROTATION_INIT, ASSERV_ARM_ROTATION_FULL); - - /* Go to the distributor. */ - - if (ice) - { - //TODO this will only work when the robot is in the red team, the need to - //know at which position the robot starts is necessary. - if (blue) - asserv_set_x_position (DISTRIBUTOR_SAMPLES_BLUE_X); - else - asserv_set_x_position (DISTRIBUTOR_SAMPLES_RED_X); - - asserv_set_y_position (DISTRIBUTRO_SAMPLES_Y - 100); - } - else - { - /* Go to the ice distributor. */ - if (ia_global.ice_status_our) - asserv_set_x_position (0); - else - asserv_set_x_position (TABLE_MAX_X); - - asser_set_y_position (DISTRIBUTOR_ICE_Y); - } - - - /* Set the classifier to the correct position. */ - /* TODO fill this part */ - - /* Poll for the position. */ - /* TODO A function for that ?? */ - - ia_load_samples (3 /* TODO change this value to the correct one.*/); -} - -/** Get ice. - */ -void -ia_get_ice (void) -{ - load_samples (true, true); -} - - -/** Depose the samples in the gutter. - */ -void -ia_depose_samples (void) -{ - asserv_go_to_gutter(); - - /*TODO open the collector. */ - - utils_delay_ms (4); - - /* TODO close the collector. */ -} diff --git a/digital/io/src/ia.h b/digital/io/src/ia.h index 7ed9a571..3a11ed30 100644 --- a/digital/io/src/ia.h +++ b/digital/io/src/ia.h @@ -43,6 +43,12 @@ #define ASSERV_ARM_SPEED_FULL 100 #define ASSERV_ARM_SPEED_HALF (ASSERV_ARM_SPEED_FULL / 2) +enum ia_team_e +{ + TEAM_COLOR_RED, + TEAM_COLOR_BLUE +}; + struct ia_t { /* Bool status of the previous sequence loaded. @@ -55,6 +61,9 @@ struct ia_t /* Bool status indicating our ice distributor status. */ bool ice_status_our; + + /* Team color */ + bool team_color; }; @@ -63,49 +72,4 @@ struct ia_t void ia_init (void); -/** Load balls procedure from a distributor. - * - * 1. Rotate the arm to the desired position to allow the robot to load x - * balls. - * 2. Go backward, this will allow the robot to continue rotating the arm and - * load the balls. - * 3. Stop the arm and put it to its initial position to disallow the robot - * to take undesired balls. - * - * See trunk/digital/io/doc/loadballs.png (use the make command before) - * - * \param balls_nb the quantity of ball to load. - */ -void -ia_load_samples (uint8_t balls_nb); - -/** Get samples procedure. Request the robot to go and get some sample of the - * team color or ice samples. - * - * 1. Go to the position in front of the distributor. - * 2. Prepare the arm to get samples. - * 3. Go forward with a controled speed to get the samples. - * 4. Prepare the classifier to classify the samples. - * 5. loadballs with the quantity of samples desired. - * 6. Continue classifier - * - * See trunk/digital/io/doc/getSamples.png and - * trunk/digital/io/doc/loadballs.png (use make to get the png files). - * - * \param blue the team color true if the robot is in the blue team. - */ -void -ia_get_samples (bool blue); - -/** Get ice. - */ -void -ia_get_ice (void); - -/** Depose the samples in the gutter. - */ -void -ia_depose_samples (void); - - #endif /* ia_h */ -- cgit v1.2.3