From 203525d84bafff3b1f9fcfff4b0f51bb45c7fa0e Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Mon, 14 Apr 2008 12:04:45 +0200 Subject: * digital/io/src - add notifier for the desired arm position ; - integrate it in the get samples FSM. --- digital/io/src/asserv.c | 26 ++++++++++++++++++++++++++ digital/io/src/asserv.h | 20 ++++++++++++++++++++ digital/io/src/getsamples_cb.c | 6 ++++++ digital/io/src/giboulee.h | 6 ++++++ digital/io/src/main.c | 13 +++++++------ 5 files changed, 65 insertions(+), 6 deletions(-) (limited to 'digital/io') 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 @@ -88,6 +88,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 @@ -54,6 +54,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 */ } } -- cgit v1.2.3