From aee65133f5500aa3659297f156786122668ffa66 Mon Sep 17 00:00:00 2001 From: NĂ©lio Laranjeiro Date: Fri, 18 Apr 2008 00:56:07 +0200 Subject: Modified gutter FSM to go to the Wall after putting the robot back to the gutter. --- digital/io/src/gutter_cb.c | 37 ++++++++++++++++++++++++++++++++----- 1 file changed, 32 insertions(+), 5 deletions(-) (limited to 'digital/io/src/gutter_cb.c') 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 @@ -56,6 +70,19 @@ gutter__CLOSE_COLLECTOR__collector_closed (void) return gutter_next (CLOSE_COLLECTOR, collector_closed); } +/* + * 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 -- cgit v1.2.3