From fd563cc503df214e7402a11aef187c69ffd0f60f Mon Sep 17 00:00:00 2001 From: NĂ©lio Laranjeiro Date: Sat, 29 Mar 2008 15:31:02 +0100 Subject: Update the getsamples fsm, the value inserted in the functions call shall be adapt before unsing it. --- digital/io/src/getsamples/Makefile | 3 +- digital/io/src/getsamples/getSamplesFsm.fsm | 6 ++ digital/io/src/getsamples/getsamples_cb.c | 116 ++++++++++++++++++++++++++++ digital/io/src/getsamples/getsamples_robo.h | 7 ++ 4 files changed, 130 insertions(+), 2 deletions(-) create mode 100644 digital/io/src/getsamples/getsamples_cb.c diff --git a/digital/io/src/getsamples/Makefile b/digital/io/src/getsamples/Makefile index 389adc71..c59fdb64 100644 --- a/digital/io/src/getsamples/Makefile +++ b/digital/io/src/getsamples/Makefile @@ -7,7 +7,7 @@ prgm: getSamplesFsm.fsm getSamplesFsm.conf exe: getSamplesFsm.fsm getSamplesFsm.conf make prgm - if [ -f getsamples_cb.c ]; then patch getsamples_cb.c getsamples_cb_skel.c; else mv getsamples_cb_skel.c getsamples_cb.c; fi; + #if [ -f getsamples_cb.c ]; then vimdiff getsamples_cb.c getsamples_cb_skel.c; else mv getsamples_cb_skel.c getsamples_cb.c; fi; dot: getSamplesFsm.fsm getSamplesFsm.conf python ../../../../tools/dfagen/dfagen.py -o dot -d $< -c getSamplesFsm.conf -p getsamples @@ -17,7 +17,6 @@ png: getsamples.dot clean: rm -f getsamples.h - rm -f getsamples_cb.c rm -f getsamples_cb.h rm -f getsamples.dot rm -f getsamples.png diff --git a/digital/io/src/getsamples/getSamplesFsm.fsm b/digital/io/src/getsamples/getSamplesFsm.fsm index b8c31e46..3f7266d0 100644 --- a/digital/io/src/getsamples/getSamplesFsm.fsm +++ b/digital/io/src/getsamples/getSamplesFsm.fsm @@ -2,6 +2,7 @@ get_samples States: + START GO_TO_POSITION PREPARE_ARM FORWARD_CONTROL @@ -10,12 +11,17 @@ States: END Events: + ok position_reached position_failed arm_moved sample_took classifier_ready +START: + ok -> GO_TO_POSITION + Go to the distributor. The distributor to reach shall be setted in the fsm structure. + 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. diff --git a/digital/io/src/getsamples/getsamples_cb.c b/digital/io/src/getsamples/getsamples_cb.c new file mode 100644 index 00000000..de10ac53 --- /dev/null +++ b/digital/io/src/getsamples/getsamples_cb.c @@ -0,0 +1,116 @@ +/* + * THIS IS AN AUTOMATICALLY GENERATED FILE, DO NOT EDIT! + * + * Skeleton for get_samples callbacks implementation. + * + * + */ +#include "getsamples_cb.h" + +/* + * PREPARE_ARM =arm_moved=> + * => FORWARD_CONTROL + * Prepare the arm to the correct position. + */ +getsamples_branch_t +getsamples__PREPARE_ARM__arm_moved (getsamples_t *user) +{ + // Try to end the position to the distributor. + asserv_set_x_position (user->distributor_x); + // Go to the color distributor. + asserv_set_y_position (user->distributor_y); + return getsamples_next (PREPARE_ARM, arm_moved); +} + +/* + * FORWARD_CONTROL =position_reached=> + * => TAKE_SAMPLES + * End the position to the distributor. + */ +getsamples_branch_t +getsamples__FORWARD_CONTROL__position_reached (getsamples_t *user) +{ + // Take as many samples as necessary. This shall be updated to verify the + asserv_move_arm (1666, 100); + return getsamples_next (FORWARD_CONTROL, position_reached); +} + +/* + * START =ok=> + * => GO_TO_POSITION + * Go to the distributor. The distributor to reach shall be setted in the fsm structure. + */ +getsamples_branch_t +getsamples__START__ok (getsamples_t *user) +{ + asserv_set_x_position (user->distributor_x - 20); + asserv_set_y_position (user->distributor_y - 20); + return getsamples_next (START, ok); +} + +/** + * TAKE_SAMPLES =sample_took=> + * no_more => BACKWARD + * If the quantity of samples are tooked, then go backeward and conitnue classifying the samples. + * more => TAKE_SAMPLES + * Continue to take samples and classify the next sample. + */ +getsamples_branch_t +getsamples__TAKE_SAMPLES__sample_took (getsamples_t *user) +{ + // Decrement the samples counter. + if (user->samples) + { + asserv_move_arm (1666, 100); + return getsamples_next_branch (TAKE_SAMPLES, sample_took, more); + } + else + { + asserv_move_arm (5000, 100); + // Try to end the position to the distributor. + asserv_set_x_position (user->distributor_x - 20); + // Go to the color distributor. + asserv_set_y_position (user->distributor_y - 20); + + return getsamples_next_branch (TAKE_SAMPLES, sample_took, no_more); + } +} + +/* + * BACKWARD =position_reached=> + * => END + * Ending this state machine. + */ +getsamples_branch_t +getsamples__BACKWARD__position_reached (getsamples_t *user) +{ + 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. + */ +getsamples_branch_t +getsamples__GO_TO_POSITION__position_failed (getsamples_t *user) +{ + // TODO In this case i don't know what to do. + return getsamples_next (GO_TO_POSITION, position_failed); +} + +/* + * 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. + */ +getsamples_branch_t +getsamples__GO_TO_POSITION__position_reached (getsamples_t *user) +{ + // Put the ARM in the position to allow the robot to take samples from the + // distributor. + asserv_move_arm (625, 100); + return getsamples_next (GO_TO_POSITION, position_reached); +} + + diff --git a/digital/io/src/getsamples/getsamples_robo.h b/digital/io/src/getsamples/getsamples_robo.h index d5ee6903..11309a10 100644 --- a/digital/io/src/getsamples/getsamples_robo.h +++ b/digital/io/src/getsamples/getsamples_robo.h @@ -27,7 +27,14 @@ struct getsamples_t { + /* The FSM. */ getsamples_state_t fsm; + /* Distributor x position to get samples. */ + uint32_t distributor_x; + /* Distributor y position to get samples. */ + uint32_t distributor_y; + /* Samples to take. */ + uint8_t samples; }; #endif /* getsamples_robo_h */ -- cgit v1.2.3