summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/top_cb.c
diff options
context:
space:
mode:
authorNélio Laranjeiro2008-04-12 02:03:38 +0200
committerNélio Laranjeiro2008-04-12 02:03:38 +0200
commit3c7512d1619b2e73424790f157aec3926e39e838 (patch)
treece237207867f09cc64e262db198a7d307358bd7e /digital/io/src/top_cb.c
parent1dd17a56e081e19dd1f8de9ea2035a6f4d83d1db (diff)
Update the fsms.
Diffstat (limited to 'digital/io/src/top_cb.c')
-rw-r--r--digital/io/src/top_cb.c172
1 files changed, 103 insertions, 69 deletions
diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c
index bf92b204..bb2f6c1d 100644
--- a/digital/io/src/top_cb.c
+++ b/digital/io/src/top_cb.c
@@ -7,88 +7,104 @@
*/
#include "common.h"
#include "fsm.h"
-#include "top.h"
#include "top_cb.h"
+#include "top.h"
#include "getsamples.h"
+#include "asserv.h"
+#include "gutter.h"
/*
- * START =ok=>
- * => GET_SAMPLES
- * Go to take some samples. The sequence shall be adapt to take the correct number of samples.
- */
-fsm_branch_t
-top__START__ok (void)
-{
- // Call the get samples state machine.
-
- // Blue color.
- if (top_data.team_color == BLUE_TEAM)
- {
- getsamples_start (BLUE_DISTRIBUTOR_X, BLUE_DISTRIBUTOR_Y,
- top_data.sequence);
- }
- else
- {
- getsamples_start (RED_DISTRIBUTOR_X, RED_DISTRIBUTOR_Y,
- top_data.sequence);
- }
-
- return top_next (START, ok);
-}
-
-/*
- * GET_SAMPLES =samples_took_ice_distributor_empty_not_enough_time=>
+ * BACKWARD =collector_full=>
* => GO_TO_GOAL
- * Some samples had been took and there is not enough time to get some ice.
+ * Go the goal to depose the samples.
*/
fsm_branch_t
-top__GET_SAMPLES__samples_took_ice_distributor_empty_not_enough_time (void)
+top__BACKWARD__collector_full (void)
{
- return top_next (GET_SAMPLES, samples_took_ice_distributor_empty_not_enough_time);
+ gutter_start ();
+ top_data.sequence_to_do = (~top_data.sequence_to_do) & 0x1f;
+ return top_next (BACKWARD, collector_full);
}
/*
- * GET_SAMPLES =samples_took=>
+ * BACKWARD =ended=>
* => GET_ICE
- * The samples had been took and now the ice is missing.
+ * Get ICE to end the sequence.
*/
fsm_branch_t
-top__GET_SAMPLES__samples_took (void)
+top__BACKWARD__ended (void)
{
- return top_next (GET_SAMPLES, samples_took);
+ if (top_data.team_color)
+ {
+ getsamples_start ( ICE_DISTRIBUTOR_LEFT, ICE_DISTRIBUTOR_Y,
+ ~top_data.boxes_used, TOP_EVENT_ice_took);
+ }
+ else
+ {
+ getsamples_start ( ICE_DISTRIBUTOR_RIGHT, ICE_DISTRIBUTOR_Y,
+ ~top_data.boxes_used, TOP_EVENT_ice_took);
+ }
+ return top_next (BACKWARD, ended);
}
/*
- * GET_SAMPLES =collector_full=>
- * => GO_TO_GOAL
- * All the room in the robot are full.
+ * BACKWARD =ice_dist_empty=>
+ * => GET_ADV_ICE
+ * Go to take the ice in the adversary distributor because our is empty.
*/
fsm_branch_t
-top__GET_SAMPLES__collector_full (void)
+top__BACKWARD__ice_dist_empty (void)
{
- return top_next (GET_SAMPLES, collector_full);
+ if (top_data.team_color)
+ {
+ getsamples_start ( ICE_DISTRIBUTOR_RIGHT, ICE_DISTRIBUTOR_Y,
+ ~top_data.boxes_used, TOP_EVENT_ice_took);
+ }
+ else
+ {
+ getsamples_start ( ICE_DISTRIBUTOR_LEFT, ICE_DISTRIBUTOR_Y,
+ ~top_data.boxes_used, TOP_EVENT_ice_took);
+ }
+
+ return top_next (BACKWARD, ice_dist_empty);
}
/*
- * GET_SAMPLES =samples_took_ice_distributor_empty_enough_time=>
- * => GET_ADV_ICE
- * Go to take ice in the adversary distributor because the one in our part of the table is empty.
+ * GET_ICE =ice_took=>
+ * => BACKWARD
+ * Go backward to end the take balls.
*/
fsm_branch_t
-top__GET_SAMPLES__samples_took_ice_distributor_empty_enough_time (void)
+top__GET_ICE__ice_took (void)
{
- return top_next (GET_SAMPLES, samples_took_ice_distributor_empty_enough_time);
+ asserv_position_t pos;
+
+ asserv_get_position (&pos);
+
+ asserv_goto (pos.x - 200, pos.y - 200);
+ return top_next (GET_ICE, ice_took);
}
/*
- * GO_TO_GOAL =samples_deposed_ice_distributor_empty=>
- * => GET_ADV_ICE
- * Go to get some adversary ice because our distributor is empty.
+ * START =ok=>
+ * => GET_SAMPLES
+ * Go to take some samples. The sequence shall be adapt to take the correct number of samples.
*/
fsm_branch_t
-top__GO_TO_GOAL__samples_deposed_ice_distributor_empty (void)
+top__START__ok (void)
{
- return top_next (GO_TO_GOAL, samples_deposed_ice_distributor_empty);
+ if (top_data.team_color == BLUE_TEAM)
+ {
+ getsamples_start ( BLUE_DISTRIBUTOR_X, BLUE_DISTRIBUTOR_Y ,
+ ~top_data.boxes_used, TOP_EVENT_samples_took);
+ }
+ else
+ {
+ getsamples_start ( RED_DISTRIBUTOR_X, RED_DISTRIBUTOR_Y,
+ ~top_data.boxes_used, TOP_EVENT_samples_took);
+ }
+
+ return top_next (START, ok);
}
/*
@@ -99,40 +115,58 @@ top__GO_TO_GOAL__samples_deposed_ice_distributor_empty (void)
fsm_branch_t
top__GO_TO_GOAL__samples_deposed (void)
{
+ if (top_data.team_color == BLUE_TEAM)
+ {
+ getsamples_start ( BLUE_DISTRIBUTOR_X, BLUE_DISTRIBUTOR_Y ,
+ ~top_data.boxes_used, TOP_EVENT_samples_took);
+ }
+ else
+ {
+ getsamples_start ( RED_DISTRIBUTOR_X, RED_DISTRIBUTOR_Y,
+ ~top_data.boxes_used, TOP_EVENT_samples_took);
+ }
return top_next (GO_TO_GOAL, samples_deposed);
}
/*
- * GET_ICE =ice_took=>
- * => GO_TO_GOAL
- * The ice had been taken and the collector is full. The robot shall go to depose it into the goal.
+ * GET_ADV_ICE =ice_took=>
+ * => BACKWARD
+ * The ice has been taken.
*/
fsm_branch_t
-top__GET_ICE__ice_took (void)
+top__GET_ADV_ICE__ice_took (void)
{
- return top_next (GET_ICE, ice_took);
-}
+ asserv_position_t pos;
-/*
- * GET_ADV_ICE =ice_took_collector_not_full=>
- * => GET_SAMPLES
- * There is enough time to get some samples and go to the goal.
- */
-fsm_branch_t
-top__GET_ADV_ICE__ice_took_collector_not_full (void)
-{
- return top_next (GET_ADV_ICE, ice_took_collector_not_full);
+ asserv_get_position (&pos);
+
+ asserv_goto (pos.x - 200, pos.y - 200);
+ return top_next (GET_ADV_ICE, ice_took);
}
/*
- * GET_ADV_ICE =ice_took=>
- * => GO_TO_GOAL
- * The ice has been taken, now the robot shall depose it.
+ * GET_SAMPLES =samples_took=>
+ * => BACKWARD
+ * The samples had been took and now the ice is missing.
*/
fsm_branch_t
-top__GET_ADV_ICE__ice_took (void)
+top__GET_SAMPLES__samples_took (void)
{
- return top_next (GET_ADV_ICE, ice_took);
+ // Call the get samples state machine.
+
+ // Blue color.
+ if (top_data.team_color == BLUE_TEAM)
+ {
+ getsamples_start (BLUE_DISTRIBUTOR_X, BLUE_DISTRIBUTOR_Y,
+ top_data.sequence, TOP_EVENT_samples_took);
+ }
+ else
+ {
+ getsamples_start (RED_DISTRIBUTOR_X, RED_DISTRIBUTOR_Y,
+ top_data.sequence, TOP_EVENT_samples_took);
+ }
+
+ return top_next (START, ok);
}