summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/getsamples_cb.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src/getsamples_cb.c')
-rw-r--r--digital/io/src/getsamples_cb.c33
1 files changed, 25 insertions, 8 deletions
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);
}