summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src/gutter_cb.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src/gutter_cb.c')
-rw-r--r--digital/io/src/gutter_cb.c37
1 files changed, 32 insertions, 5 deletions
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.