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.c | 4 +- digital/io/src/getsamples.fsm | 11 ++++ digital/io/src/getsamples.h | 4 +- digital/io/src/getsamples_cb.c | 112 +++++++++++++++++++++++++++++++++++------ digital/io/src/gutter.fsm | 9 +++- digital/io/src/gutter_cb.c | 31 ++++++++++++ digital/io/src/main.c | 2 +- digital/io/src/move_cb.c | 16 ++++-- digital/io/src/top_cb.c | 6 +-- 9 files changed, 169 insertions(+), 26 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/getsamples.c b/digital/io/src/getsamples.c index ead49ecc..c1a7dc60 100644 --- a/digital/io/src/getsamples.c +++ b/digital/io/src/getsamples.c @@ -35,11 +35,13 @@ struct getsamples_data_t getsamples_data_; /* Start the get samples FSM. */ void -getsamples_start (int16_t approach_angle, uint8_t sample_bitfield) +getsamples_start (int16_t approach_angle, uint8_t sample_bitfield, uint8_t + direction) { /* Set parameters */ getsamples_data_.approach_angle = approach_angle; getsamples_data_.sample_bitfield = sample_bitfield; + getsamples_data_.direction = direction; /* Remove unhandled traps */ getsamples_data_.sample_bitfield &= (_BV (trap_count) - 1) & ~_BV (garbage); diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm index 175a1824..4b54c5f8 100644 --- a/digital/io/src/getsamples.fsm +++ b/digital/io/src/getsamples.fsm @@ -25,6 +25,8 @@ States: put the arm back in front of the input hole to prevent sample to enter MOVE_BACKWARD_FROM_DISTRIBUTOR move a little bit backward from the distributor + WAIT_AND_TRY_AGAIN + wait and try again Events: start @@ -69,8 +71,10 @@ TAKE_SAMPLES: take a new one reset the timeout arm_pass_noted_position: no_more -> MOVE_AWAY_FROM_DISTRIBUTOR + store current position go backward wait_finished -> MOVE_AWAY_FROM_DISTRIBUTOR + store current position timed out, give up go backward @@ -78,6 +82,8 @@ MOVE_AWAY_FROM_DISTRIBUTOR: bot_move_succeed -> CLOSE_INPUT_HOLE close input hole setup a close timeout + bot_move_failed -> WAIT_AND_TRY_AGAIN + ask to be wake up in a certain time arm_pass_noted_position -> . prepare the classification of the taken sample @@ -89,3 +95,8 @@ CLOSE_INPUT_HOLE: tell the top FSM we have finished arm_pass_noted_position -> . prepare the classification of the taken sample + +WAIT_AND_TRY_AGAIN: + wait_finished -> MOVE_AWAY_FROM_DISTRIBUTOR + compute remaining distance (protection agains 0 & 300) + try to move away again diff --git a/digital/io/src/getsamples.h b/digital/io/src/getsamples.h index e927397e..2adf0cb4 100644 --- a/digital/io/src/getsamples.h +++ b/digital/io/src/getsamples.h @@ -47,6 +47,7 @@ struct getsamples_data_t uint8_t sample_bitfield; /** Position of the arm where we want to be awken. */ uint16_t arm_noted_position; + uint8_t direction; }; @@ -60,6 +61,7 @@ struct getsamples_data_t * used to know the number of samples to collect from the distributor. */ void -getsamples_start (int16_t approach_angle, uint8_t sample_bitfield); +getsamples_start (int16_t approach_angle, uint8_t sample_bitfield, uint8_t + direction); #endif /* getsamples_h */ 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 diff --git a/digital/io/src/gutter.fsm b/digital/io/src/gutter.fsm index 966ee8f0..97163cc0 100644 --- a/digital/io/src/gutter.fsm +++ b/digital/io/src/gutter.fsm @@ -11,6 +11,8 @@ States: make the bot reversing against the gutter DROP_BALLS open the collector to let the balls fall into the gutter + WAIT_AND_TRY_AGAIN + ask the top FSM to wake us up in a few times Events: start @@ -38,9 +40,14 @@ GO_TO_THE_GUTTER_WALL: # preventing from fucking the wall # The best way is probably to abort this FSM with an error, tell the upper one # (top) we need to try at another place (the gutter is quite long). -# bot_move_failed + bot_move_failed -> WAIT_AND_TRY_AGAIN + ask the top FSM to wake us up in a few times DROP_BALLS: wait_finished -> IDLE close the rear panel tell the top FSM we have finished + +WAIT_AND_TRY_AGAIN: + wait_finished -> GO_TO_THE_GUTTER_WALL + try the fuck the wall again diff --git a/digital/io/src/gutter_cb.c b/digital/io/src/gutter_cb.c index 8d11a63a..96f5c605 100644 --- a/digital/io/src/gutter_cb.c +++ b/digital/io/src/gutter_cb.c @@ -42,6 +42,11 @@ uint16_t gutter_wait_cycle_; */ #define GUTTER_WAIT_FOR_BALLS_TO_DROP 300 +/** + * Time to wait before trying the go to the wall again. + */ +#define GUTTER_WAIT_BEFORE_TRY_AGAIN (225) + /* * ROTATE_REAR_SIDE_TO_GUTTER =bot_move_succeed=> * => GO_TO_THE_GUTTER_WALL @@ -55,6 +60,19 @@ gutter__ROTATE_REAR_SIDE_TO_GUTTER__bot_move_succeed (void) return gutter_next (ROTATE_REAR_SIDE_TO_GUTTER, bot_move_succeed); } +/* + * WAIT_AND_TRY_AGAIN =wait_finished=> + * => GO_TO_THE_GUTTER_WALL + * try the fuck the wall again + */ +fsm_branch_t +gutter__WAIT_AND_TRY_AGAIN__wait_finished (void) +{ + /* Make the bot reversing against the gutter */ + asserv_go_to_the_wall (); + return gutter_next (WAIT_AND_TRY_AGAIN, wait_finished); +} + /* * IDLE =start=> * => ROTATE_REAR_SIDE_TO_GUTTER @@ -70,6 +88,18 @@ gutter__IDLE__start (void) return gutter_next (IDLE, start); } +/* + * GO_TO_THE_GUTTER_WALL =bot_move_failed=> + * => WAIT_AND_TRY_AGAIN + * ask the top FSM to wake us up in a few times + */ +fsm_branch_t +gutter__GO_TO_THE_GUTTER_WALL__bot_move_failed (void) +{ + main_getsamples_wait_cycle = GUTTER_WAIT_BEFORE_TRY_AGAIN; + return gutter_next (GO_TO_THE_GUTTER_WALL, bot_move_failed); +} + /* * GO_TO_THE_GUTTER_WALL =bot_move_succeed=> * => DROP_BALLS @@ -91,6 +121,7 @@ gutter__GO_TO_THE_GUTTER_WALL__bot_move_succeed (void) * DROP_BALLS =wait_finished=> * => IDLE * close the rear panel + * tell the top FSM we have finished */ fsm_branch_t gutter__DROP_BALLS__wait_finished (void) diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 844e0d1d..a24e7e14 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -518,7 +518,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - 1b: the approach angle to face the distributor ; * - 1b: how many and where to put collected samples ; */ - getsamples_start (args[0] << 8, args[1]); + getsamples_start (args[0] << 8, args[1], 0); break; case c ('A', 1): diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index e85a4ef7..c55bfa30 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -157,7 +157,7 @@ move_compute_obstacle_position (asserv_position_t cur, /* Convert the angle */ uint32_t angle = cur.a; /* Change angle when going backward */ - if (asserv_get_moving_direction () == 2) + if (0) angle += 0x8000; angle = angle << 8; DPRINTF ("We are at (%d ; %d ; %x)\n", cur.x, cur.y, cur.a); @@ -236,7 +236,8 @@ move_after_moving_backward (void) fsm_branch_t move__WAIT_FOR_CLEAR_PATH__wait_finished (void) { - if (!sharp_path_obstrued (move_data.cached_moving_direction)) +/// if (!sharp_path_obstrued (move_data.cached_moving_direction)) + if (!sharp_path_obstrued (1)) { /* Try to go to the final position */ if (move_data.backward_movement_allowed) @@ -264,12 +265,12 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void) else { /* Store current moving direction */ - move_data.cached_moving_direction = asserv_get_moving_direction (); + move_data.cached_moving_direction = asserv_get_moving_direction (); /* Stop the bot */ asserv_stop_motor (); /* Post an event for the top FSM to be waked up later */ main_move_wait_cycle = MOVE_WAIT_TIME_FOR_POOLING_SHARP; - return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_intermediate_path_found); + return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_no_intermediate_path_found); } } } @@ -411,6 +412,8 @@ move__MOVING_BACKWARD__bot_move_failed (void) } else { + /* Store current moving direction */ + move_data.cached_moving_direction = 2; /* Stop the bot */ asserv_stop_motor (); /* Post an event for the top FSM to be waked up later */ @@ -438,6 +441,8 @@ move__MOVING_BACKWARD__bot_move_succeed (void) } else { + /* Store current moving direction */ + move_data.cached_moving_direction = asserv_get_moving_direction (); /* Stop the bot */ asserv_stop_motor (); /* Post an event for the top FSM to be waked up later */ @@ -459,7 +464,8 @@ move__MOVING_TO_FINAL_POSITION__bot_move_failed (void) /* Compute obstacle position */ move_obstacle_here (); /* Store current moving direction */ - move_data.cached_moving_direction = asserv_get_moving_direction (); + move_data.cached_moving_direction = move_data.backward_movement_allowed ? + 2 : 1; /* Move backward to turn freely */ asserv_move_linearly (-PG_MOVE_DISTANCE); return move_next (MOVING_TO_FINAL_POSITION, bot_move_failed); diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 2710543d..9383d887 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -145,7 +145,7 @@ top__GO_TO_ADVERSE_ICE_DISTRIBUTOR__move_fsm_finished (void) * samples. The problem is this should depend on the time we have until * the end of match */ uint8_t bitfield = 0x3E; - getsamples_start (PG_DISTRIBUTOR_ICE_ADVERSE_A, bitfield); + getsamples_start (PG_DISTRIBUTOR_ICE_ADVERSE_A, bitfield, 0); return top_next (GO_TO_ADVERSE_ICE_DISTRIBUTOR, move_fsm_finished); } @@ -170,7 +170,7 @@ top__GO_TO_OUR_ICE_DISTRIBUTOR__move_fsm_finished (void) /* Second time we try to get our ice, let's took only two */ bitfield = _BV (middle_left_box) | _BV (middle_right_box); } - getsamples_start (PG_DISTRIBUTOR_ICE_OUR_A, bitfield); + getsamples_start (PG_DISTRIBUTOR_ICE_OUR_A, bitfield, 0); return top_next (GO_TO_OUR_ICE_DISTRIBUTOR, move_fsm_finished); } @@ -195,7 +195,7 @@ top__GO_TO_SAMPLE_DISTRIBUTOR__move_fsm_finished (void) bitfield = _BV (out_left_box) | _BV (middle_box) | _BV (out_right_box); } - getsamples_start (PG_DISTRIBUTOR_SAMPLE_OUR_A, bitfield); + getsamples_start (PG_DISTRIBUTOR_SAMPLE_OUR_A, bitfield, 1); return top_next (GO_TO_SAMPLE_DISTRIBUTOR, move_fsm_finished); } -- cgit v1.2.3