From 3e87ad74d0dabecee698f3acefa6f38b159484c3 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 23 Apr 2008 13:30:41 +0200 Subject: * digital/io/src - rewrite gutter FSM. --- digital/io/src/gutter.fsm | 48 ++++++++++++++++++------------- digital/io/src/gutter_cb.c | 70 ++++++++++++++++++++-------------------------- 2 files changed, 59 insertions(+), 59 deletions(-) diff --git a/digital/io/src/gutter.fsm b/digital/io/src/gutter.fsm index a8b1409a..966ee8f0 100644 --- a/digital/io/src/gutter.fsm +++ b/digital/io/src/gutter.fsm @@ -1,36 +1,46 @@ -#FSM gutter +# Gutter FSM +# Drop balls from the inside of the bot into the gutter to score points. gutter States: IDLE + waiting for the beginning of the gutter FSM ROTATE_REAR_SIDE_TO_GUTTER + stand back to the gutter GO_TO_THE_GUTTER_WALL - OPEN_COLLECTOR - CLOSE_COLLECTOR + make the bot reversing against the gutter + DROP_BALLS + open the collector to let the balls fall into the gutter Events: start - rotation_done - ready - collector_opened - collector_closed + initialize the FSM + bot_move_succeed + the bot has finished to moved successfully + bot_move_failed + the bot movement failed (blocked by something for example) + wait_finished + we have wait the desired time IDLE: start -> ROTATE_REAR_SIDE_TO_GUTTER - Pur the robot back to the gutter to allow it to drop the balls in the gutter. + put the bot back to 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. + bot_move_succeed -> GO_TO_THE_GUTTER_WALL + make the bot reversing against the gutter GO_TO_THE_GUTTER_WALL: - ready -> OPEN_COLLECTOR - Open the collector and wait for a while. + bot_move_succeed -> DROP_BALLS + open the collector to drop the balls + wait for a while +# TODO: handle the case where a ball is between the bot and the border, +# preventing from fucking the wall +# The best way is probably to abort this FSM with an error, tell the upper one +# (top) we need to try at another place (the gutter is quite long). +# bot_move_failed -OPEN_COLLECTOR: - collector_opened -> CLOSE_COLLECTOR - Close the rear panel. - -CLOSE_COLLECTOR: - collector_closed-> IDLE - The samples has been inserted in the gutter. Go to the idle state. +DROP_BALLS: + wait_finished -> IDLE + close the rear panel + tell the top FSM we have finished diff --git a/digital/io/src/gutter_cb.c b/digital/io/src/gutter_cb.c index d69ea833..00a00b55 100644 --- a/digital/io/src/gutter_cb.c +++ b/digital/io/src/gutter_cb.c @@ -24,74 +24,64 @@ * }}} */ #include "common.h" #include "fsm.h" -#include "gutter.h" #include "gutter_cb.h" -#include "trap.h" -#include "modules/utils/utils.h" -#include "asserv.h" + +#include "asserv.h" /* asserv_go_to_the_wall */ +#include "trap.h" /* trap_* */ +#include "playground.h" /* PG_GUTTER_A */ /* - * ROTATE_REAR_SIDE_TO_GUTTER =rotation_done=> + * ROTATE_REAR_SIDE_TO_GUTTER =bot_move_succeed=> * => 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. + * make the bot reversing against the gutter */ fsm_branch_t -gutter__ROTATE_REAR_SIDE_TO_GUTTER__rotation_done (void) +gutter__ROTATE_REAR_SIDE_TO_GUTTER__bot_move_succeed (void) { - // Go to the wall in backward mode. - asserv_go_to_the_wall(); - return gutter_next (ROTATE_REAR_SIDE_TO_GUTTER, rotation_done); + /* Make the bot reversing against the gutter */ + asserv_go_to_the_wall (); + return gutter_next (ROTATE_REAR_SIDE_TO_GUTTER, bot_move_succeed); } /* * IDLE =start=> * => ROTATE_REAR_SIDE_TO_GUTTER - * Pur the robot back to the gutter to allow it to drop the balls in the gutter. + * put the bot back to the gutter */ fsm_branch_t gutter__IDLE__start (void) { - // Request the robot to rotate to ends with the rear panel in front of the - // gutter. - asserv_goto_angle (0x8000); + /* Put the bot back to the gutter */ + asserv_goto_angle (PG_GUTTER_A); return gutter_next (IDLE, start); } /* - * CLOSE_COLLECTOR =collector_closed=> - * => END - * The samples has been inserted in the gutter. - */ -fsm_branch_t -gutter__CLOSE_COLLECTOR__collector_closed (void) -{ - //Close the collector. - trap_close_rear_panel(); - return gutter_next (CLOSE_COLLECTOR, collector_closed); -} - -/* - * GO_TO_THE_GUTTER_WALL =ready=> - * => OPEN_COLLECTOR - * Open the collector and wait for a while. + * GO_TO_THE_GUTTER_WALL =bot_move_succeed=> + * => DROP_BALLS + * open the collector to drop the balls + * wait for a while */ fsm_branch_t -gutter__GO_TO_THE_GUTTER_WALL__ready (void) +gutter__GO_TO_THE_GUTTER_WALL__bot_move_succeed (void) { - // Open the rear panel. + /* Open the rear panel */ trap_open_rear_panel (); - return gutter_next (GO_TO_THE_GUTTER_WALL, ready); + /* Wait for a while XXX TODO */ + return gutter_next (GO_TO_THE_GUTTER_WALL, bot_move_succeed); } /* - * OPEN_COLLECTOR =collector_opened=> - * => CLOSE_COLLECTOR - * Wait some time and clse the door. + * DROP_BALLS =wait_finished=> + * => IDLE + * close the rear panel */ fsm_branch_t -gutter__OPEN_COLLECTOR__collector_opened (void) +gutter__DROP_BALLS__wait_finished (void) { - utils_delay_ms(4.4); - return gutter_next (OPEN_COLLECTOR, collector_opened); + /* Close the rear panel */ + trap_close_rear_panel (); + /* Tell the top FSM we have finished */ + fsm_handle_event (&top_fsm, TOP_EVENT_gutter_fsm_finished); + return gutter_next (DROP_BALLS, wait_finished); } - -- cgit v1.2.3