From 26405c3578bfc4877f250ef10190693e7e09df76 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Thu, 1 May 2008 15:41:42 +0200 Subject: * digital/io/src - add a state to go backward a litte bit after approaching a distributor to ease the get samples. --- digital/io/src/getsamples.fsm | 6 ++++++ digital/io/src/getsamples_cb.c | 33 +++++++++++++++++++++++++-------- 2 files changed, 31 insertions(+), 8 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm index 1fdfaa98..2f495626 100644 --- a/digital/io/src/getsamples.fsm +++ b/digital/io/src/getsamples.fsm @@ -23,6 +23,8 @@ States: move the bot away from the distributor by going backward CLOSE_INPUT_HOLE put the arm back in front of the input hole to prevent sample to enter + MOVE_BACKWARD_FROM_DISTRIBUTOR + move a little bit backward from the distributor Events: start @@ -51,6 +53,10 @@ OPEN_INPUT_HOLE: start approaching the distributor now APPROACH_DISTRIBUTOR: + bot_move_succeed -> MOVE_BACKWARD_FROM_DISTRIBUTOR + move a little bit backward from the distributor + +MOVE_BACKWARD_FROM_DISTRIBUTOR: bot_move_succeed -> TAKE_SAMPLES start taking some samples diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c index 7e052393..3099452f 100644 --- a/digital/io/src/getsamples_cb.c +++ b/digital/io/src/getsamples_cb.c @@ -36,6 +36,11 @@ #include "io.h" +/** + * The distance to go backward from the distributor. + */ +#define GET_SAMPLES_MOVE_BACKWARD_DISTANCE 7 + /** * 'Private' get samples data used internaly by the FSM. */ @@ -81,6 +86,22 @@ getsamples_count_samples (void) count++; return count; } +/* + * MOVE_BACKWARD_FROM_DISTRIBUTOR =bot_move_succeed=> + * => TAKE_SAMPLES + * start taking some samples + */ +fsm_branch_t +getsamples__MOVE_BACKWARD_FROM_DISTRIBUTOR__bot_move_succeed (void) +{ + /* start taking some samples */ + getsamples_data_.arm_noted_position = asserv_get_arm_position () + + BOT_ARM_NOTED_POSITION; + asserv_arm_set_position_reached (getsamples_data_.arm_noted_position); + asserv_move_arm (getsamples_count_samples () * BOT_ARM_THIRD_ROUND, + BOT_ARM_SPEED); + return getsamples_next (MOVE_BACKWARD_FROM_DISTRIBUTOR, bot_move_succeed); +} /* * FACE_DISTRIBUTOR =bot_move_succeed=> @@ -179,17 +200,13 @@ getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void) /* * APPROACH_DISTRIBUTOR =bot_move_succeed=> - * => TAKE_SAMPLES - * start taking some samples + * => MOVE_BACKWARD_FROM_DISTRIBUTOR + * move a little bit backward from the distributor */ fsm_branch_t getsamples__APPROACH_DISTRIBUTOR__bot_move_succeed (void) { - /* start taking some samples */ - getsamples_data_.arm_noted_position = asserv_get_arm_position () + - BOT_ARM_NOTED_POSITION; - asserv_arm_set_position_reached (getsamples_data_.arm_noted_position); - asserv_move_arm (getsamples_count_samples () * BOT_ARM_THIRD_ROUND, - BOT_ARM_SPEED); + /* Move a little bit backward from the distributor */ + asserv_move_linearly (-GET_SAMPLES_MOVE_BACKWARD_DISTANCE); return getsamples_next (APPROACH_DISTRIBUTOR, bot_move_succeed); } -- cgit v1.2.3