summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/asserv.c26
-rw-r--r--digital/io/src/asserv.h20
-rw-r--r--digital/io/src/getsamples_cb.c6
-rw-r--r--digital/io/src/giboulee.h6
-rw-r--r--digital/io/src/main.c13
5 files changed, 65 insertions, 6 deletions
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
index 0b801a13..60720f34 100644
--- a/digital/io/src/asserv.c
+++ b/digital/io/src/asserv.c
@@ -89,6 +89,11 @@ static uint8_t asserv_retransmit_counter;
static uint8_t asserv_retransmit_length;
/**
+ * The arm position of notification for the get sample FSM.
+ */
+static uint16_t asserv_arm_notify_position;
+
+/**
* Update TWI module until request (send or receive) is finished.
* This functions is blocking.
*/
@@ -475,3 +480,24 @@ asserv_goto (uint32_t x, uint32_t y)
{
}
+/* Notify get samples FSM when the arm reach desired position. */
+void
+asserv_arm_set_position_reached (uint16_t position)
+{
+ /* Store the position */
+ asserv_arm_notify_position = position;
+}
+
+/* Check if notification of the get sample FSM is required in term of position
+ * of the arm.
+ */
+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))
+ return 1;
+ return 0;
+}
diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h
index 46448a59..1a5011fc 100644
--- a/digital/io/src/asserv.h
+++ b/digital/io/src/asserv.h
@@ -264,4 +264,24 @@ asserv_set_speed (uint8_t linear_high, uint8_t angular_high,
void
asserv_goto (uint32_t x, uint32_t y);
+/**
+ * Set the notifier of get samples FSM when the arm reach desired position.
+ * You should called this function from the get sample FSM to tell the asserv
+ * module you want to be notified when the arm reached the desired position.
+ * @param position the desired position of the arm (absolute). Note, 0 is a
+ * reset value do disable the notifier. If you want to use it, please add 1.
+ */
+void
+asserv_arm_set_position_reached (uint16_t position);
+
+/**
+ * Check if notification of the get sample FSM is required in term of
+ * position of the arm.
+ * @return
+ * - 0 if the notification is not needed ;
+ * - 1 if the notification of the get sample FSM is required.
+ */
+uint8_t
+asserv_arm_position_reached (void);
+
#endif /* asserv_h */
diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c
index 5e963ce6..85dca001 100644
--- a/digital/io/src/getsamples_cb.c
+++ b/digital/io/src/getsamples_cb.c
@@ -86,6 +86,12 @@ getsamples__TAKE_SAMPLES__arm_pass_noted_position (void)
/* 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 ();
/* Continue to take sample */
diff --git a/digital/io/src/giboulee.h b/digital/io/src/giboulee.h
index bda60c8f..aa8309dd 100644
--- a/digital/io/src/giboulee.h
+++ b/digital/io/src/giboulee.h
@@ -55,6 +55,12 @@
#define BOT_ARM_MIN_TO_OPEN (BOT_ARM_STEP_ROUND / 100 * 15)
/**
+ * Steps number relative to the beginning of a third round for the 'noted'
+ * position required by the get sample FSM.
+ */
+#define BOT_ARM_NOTED_POSITION (BOT_ARM_THIRD_ROUND / 3 * 2)
+
+/**
* How to compute a angle for giboulee?
* One degree is 65536 / 360
*/
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index d03714c4..f2bd5355 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -125,12 +125,13 @@ main_loop (void)
}
/* TODO: Check if the sensor placed at the noted position has seen
* an arm passed and forward this event to the getsamples FSM */
-// if (arm_in_front_of_reached_position)
-// {
-// /* Reset the sensor back to see a new transit of the arm */
-// fsm_handle_event (&getsamples_fsm,
-// GETSAMPLES_EVENT_arm_pass_noted_position);
-// }
+ if (asserv_arm_position_reached ())
+ {
+ /* Reset the notifier */
+ asserv_arm_set_position_reached (0);
+ fsm_handle_event (&getsamples_fsm,
+ GETSAMPLES_EVENT_arm_pass_noted_position);
+ }
/* Check other sensors */
}
}