summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src
diff options
context:
space:
mode:
authorNélio Laranjeiro2008-04-18 00:56:07 +0200
committerNélio Laranjeiro2008-04-18 00:56:07 +0200
commitaee65133f5500aa3659297f156786122668ffa66 (patch)
tree651c8a5cb25817e6b3c036403266741d73720d29 /digital/io/src
parentbc77f0483fc4e41fab5ca184a8e2f2664b935d20 (diff)
Modified gutter FSM to go to the Wall after putting the robot back to
the gutter.
Diffstat (limited to 'digital/io/src')
-rw-r--r--digital/io/src/gutter.fsm14
-rw-r--r--digital/io/src/gutter_cb.c37
-rw-r--r--digital/io/src/test/gutter/main.c29
3 files changed, 74 insertions, 6 deletions
diff --git a/digital/io/src/gutter.fsm b/digital/io/src/gutter.fsm
index 906feaf9..a8b1409a 100644
--- a/digital/io/src/gutter.fsm
+++ b/digital/io/src/gutter.fsm
@@ -3,16 +3,28 @@ gutter
States:
IDLE
+ ROTATE_REAR_SIDE_TO_GUTTER
+ GO_TO_THE_GUTTER_WALL
OPEN_COLLECTOR
CLOSE_COLLECTOR
Events:
start
+ rotation_done
+ ready
collector_opened
collector_closed
IDLE:
- start -> OPEN_COLLECTOR
+ start -> ROTATE_REAR_SIDE_TO_GUTTER
+ Pur the robot back to the gutter to allow it to drop the balls in the gutter.
+
+ROTATE_REAR_SIDE_TO_GUTTER:
+ rotation_done -> GO_TO_THE_GUTTER_WALL
+ The rotation is done, the robot can go to the wall in backward mode to drop the balls in the gutter.
+
+GO_TO_THE_GUTTER_WALL:
+ ready -> OPEN_COLLECTOR
Open the collector and wait for a while.
OPEN_COLLECTOR:
diff --git a/digital/io/src/gutter_cb.c b/digital/io/src/gutter_cb.c
index 85b48ef5..d69ea833 100644
--- a/digital/io/src/gutter_cb.c
+++ b/digital/io/src/gutter_cb.c
@@ -28,21 +28,35 @@
#include "gutter_cb.h"
#include "trap.h"
#include "modules/utils/utils.h"
+#include "asserv.h"
+
+/*
+ * ROTATE_REAR_SIDE_TO_GUTTER =rotation_done=>
+ * => GO_TO_THE_GUTTER_WALL
+ * The rotation is done, the robot can go to the wall in backward mode to drop the balls in the gutter.
+ */
+fsm_branch_t
+gutter__ROTATE_REAR_SIDE_TO_GUTTER__rotation_done (void)
+{
+ // Go to the wall in backward mode.
+ asserv_go_to_the_wall();
+ return gutter_next (ROTATE_REAR_SIDE_TO_GUTTER, rotation_done);
+}
/*
* IDLE =start=>
- * => OPEN_COLLECTOR
- * Open the collector and wait for a while.
+ * => ROTATE_REAR_SIDE_TO_GUTTER
+ * Pur the robot back to the gutter to allow it to drop the balls in the gutter.
*/
fsm_branch_t
gutter__IDLE__start (void)
{
- // Open the rear panel.
- trap_open_rear_panel ();
+ // Request the robot to rotate to ends with the rear panel in front of the
+ // gutter.
+ asserv_goto_angle (0x8000);
return gutter_next (IDLE, start);
}
-
/*
* CLOSE_COLLECTOR =collector_closed=>
* => END
@@ -57,6 +71,19 @@ gutter__CLOSE_COLLECTOR__collector_closed (void)
}
/*
+ * GO_TO_THE_GUTTER_WALL =ready=>
+ * => OPEN_COLLECTOR
+ * Open the collector and wait for a while.
+ */
+fsm_branch_t
+gutter__GO_TO_THE_GUTTER_WALL__ready (void)
+{
+ // Open the rear panel.
+ trap_open_rear_panel ();
+ return gutter_next (GO_TO_THE_GUTTER_WALL, ready);
+}
+
+/*
* OPEN_COLLECTOR =collector_opened=>
* => CLOSE_COLLECTOR
* Wait some time and clse the door.
diff --git a/digital/io/src/test/gutter/main.c b/digital/io/src/test/gutter/main.c
index 877545fc..059f5f34 100644
--- a/digital/io/src/test/gutter/main.c
+++ b/digital/io/src/test/gutter/main.c
@@ -37,6 +37,12 @@ gutter_print_test (fsm_t *gutter)
case GUTTER_STATE_IDLE:
printf ("IDLE");
break;
+ case GUTTER_STATE_ROTATE_REAR_SIDE_TO_GUTTER:
+ printf ("ROTATE_REAR_SIDE_TO_GUTTER");
+ break;
+ case GUTTER_STATE_GO_TO_THE_GUTTER_WALL:
+ printf ("GO_TO_THE_GUTTER_WALL");
+ break;
case GUTTER_STATE_CLOSE_COLLECTOR:
printf ("CLOSE COLLECTOR");
break;
@@ -54,7 +60,17 @@ main (void)
{
fsm_init (&gutter_fsm);
+ gutter_print_test (&gutter_fsm);
fsm_handle_event (&gutter_fsm, GUTTER_EVENT_start);
+
+ // Request the main_loop to go to next state because the angle had
+ // been reached.
+ fsm_handle_event (&gutter_fsm, GUTTER_EVENT_rotation_done);
+ gutter_print_test (&gutter_fsm);
+
+ // Request the main_loop to go to next state because the angle had
+ // been reached.
+ fsm_handle_event (&gutter_fsm, GUTTER_EVENT_ready);
gutter_print_test (&gutter_fsm);
fsm_handle_event (&gutter_fsm, GUTTER_EVENT_collector_opened);
@@ -78,3 +94,16 @@ trap_close_rear_panel(void)
printf ("\t Close rear panel\n");
}
+void
+asserv_goto_angle (int16_t angle)
+{
+ printf ("\t Angle requested\n");
+}
+
+/* Go to the wall (moving backward). */
+void
+asserv_go_to_the_wall (void)
+{
+ printf ("\t go to the wall\n");
+}
+