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/getsamples_cb.c | 39 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) (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 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); } -- cgit v1.2.3