summaryrefslogtreecommitdiff
path: root/digital/io
diff options
context:
space:
mode:
authorJérémy Dufour2008-05-01 15:41:42 +0200
committerJérémy Dufour2008-05-01 15:41:42 +0200
commit26405c3578bfc4877f250ef10190693e7e09df76 (patch)
tree9173209d4643dd2da4b63b5456815c8176fe0bd6 /digital/io
parent5a1f82f99f570b274c00c7ec4eb26cb0f8486657 (diff)
* digital/io/src
- add a state to go backward a litte bit after approaching a distributor to ease the get samples.
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/getsamples.fsm6
-rw-r--r--digital/io/src/getsamples_cb.c33
2 files changed, 31 insertions, 8 deletions
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
@@ -37,6 +37,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.
*/
extern struct getsamples_data_t getsamples_data_;
@@ -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);
}