summaryrefslogtreecommitdiff
path: root/digital/io/src/getsamples_cb.c
diff options
context:
space:
mode:
authorJérémy Dufour2008-05-03 11:15:31 +0200
committerJérémy Dufour2008-05-03 11:15:31 +0200
commit1bcff7dbcbb284f5564ac708292b23fd447395a9 (patch)
tree2cc5f47226bd7155f9ed575d1a76593b6f14a802 /digital/io/src/getsamples_cb.c
parentd803fd8ba61dba29c904ee1fccfab2b7177834d4 (diff)
* 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.
Diffstat (limited to 'digital/io/src/getsamples_cb.c')
-rw-r--r--digital/io/src/getsamples_cb.c39
1 files changed, 39 insertions, 0 deletions
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,10 +155,24 @@ 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
* tell the top FSM we have finished
@@ -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,10 +245,24 @@ 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
* 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);
}