summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src')
-rw-r--r--digital/io/src/asserv.c14
-rw-r--r--digital/io/src/getsamples.h2
-rw-r--r--digital/io/src/getsamples_cb.c31
-rw-r--r--digital/io/src/giboulee.h4
-rw-r--r--digital/io/src/main.c4
5 files changed, 28 insertions, 27 deletions
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
index edf08a5d..9addd320 100644
--- a/digital/io/src/asserv.c
+++ b/digital/io/src/asserv.c
@@ -424,15 +424,6 @@ asserv_move_arm (int16_t offset, uint8_t speed)
asserv_move_arm_absolute (asserv_arm_current_position, speed);
}
-/* Move the arm to close the input hole. */
-void
-asserv_close_input_hole (void)
-{
- /* Move the arm */
- asserv_move_arm (asserv_arm_current_position %
- BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED);
-}
-
/* Set current X position. */
void
asserv_set_x_position (int32_t x)
@@ -551,9 +542,10 @@ uint8_t
asserv_arm_position_reached (void)
{
/* If the position has been reached */
- /* TODO: manage overflow! */
if (asserv_arm_notify_position &&
- (asserv_get_arm_position () >= asserv_arm_notify_position))
+ ((int16_t) (asserv_get_arm_position ()
+ - asserv_arm_notify_position) >= 0))
return 1;
return 0;
}
+
diff --git a/digital/io/src/getsamples.h b/digital/io/src/getsamples.h
index 70a76696..e927397e 100644
--- a/digital/io/src/getsamples.h
+++ b/digital/io/src/getsamples.h
@@ -45,6 +45,8 @@ struct getsamples_data_t
* distributor.
*/
uint8_t sample_bitfield;
+ /** Position of the arm where we want to be awken. */
+ uint16_t arm_noted_position;
};
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);
}
diff --git a/digital/io/src/giboulee.h b/digital/io/src/giboulee.h
index d8f93ef0..0dae6098 100644
--- a/digital/io/src/giboulee.h
+++ b/digital/io/src/giboulee.h
@@ -50,9 +50,9 @@
/**
* The number of steps to do to open the input hole when the arm is closing
* it.
- * For the moment, 15% of a complete turn
+ * For the moment, 30 degree.
*/
-#define BOT_ARM_MIN_TO_OPEN (BOT_ARM_STEP_ROUND * 15L / 100)
+#define BOT_ARM_MIN_TO_OPEN (BOT_ARM_STEP_ROUND * 30L / 360)
/**
* Steps number relative to the beginning of a third round for the 'noted'
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 73b41196..883bb25d 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -256,10 +256,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
/* Go to the distributor */
asserv_go_to_distributor ();
break;
- case 'c':
- /* Close the input hole with the arm */
- asserv_close_input_hole ();
- break;
}
}
break;