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.fsm | 14 +++++++++++++- digital/io/src/gutter_cb.c | 37 ++++++++++++++++++++++++++++++++----- digital/io/src/test/gutter/main.c | 29 +++++++++++++++++++++++++++++ 3 files changed, 74 insertions(+), 6 deletions(-) (limited to 'digital/io/src') 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 @@ -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 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"); +} + -- cgit v1.2.3