From 23bce6a75a17b9cd9d3f35a251baa1b0635697a4 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Sat, 3 May 2008 12:43:19 +0200 Subject: * digital/io/src - manage the event where the bot can be blocked (when moving) in the different FSM, by waiting and trying again ; - ensure we update the classifier (with the servos motor) even when the arm move in a state where it should not. --- digital/io/src/gutter_cb.c | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) (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 8d11a63a..96f5c605 100644 --- a/digital/io/src/gutter_cb.c +++ b/digital/io/src/gutter_cb.c @@ -42,6 +42,11 @@ uint16_t gutter_wait_cycle_; */ #define GUTTER_WAIT_FOR_BALLS_TO_DROP 300 +/** + * Time to wait before trying the go to the wall again. + */ +#define GUTTER_WAIT_BEFORE_TRY_AGAIN (225) + /* * ROTATE_REAR_SIDE_TO_GUTTER =bot_move_succeed=> * => GO_TO_THE_GUTTER_WALL @@ -55,6 +60,19 @@ gutter__ROTATE_REAR_SIDE_TO_GUTTER__bot_move_succeed (void) return gutter_next (ROTATE_REAR_SIDE_TO_GUTTER, bot_move_succeed); } +/* + * WAIT_AND_TRY_AGAIN =wait_finished=> + * => GO_TO_THE_GUTTER_WALL + * try the fuck the wall again + */ +fsm_branch_t +gutter__WAIT_AND_TRY_AGAIN__wait_finished (void) +{ + /* Make the bot reversing against the gutter */ + asserv_go_to_the_wall (); + return gutter_next (WAIT_AND_TRY_AGAIN, wait_finished); +} + /* * IDLE =start=> * => ROTATE_REAR_SIDE_TO_GUTTER @@ -70,6 +88,18 @@ gutter__IDLE__start (void) return gutter_next (IDLE, start); } +/* + * GO_TO_THE_GUTTER_WALL =bot_move_failed=> + * => WAIT_AND_TRY_AGAIN + * ask the top FSM to wake us up in a few times + */ +fsm_branch_t +gutter__GO_TO_THE_GUTTER_WALL__bot_move_failed (void) +{ + main_getsamples_wait_cycle = GUTTER_WAIT_BEFORE_TRY_AGAIN; + return gutter_next (GO_TO_THE_GUTTER_WALL, bot_move_failed); +} + /* * GO_TO_THE_GUTTER_WALL =bot_move_succeed=> * => DROP_BALLS @@ -91,6 +121,7 @@ gutter__GO_TO_THE_GUTTER_WALL__bot_move_succeed (void) * DROP_BALLS =wait_finished=> * => IDLE * close the rear panel + * tell the top FSM we have finished */ fsm_branch_t gutter__DROP_BALLS__wait_finished (void) -- cgit v1.2.3