From 1bcff7dbcbb284f5564ac708292b23fd447395a9 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Sat, 3 May 2008 11:15:31 +0200 Subject: * digital/io/src * top FSM - change the position where to drop balls at the gutter (to prevent the order of dropped balls to change) ; - change the order of balls taken at the distributor: first two samples with three ice, then three samples with two ice. * get samples FSM - manage the case where the arm passed the noted position in the state where it should not (because the arm was blocked) ; - add some debug messages. * gutter FSM - reduce the time spent to drop the balls at the gutter. * move FSM - update the algorithm to avoid obstacle: when a obstacle is seen, first way, then if it is not present anymore continue, otherwise, try to find an alternative path; if none found, wait again. * chrono - near end of the match time increased to 83s. --- digital/io/src/chrono.c | 2 +- digital/io/src/getsamples.fsm | 4 ++++ digital/io/src/getsamples_cb.c | 39 ++++++++++++++++++++++++++++++++++++++ digital/io/src/gutter_cb.c | 2 +- digital/io/src/main.c | 2 ++ digital/io/src/move.c | 2 ++ digital/io/src/move.fsm | 14 +++++++++----- digital/io/src/move_cb.c | 43 +++++++++++++++++++++++++++++++++--------- digital/io/src/playground.h | 2 +- digital/io/src/top_cb.c | 23 +++++++++++----------- 10 files changed, 104 insertions(+), 29 deletions(-) diff --git a/digital/io/src/chrono.c b/digital/io/src/chrono.c index 051f8670..552232a3 100644 --- a/digital/io/src/chrono.c +++ b/digital/io/src/chrono.c @@ -60,7 +60,7 @@ * CHRONO_OVERFLOW_MAX * seconds / 90 */ #define CHRONO_OVERFLOW_COUNT_NEAR_END_MATCH \ - ((uint8_t) (CHRONO_OVERFLOW_MAX * 65 / 90)) + ((uint8_t) (CHRONO_OVERFLOW_MAX * 83 / 90)) /** * Match is finished. diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm index 0079d458..175a1824 100644 --- a/digital/io/src/getsamples.fsm +++ b/digital/io/src/getsamples.fsm @@ -78,6 +78,8 @@ MOVE_AWAY_FROM_DISTRIBUTOR: bot_move_succeed -> CLOSE_INPUT_HOLE close input hole setup a close timeout + arm_pass_noted_position -> . + prepare the classification of the taken sample CLOSE_INPUT_HOLE: arm_move_succeed -> IDLE @@ -85,3 +87,5 @@ CLOSE_INPUT_HOLE: wait_finished -> IDLE timed out, give up tell the top FSM we have finished + arm_pass_noted_position -> . + prepare the classification of the taken sample diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c index 1ba50ac1..af7b6ced 100644 --- a/digital/io/src/getsamples_cb.c +++ b/digital/io/src/getsamples_cb.c @@ -35,6 +35,8 @@ #include "main.h" /* main_post_event_for_top_fsm */ #include "chrono.h" +#include "modules/proto/proto.h" + #include "io.h" /** @@ -92,6 +94,7 @@ getsamples_count_samples (void) count++; return count; } + /* * MOVE_BACKWARD_FROM_DISTRIBUTOR =bot_move_succeed=> * => TAKE_SAMPLES @@ -109,6 +112,7 @@ getsamples__MOVE_BACKWARD_FROM_DISTRIBUTOR__bot_move_succeed (void) BOT_ARM_SPEED); /* Post an event for the top FSM to be waked up later */ main_getsamples_wait_cycle = GET_SAMPLES_ARM_TIMEOUT; + proto_send1b ('T', 0); return getsamples_next (MOVE_BACKWARD_FROM_DISTRIBUTOR, bot_move_succeed); } @@ -122,6 +126,7 @@ getsamples__FACE_DISTRIBUTOR__bot_move_succeed (void) { /* Move the arm to open the input hole to be able to take some samples */ asserv_move_arm (-BOT_ARM_MIN_TO_OPEN, BOT_ARM_SPEED); + proto_send0 ('O'); return getsamples_next (FACE_DISTRIBUTOR, bot_move_succeed); } @@ -135,6 +140,7 @@ getsamples__OPEN_INPUT_HOLE__arm_move_succeed (void) { /* Approach the distributor */ asserv_go_to_distributor (); + proto_send0 ('A'); return getsamples_next (OPEN_INPUT_HOLE, arm_move_succeed); } @@ -149,9 +155,23 @@ getsamples__CLOSE_INPUT_HOLE__wait_finished (void) { /* Give up, tell the top FSM we have finished */ main_post_event_for_top_fsm = TOP_EVENT_get_samples_fsm_finished; + proto_send0 ('I'); return getsamples_next (CLOSE_INPUT_HOLE, wait_finished); } +/* + * CLOSE_INPUT_HOLE =arm_pass_noted_position=> + * => CLOSE_INPUT_HOLE + * prepare the classification of the taken sample + */ +fsm_branch_t +getsamples__CLOSE_INPUT_HOLE__arm_pass_noted_position (void) +{ + /* Prepare the classification of the taken sample */ + getsamples_configure_classifier (); + return getsamples_next (CLOSE_INPUT_HOLE, arm_pass_noted_position); +} + /* * CLOSE_INPUT_HOLE =arm_move_succeed=> * => IDLE @@ -176,6 +196,7 @@ getsamples__TAKE_SAMPLES__wait_finished (void) { /* Go backward */ asserv_move_linearly (-PG_DISTANCE_DISTRIBUTOR); + proto_send1b ('M', 1); return getsamples_next (TAKE_SAMPLES, wait_finished); } @@ -202,12 +223,14 @@ getsamples__TAKE_SAMPLES__arm_pass_noted_position (void) /* Post an event for the top FSM to be waked up later */ main_getsamples_wait_cycle = GET_SAMPLES_ARM_TIMEOUT; /* Continue to take sample */ + proto_send1b ('T', 1); return getsamples_next_branch (TAKE_SAMPLES, arm_pass_noted_position, more); } else { /* Go backward */ asserv_move_linearly (-PG_DISTANCE_DISTRIBUTOR); + proto_send1b ('M', 0); return getsamples_next_branch (TAKE_SAMPLES, arm_pass_noted_position, no_more); } } @@ -222,9 +245,23 @@ getsamples__IDLE__start (void) { /* Face the distributor */ asserv_goto_angle (getsamples_data_.approach_angle); + proto_send0 ('F'); return getsamples_next (IDLE, start); } +/* + * 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); +} + /* * MOVE_AWAY_FROM_DISTRIBUTOR =bot_move_succeed=> * => CLOSE_INPUT_HOLE @@ -238,6 +275,7 @@ getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void) asserv_move_arm (BOT_ARM_MIN_TO_OPEN + BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED); /* Post an event for the top FSM to be waked up later */ main_getsamples_wait_cycle = GET_SAMPLES_ARM_TIMEOUT; + proto_send0 ('C'); return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, bot_move_succeed); } @@ -251,5 +289,6 @@ getsamples__APPROACH_DISTRIBUTOR__bot_move_succeed (void) { /* Move a little bit backward from the distributor */ asserv_move_linearly (-GET_SAMPLES_MOVE_BACKWARD_DISTANCE); + proto_send0 ('m'); return getsamples_next (APPROACH_DISTRIBUTOR, bot_move_succeed); } diff --git a/digital/io/src/gutter_cb.c b/digital/io/src/gutter_cb.c index 5abf7bcb..8d11a63a 100644 --- a/digital/io/src/gutter_cb.c +++ b/digital/io/src/gutter_cb.c @@ -40,7 +40,7 @@ uint16_t gutter_wait_cycle_; * Count of cycles to wait before we estimate all the balls have been dropped * into the gutter. A cycle normally last 4.4ms. */ -#define GUTTER_WAIT_FOR_BALLS_TO_DROP 1126 +#define GUTTER_WAIT_FOR_BALLS_TO_DROP 300 /* * ROTATE_REAR_SIDE_TO_GUTTER =bot_move_succeed=> diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 3c045368..844e0d1d 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -78,6 +78,7 @@ uint16_t main_sharp_ignore_event; /** * Flag for homologation, to disable the path finding and always stop in front * of an obstacle and wait. + * Do not touch anymore ! XXX FIXME TODO */ uint8_t main_always_stop_for_obstacle = 1; @@ -623,6 +624,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Omo-logo-ation flag, to prevent avoiding obstacle and stop * instead. * - 1b: state of the flag (0 to disable, 1 to enable). + * Do not touch anymore ! XXX FIXME TODO */ main_always_stop_for_obstacle = args[0]; } diff --git a/digital/io/src/move.c b/digital/io/src/move.c index 005a8fbc..c30e5ace 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -48,6 +48,8 @@ move_start (uint16_t position_x, uint16_t position_y, uint8_t move_data.final.x = position_x; move_data.final.y = position_y; move_data.backward_movement_allowed = backward_movement_allowed; + /* XXX */ + main_always_stop_for_obstacle = 1; /* Reset move FSM flags */ main_sharp_ignore_event = 0; main_move_wait_cycle = 0; diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index 317f25d8..c08d0454 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -91,9 +91,13 @@ MOVING_TO_INTERMEDIATE_POSITION: move backward to turn freely WAIT_FOR_CLEAR_PATH: - wait_finished: obstacle -> . - check for obstacle using stored moving direction - post an event for the top FSM to be waked up later wait_finished: no_obstacle -> MOVING_TO_FINAL_POSITION - check for obstacle using stored moving direction - try to go the final position + ask the asserv to go to the final position + wait_finished: obstacle_and_intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION + compute the obstacle position + get next intermediate position from path module + go to next intermediate position + wait_finished: obstacle_and_no_intermediate_path_found -> . + compute the obstacle position + get next intermediate position from path module failed + post an event for the top FSM to be waked up later diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index 0de7ea41..e85a4ef7 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -68,7 +68,7 @@ /** * The generic validity time (in term of number of cyles). */ -#define MOVE_OBSTACLE_VALIDITY (4 * 225) +#define MOVE_OBSTACLE_VALIDITY (6 * 225) /** * Cycles count to ignore sharp event in the main loop. @@ -79,7 +79,7 @@ * Number of cycles to wait before trying to read the sharps values again when * we are stopped. */ -#define MOVE_WAIT_TIME_FOR_POOLING_SHARP (50) +#define MOVE_WAIT_TIME_FOR_POOLING_SHARP (MOVE_MAIN_IGNORE_SHARP_EVENT) /** * A detection offset for the sharps. @@ -223,11 +223,15 @@ move_after_moving_backward (void) /* * WAIT_FOR_CLEAR_PATH =wait_finished=> * no_obstacle => MOVING_TO_FINAL_POSITION - * check for obstacle using stored moving direction - * try to go the final position - * obstacle => WAIT_FOR_CLEAR_PATH - * check for obstacle using stored moving direction + * ask the asserv to go to the final position + * obstacle_and_no_intermediate_path_found => WAIT_FOR_CLEAR_PATH + * compute the obstacle position + * get next intermediate position from path module failed * post an event for the top FSM to be waked up later + * obstacle_and_intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION + * compute the obstacle position + * get next intermediate position from path module + * go to next intermediate position */ fsm_branch_t move__WAIT_FOR_CLEAR_PATH__wait_finished (void) @@ -243,9 +247,30 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void) } else { - /* 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); + /* Enable path finding */ + main_always_stop_for_obstacle = 0; + /* Get next position */ + if (path_get_next (&move_data.intermediate.x, &move_data.intermediate.y)) + { + /* Ignore sharps */ + main_sharp_ignore_event = MOVE_MAIN_IGNORE_SHARP_EVENT; + /* Go to the next intermediate position */ + if (move_data.backward_movement_allowed) + asserv_goto_back (move_data.intermediate.x, move_data.intermediate.y); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y); + return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_intermediate_path_found); + } + 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 */ + 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); + } } } diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h index ae97784e..6659d32b 100644 --- a/digital/io/src/playground.h +++ b/digital/io/src/playground.h @@ -110,7 +110,7 @@ /** * The position of the gutter. */ -#define PG_GUTTER_X (PG_X_VALUE_COMPUTING (2250)) +#define PG_GUTTER_X (PG_X_VALUE_COMPUTING (2600)) #define PG_GUTTER_Y (300) #define PG_GUTTER_A (90 * BOT_ANGLE_DEGREE) diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 0cccdf38..2710543d 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -144,7 +144,7 @@ top__GO_TO_ADVERSE_ICE_DISTRIBUTOR__move_fsm_finished (void) /* Start the get samples FSM with the correct angle to get only two * samples. The problem is this should depend on the time we have until * the end of match */ - uint8_t bitfield = _BV (out_right_box) | _BV (middle_right_box); + uint8_t bitfield = 0x3E; getsamples_start (PG_DISTRIBUTOR_ICE_ADVERSE_A, bitfield); return top_next (GO_TO_ADVERSE_ICE_DISTRIBUTOR, move_fsm_finished); } @@ -161,14 +161,14 @@ top__GO_TO_OUR_ICE_DISTRIBUTOR__move_fsm_finished (void) uint8_t bitfield = 0; if (top_fsm_loop_count_ == 0) { - /* First time we try to get our ice, let's took two */ - bitfield = _BV (middle_left_box) | _BV (middle_right_box); + /* First time we try to get our ice, let's took three */ + bitfield = _BV (out_left_box) | _BV (middle_box) | _BV + (out_right_box); } else { - /* Second time we try to get our ice, let's took only three */ - bitfield = _BV (out_left_box) | _BV (middle_box) | _BV - (out_right_box); + /* 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); return top_next (GO_TO_OUR_ICE_DISTRIBUTOR, move_fsm_finished); @@ -186,15 +186,14 @@ top__GO_TO_SAMPLE_DISTRIBUTOR__move_fsm_finished (void) uint8_t bitfield = 0; if (top_fsm_loop_count_ == 0) { - /* First time we try to get our samples, let's took three of them */ - bitfield = _BV (out_left_box) | _BV (middle_box) | _BV - (out_right_box); + /* First time we try to get our samples, let's took only two of them */ + bitfield = _BV (middle_left_box) | _BV (middle_right_box); } else { - /* Second time we try to get our samples, let's took only two of them - */ - bitfield = _BV (middle_left_box) | _BV (middle_right_box); + /* Second time we try to get our samples, let's took three of them */ + bitfield = _BV (out_left_box) | _BV (middle_box) | _BV + (out_right_box); } getsamples_start (PG_DISTRIBUTOR_SAMPLE_OUR_A, bitfield); return top_next (GO_TO_SAMPLE_DISTRIBUTOR, move_fsm_finished); -- cgit v1.2.3