summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/getsamples_cb.c
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-22 21:57:24 +0200
committerNicolas Schodet2008-04-22 21:57:24 +0200
commita07b9473d9c95904a9f0ff9e6bb5f9431d8735d2 (patch)
tree49f4ac8e2f3ea7b0832270e271af9027fd8e75ce /digital/io/src/getsamples_cb.c
parent157aee8d63e31da7687492601f3efe5bed21f4ad (diff)
* digital/io/src:
- made the arm control work.
Diffstat (limited to 'digital/io/src/getsamples_cb.c')
-rw-r--r--digital/io/src/getsamples_cb.c31
1 files changed, 21 insertions, 10 deletions
diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c
index 3ade76a7..57f088eb 100644
--- a/digital/io/src/getsamples_cb.c
+++ b/digital/io/src/getsamples_cb.c
@@ -70,6 +70,17 @@ getsamples_configure_classifier (void)
}
}
+/* Count the number of samples to take. */
+uint8_t
+getsamples_count_samples (void)
+{
+ uint8_t i, bit, count = 0;
+ for (i = 0, bit = 1; i < 8; i++, bit <<= 1)
+ if (getsamples_data_.sample_bitfield & bit)
+ count++;
+ return count;
+}
+
/*
* FACE_DISTRIBUTOR =bot_move_succeed=>
* => OPEN_INPUT_HOLE
@@ -120,19 +131,15 @@ getsamples__CLOSE_INPUT_HOLE__arm_move_succeed (void)
fsm_branch_t
getsamples__TAKE_SAMPLES__arm_pass_noted_position (void)
{
+ /* Prepare classification */
+ getsamples_configure_classifier ();
/* More samples? */
if (getsamples_data_.sample_bitfield)
{
/* Compute notifier */
- uint16_t arm_current_position = asserv_get_arm_position ();
- uint16_t arm_notify_position =
- arm_current_position + BOT_ARM_NOTED_POSITION -
- (arm_current_position % BOT_ARM_THIRD_ROUND);
- asserv_arm_set_position_reached (arm_notify_position);
- /* Prepare classification */
- getsamples_configure_classifier ();
+ getsamples_data_.arm_noted_position += BOT_ARM_THIRD_ROUND;
+ asserv_arm_set_position_reached (getsamples_data_.arm_noted_position);
/* Continue to take sample */
- asserv_move_arm (BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED);
return getsamples_next_branch (TAKE_SAMPLES, arm_pass_noted_position, more);
}
else
@@ -165,7 +172,7 @@ fsm_branch_t
getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void)
{
/* Move the arm to close the input hole */
- asserv_close_input_hole ();
+ asserv_move_arm (BOT_ARM_MIN_TO_OPEN, BOT_ARM_SPEED);
return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, bot_move_succeed);
}
@@ -178,6 +185,10 @@ fsm_branch_t
getsamples__APPROACH_DISTRIBUTOR__bot_move_succeed (void)
{
/* start taking some samples */
- asserv_move_arm (BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED);
+ 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 (APPROACH_DISTRIBUTOR, bot_move_succeed);
}