From 23bce6a75a17b9cd9d3f35a251baa1b0635697a4 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Sat, 3 May 2008 12:43:19 +0200 Subject: * digital/io/src - manage the event where the bot can be blocked (when moving) in the different FSM, by waiting and trying again ; - ensure we update the classifier (with the servos motor) even when the arm move in a state where it should not. --- digital/io/src/getsamples_cb.c | 112 +++++++++++++++++++++++++++++++++++------ 1 file changed, 98 insertions(+), 14 deletions(-) (limited to 'digital/io/src/getsamples_cb.c') diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c index af7b6ced..d42f05bd 100644 --- a/digital/io/src/getsamples_cb.c +++ b/digital/io/src/getsamples_cb.c @@ -36,9 +36,13 @@ #include "chrono.h" #include "modules/proto/proto.h" +#include "modules/utils/utils.h" #include "io.h" + +uint16_t originate_position_; + /** * The distance to go backward from the distributor. */ @@ -49,6 +53,11 @@ */ #define GET_SAMPLES_ARM_TIMEOUT (5 * 225) +/** + * Move back timeout. + */ +#define GET_SAMPLES_MOVE_AWAY_TIMEOUT (225) + /** * 'Private' get samples data used internaly by the FSM. */ @@ -159,6 +168,19 @@ getsamples__CLOSE_INPUT_HOLE__wait_finished (void) return getsamples_next (CLOSE_INPUT_HOLE, wait_finished); } +/* + * CLOSE_INPUT_HOLE =arm_move_succeed=> + * => IDLE + * tell the top FSM we have finished + */ +fsm_branch_t +getsamples__CLOSE_INPUT_HOLE__arm_move_succeed (void) +{ + /* Tell the top FSM we have finished */ + main_post_event_for_top_fsm = TOP_EVENT_get_samples_fsm_finished; + return getsamples_next (CLOSE_INPUT_HOLE, arm_move_succeed); +} + /* * CLOSE_INPUT_HOLE =arm_pass_noted_position=> * => CLOSE_INPUT_HOLE @@ -173,27 +195,62 @@ getsamples__CLOSE_INPUT_HOLE__arm_pass_noted_position (void) } /* - * CLOSE_INPUT_HOLE =arm_move_succeed=> - * => IDLE - * tell the top FSM we have finished + * WAIT_AND_TRY_AGAIN =wait_finished=> + * => MOVE_AWAY_FROM_DISTRIBUTOR + * compute remaining distance (protection agains 0 & 300) + * try to move away again */ fsm_branch_t -getsamples__CLOSE_INPUT_HOLE__arm_move_succeed (void) +getsamples__WAIT_AND_TRY_AGAIN__wait_finished (void) { - /* Tell the top FSM we have finished */ - main_post_event_for_top_fsm = TOP_EVENT_get_samples_fsm_finished; - return getsamples_next (CLOSE_INPUT_HOLE, arm_move_succeed); + /* Get position */ + asserv_position_t position; + asserv_get_position (&position); + + uint16_t remaining_distance; + + if (getsamples_data_.direction == 0) + { + /* Horizontal */ + remaining_distance = UTILS_ABS (position.x - originate_position_); + } + else + { + /* Vertical */ + remaining_distance = UTILS_ABS (position.y - originate_position_); + } + /* Compute real remaining */ + remaining_distance = PG_DISTANCE_DISTRIBUTOR - remaining_distance; + /* Bound */ + UTILS_BOUND (remaining_distance, 1, PG_DISTANCE_DISTRIBUTOR); + /* Move away */ + asserv_move_linearly (-remaining_distance); + return getsamples_next (WAIT_AND_TRY_AGAIN, wait_finished); } /* * TAKE_SAMPLES =wait_finished=> * => MOVE_AWAY_FROM_DISTRIBUTOR + * store current position * timed out, give up * go backward */ fsm_branch_t getsamples__TAKE_SAMPLES__wait_finished (void) { + /* Get position */ + asserv_position_t position; + asserv_get_position (&position); + if (getsamples_data_.direction == 0) + { + /* Horizontal */ + originate_position_ = position.x; + } + else + { + /* Vertical */ + originate_position_ = position.y; + } /* Go backward */ asserv_move_linearly (-PG_DISTANCE_DISTRIBUTOR); proto_send1b ('M', 1); @@ -203,6 +260,7 @@ getsamples__TAKE_SAMPLES__wait_finished (void) /* * TAKE_SAMPLES =arm_pass_noted_position=> * no_more => MOVE_AWAY_FROM_DISTRIBUTOR + * store current position * go backward * more => TAKE_SAMPLES * prepare the classification of the taken sample @@ -228,6 +286,19 @@ getsamples__TAKE_SAMPLES__arm_pass_noted_position (void) } else { + /* Get position */ + asserv_position_t position; + asserv_get_position (&position); + if (getsamples_data_.direction == 0) + { + /* Horizontal */ + originate_position_ = position.x; + } + else + { + /* Vertical */ + originate_position_ = position.y; + } /* Go backward */ asserv_move_linearly (-PG_DISTANCE_DISTRIBUTOR); proto_send1b ('M', 0); @@ -250,16 +321,16 @@ getsamples__IDLE__start (void) } /* - * MOVE_AWAY_FROM_DISTRIBUTOR =arm_pass_noted_position=> - * => MOVE_AWAY_FROM_DISTRIBUTOR - * prepare the classification of the taken sample + * MOVE_AWAY_FROM_DISTRIBUTOR =bot_move_failed=> + * => WAIT_AND_TRY_AGAIN + * ask to be wake up in a certain time */ fsm_branch_t -getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__arm_pass_noted_position (void) +getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_failed (void) { - /* Prepare the classification of the taken sample */ - getsamples_configure_classifier (); - return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, arm_pass_noted_position); + /* Post an event for the top FSM to be waked up later */ + main_getsamples_wait_cycle = GET_SAMPLES_MOVE_AWAY_TIMEOUT; + return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, bot_move_failed); } /* @@ -279,6 +350,19 @@ getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void) return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, bot_move_succeed); } +/* + * MOVE_AWAY_FROM_DISTRIBUTOR =arm_pass_noted_position=> + * => MOVE_AWAY_FROM_DISTRIBUTOR + * prepare the classification of the taken sample + */ +fsm_branch_t +getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__arm_pass_noted_position (void) +{ + /* Prepare the classification of the taken sample */ + getsamples_configure_classifier (); + return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, arm_pass_noted_position); +} + /* * APPROACH_DISTRIBUTOR =bot_move_succeed=> * => MOVE_BACKWARD_FROM_DISTRIBUTOR -- cgit v1.2.3