summaryrefslogtreecommitdiff
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
parent1dd17a56e081e19dd1f8de9ea2035a6f4d83d1db (diff)
Update the fsms.
-rw-r--r--digital/io/src/Makefile2
-rw-r--r--digital/io/src/getsamples.c3
-rw-r--r--digital/io/src/getsamples.fsm12
-rw-r--r--digital/io/src/getsamples.h4
-rw-r--r--digital/io/src/getsamples_cb.c28
-rw-r--r--digital/io/src/gutter.c35
-rw-r--r--digital/io/src/gutter.h32
-rw-r--r--digital/io/src/gutter_cb.c5
-rw-r--r--digital/io/src/top.c2
-rw-r--r--digital/io/src/top.fsm39
-rw-r--r--digital/io/src/top.h12
-rw-r--r--digital/io/src/top_cb.c172
12 files changed, 217 insertions, 129 deletions
diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile
index 1898742a..a251442f 100644
--- a/digital/io/src/Makefile
+++ b/digital/io/src/Makefile
@@ -4,7 +4,7 @@ io_SOURCES = main.c asserv.c servo.avr.c eeprom.avr.c trap.c sharp.c \
simu.host.c \
fsm.c \
getsamples.c getsamples_fsm.c getsamples_cb.c \
- gutter_fsm.c gutter_cb.c \
+ gutter_fsm.c gutter_cb.c gutter.c \
move.c move_fsm.c move_cb.c \
top.c top_fsm.c top_cb.c
MODULES = proto uart twi utils adc
diff --git a/digital/io/src/getsamples.c b/digital/io/src/getsamples.c
index b80881eb..96644a59 100644
--- a/digital/io/src/getsamples.c
+++ b/digital/io/src/getsamples.c
@@ -32,12 +32,13 @@ struct getsamples_data_t getsamples_data;
/** Start a getsamples FSM. */
void
getsamples_start (uint32_t distributor_x, uint32_t distributor_y,
- uint8_t samples)
+ uint8_t samples, uint8_t event_to_post)
{
/* Set parameters. */
getsamples_data.distributor_x = distributor_x;
getsamples_data.distributor_y = distributor_y;
getsamples_data.samples = samples;
+ getsamples_data.event = event_to_post;
/* Start FSM. */
fsm_init (&getsamples_fsm);
fsm_handle_event (&getsamples_fsm, GETSAMPLES_EVENT_ok);
diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm
index 3f7266d0..daf1b738 100644
--- a/digital/io/src/getsamples.fsm
+++ b/digital/io/src/getsamples.fsm
@@ -7,16 +7,15 @@ States:
PREPARE_ARM
FORWARD_CONTROL
TAKE_SAMPLES
- BACKWARD
END
Events:
ok
- position_reached
position_failed
arm_moved
sample_took
classifier_ready
+ position_reached
START:
ok -> GO_TO_POSITION
@@ -25,8 +24,6 @@ START:
GO_TO_POSITION:
position_reached -> PREPARE_ARM
Go to the position desired, it is very near the position of the distributor in case it is a ice distributor or sample distributor.
- position_failed -> GO_TO_POSITION
- Go to another point before trying to go to this one again.
PREPARE_ARM:
arm_moved -> FORWARD_CONTROL
@@ -39,9 +36,6 @@ FORWARD_CONTROL:
TAKE_SAMPLES:
sample_took: more -> .
Continue to take samples and classify the next sample.
- sample_took: no_more -> BACKWARD
- If the quantity of samples are tooked, then go backeward and conitnue classifying the samples.
+ sample_took: no_more -> END
+ If the quantity of samples are taken, then go backward and continue classifying the samples.
-BACKWARD:
- position_reached -> END
- Ending this state machine.
diff --git a/digital/io/src/getsamples.h b/digital/io/src/getsamples.h
index 883441d5..6bfb3d63 100644
--- a/digital/io/src/getsamples.h
+++ b/digital/io/src/getsamples.h
@@ -34,6 +34,8 @@ struct getsamples_data_t
uint32_t distributor_y;
/* Samples to take. */
uint8_t samples;
+ /* event of the main fsm to post. */
+ uint8_t event;
};
/** getsamples global. */
@@ -42,7 +44,7 @@ extern struct getsamples_data_t getsamples_data;
/** Start a getsamples FSM. */
void
getsamples_start (uint32_t distributor_x, uint32_t distributor_y,
- uint8_t samples);
+ uint8_t samples, uint8_t event_to_post);
/** Configure the classifier using the bit fields in the getsamples_data
* structure.
diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c
index 68268795..3c15fc42 100644
--- a/digital/io/src/getsamples_cb.c
+++ b/digital/io/src/getsamples_cb.c
@@ -75,7 +75,7 @@ getsamples__START__ok (void)
/*
* TAKE_SAMPLES =sample_took=>
- * no_more => BACKWARD
+ * no_more => END
* If the quantity of samples are taken, then go backward and continue classifying the samples.
* more => TAKE_SAMPLES
* Continue to take samples and classify the next sample.
@@ -96,35 +96,11 @@ getsamples__TAKE_SAMPLES__sample_took (void)
asserv_goto (getsamples_data.distributor_x - 200,
getsamples_data.distributor_y - 200);
+ fsm_handle_event (&top_fsm, getsamples_data.event);
return getsamples_next_branch (TAKE_SAMPLES, sample_took, no_more);
}
}
-/*
- * BACKWARD =position_reached=>
- * => END
- * Ending this state machine.
- */
-fsm_branch_t
-getsamples__BACKWARD__position_reached (void)
-{
- asserv_move_arm (5000, 100);
-
- fsm_handle_event (&top_fsm, TOP_EVENT_samples_took);
- return getsamples_next (BACKWARD, position_reached);
-}
-
-/*
- * GO_TO_POSITION =position_failed=>
- * => GO_TO_POSITION
- * Go to another point before trying to go to this one again.
- */
-fsm_branch_t
-getsamples__GO_TO_POSITION__position_failed (void)
-{
- // TODO In this case i don't know what to do.
- return getsamples_next (GO_TO_POSITION, position_failed);
-}
/*
* GO_TO_POSITION =position_reached=>
diff --git a/digital/io/src/gutter.c b/digital/io/src/gutter.c
new file mode 100644
index 00000000..0337b974
--- /dev/null
+++ b/digital/io/src/gutter.c
@@ -0,0 +1,35 @@
+/* gutter.c */
+/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+ *
+ * Copyright (C) 2008 Nélio Laranjeiro
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+#include "common.h"
+#include "gutter.h"
+#include "fsm.h"
+
+void
+gutter_start (void)
+{
+ /* Start the FSM. */
+ fsm_init (&gutter_fsm);
+ fsm_handle_event (&gutter_fsm, GUTTER_EVENT_ok);
+}
diff --git a/digital/io/src/gutter.h b/digital/io/src/gutter.h
new file mode 100644
index 00000000..32d0b226
--- /dev/null
+++ b/digital/io/src/gutter.h
@@ -0,0 +1,32 @@
+#ifndef gutter_h
+#define gutter_h
+/* gutter.h */
+/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+ *
+ * Copyright (C) 2008 Nélio Laranjeiro
+ *
+ * APBTeam:
+ * Web: http://apbteam.org/
+ * Email: team AT apbteam DOT org
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ *
+ * }}} */
+
+/* gutter start */
+void
+gutter_start (void);
+
+#endif /* gutter_h */
diff --git a/digital/io/src/gutter_cb.c b/digital/io/src/gutter_cb.c
index 336ae404..f2c01539 100644
--- a/digital/io/src/gutter_cb.c
+++ b/digital/io/src/gutter_cb.c
@@ -24,10 +24,13 @@
* }}} */
#include "common.h"
#include "fsm.h"
+#include "gutter.h"
#include "gutter_cb.h"
#include "asserv.h"
#include "trap.h"
#include "modules/utils/utils.h"
+#include "top.h"
+#include "topfsm_fsm.h"
/*
* START =ok=>
@@ -51,6 +54,8 @@ gutter__CLOSE_COLLECTOR__collector_closed (void)
{
//Close the collector.
trap_close_rear_panel();
+ // Post an event to the top fsm machine
+ fsm_handle_event (&top_fsm, TOPFSM_EVENT_samples_deposed);
return gutter_next (CLOSE_COLLECTOR, collector_closed);
}
diff --git a/digital/io/src/top.c b/digital/io/src/top.c
index f358445d..c685af19 100644
--- a/digital/io/src/top.c
+++ b/digital/io/src/top.c
@@ -35,6 +35,8 @@ top_start (uint8_t color_team)
{
/* Set parameters. */
top_data.sequence = 0x15;
+ top_data.boxes_used = 0x0;
+ top_data.sequence_to_do = 0x15;
/* Get the team color.
* !! I supose that the 0 value is 0 and red value is 1 */
top_data.team_color = color_team;
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
index 1da78745..21a6923f 100644
--- a/digital/io/src/top.fsm
+++ b/digital/io/src/top.fsm
@@ -4,9 +4,10 @@ top
States:
START
GET_SAMPLES
- GET_ICE
GO_TO_GOAL
+ GET_ICE
GET_ADV_ICE
+ BACKWARD
Events:
ok
@@ -14,39 +15,33 @@ Events:
collector_full
ice_took
samples_deposed
- samples_took_ice_distributor_empty_not_enough_time
- samples_took_ice_distributor_empty_enough_time
- samples_deposed_ice_distributor_empty
- ice_took_collector_full
- not_enough_time
- ice_took_collector_not_full
+ ice_dist_empty
+ ended
START:
ok -> GET_SAMPLES
Go to take some samples. The sequence shall be adapt to take the correct number of samples.
GET_SAMPLES:
- samples_took -> GET_ICE
+ samples_took -> BACKWARD
The samples had been took and now the ice is missing.
- samples_took_ice_distributor_empty_not_enough_time -> GO_TO_GOAL
- Some samples had been took and there is not enough time to get some ice.
- 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.
- collector_full -> GO_TO_GOAL
- All the room in the robot are full.
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.
+ ice_took -> BACKWARD
+ Go backward to end the take balls.
GO_TO_GOAL:
samples_deposed -> GET_SAMPLES
The samples had been deposed, it shall try to get more samples. This state will call the getsamples FSM and the moveFSM.
- samples_deposed_ice_distributor_empty -> GET_ADV_ICE
- Go to get some adversary ice because our distributor is empty.
GET_ADV_ICE:
- ice_took -> GO_TO_GOAL
- The ice has been taken, now the robot shall depose it.
- ice_took_collector_not_full -> GET_SAMPLES
- There is enough time to get some samples and go to the goal.
+ ice_took -> BACKWARD
+ The ice has been taken.
+
+BACKWARD:
+ ended -> GET_ICE
+ Get ICE to end the sequence.
+ collector_full -> GO_TO_GOAL
+ Go the goal to depose the samples.
+ ice_dist_empty -> GET_ADV_ICE
+ Go to take the ice in the adversary distributor because our is empty.
diff --git a/digital/io/src/top.h b/digital/io/src/top.h
index 169cf278..aa05c07b 100644
--- a/digital/io/src/top.h
+++ b/digital/io/src/top.h
@@ -41,6 +41,14 @@ enum team_color_e
RED_TEAM
};
+enum sequence_e
+{
+ /* 3 color balls, 2 ice */
+ SEQUCENCE_ONE = 0x15,
+ /* 2 color balls, 3 ice */
+ SEQUENCE_TWO = 0xA
+};
+
struct top_data_t
{
/** The sequence to get.
@@ -50,6 +58,10 @@ struct top_data_t
* bit 1 = slot 1 and so on.
*/
uint8_t sequence;
+ /** The boxes already in use. */
+ uint8_t boxes_used;
+ /** sequence to realize. */
+ uint8_t sequence_to_do;
/** The color of the balls the robot shall take.
* RED_TEAM or BLUE_TEAM. */
uint8_t team_color;
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);
}