From b37be4dc387ff573c5d6baca14472a56c682c537 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 23 Apr 2008 13:31:14 +0200 Subject: * digital/io/src - integrate gutter FSM into the main loop. --- digital/io/src/gutter.c | 14 ++++++++++++++ digital/io/src/gutter.h | 17 ++++++++++++++++- digital/io/src/gutter_cb.c | 17 ++++++++++++++++- digital/io/src/main.c | 10 ++++++++++ 4 files changed, 56 insertions(+), 2 deletions(-) diff --git a/digital/io/src/gutter.c b/digital/io/src/gutter.c index 569f0f6b..6bf37567 100644 --- a/digital/io/src/gutter.c +++ b/digital/io/src/gutter.c @@ -26,6 +26,10 @@ #include "gutter.h" #include "fsm.h" +/* Number of cycles to wait. */ +extern uint16_t gutter_wait_cycle_; + +/* Start the gutter FSM. */ void gutter_start (void) { @@ -33,3 +37,13 @@ gutter_start (void) fsm_init (&gutter_fsm); fsm_handle_event (&gutter_fsm, GUTTER_EVENT_start); } + +/* Do we need to generate a wait_finished event? */ +uint8_t +gutter_generate_wait_finished_event (void) +{ + if (gutter_wait_cycle_) + return !(gutter_wait_cycle_--); + else + return 0; +} diff --git a/digital/io/src/gutter.h b/digital/io/src/gutter.h index 32d0b226..c84a9e40 100644 --- a/digital/io/src/gutter.h +++ b/digital/io/src/gutter.h @@ -25,8 +25,23 @@ * * }}} */ -/* gutter start */ +/** + * Start the gutter FSM. + */ void gutter_start (void); +/** + * Do we need to generate a wait_finished event? + * You need to call this function in the main loop in order to ensure we + * generate a wait_finished event when the gutter FSM need one. + * The purpose is to let the gutter FSM wait a specific number of cycle it + * wants. + * @return + * - 0 if you do need to generate a wait_finished event + * - 1 if you need to generate a wait_finished event for the gutter FSM. + */ +uint8_t +gutter_generate_wait_finished_event (void); + #endif /* gutter_h */ diff --git a/digital/io/src/gutter_cb.c b/digital/io/src/gutter_cb.c index 00a00b55..3824a259 100644 --- a/digital/io/src/gutter_cb.c +++ b/digital/io/src/gutter_cb.c @@ -30,6 +30,17 @@ #include "trap.h" /* trap_* */ #include "playground.h" /* PG_GUTTER_A */ +/** + * Gutter private data to wait a certain number of cycles. + */ +uint16_t gutter_wait_cycle_; + +/** + * Count of cycles to wait before we estimate all the balls have been dropped + * into the gutter. A cycle normally last 4.4ms. + */ +#define GUTTER_WAIT_FOR_BALLS_TO_DROP 1126 + /* * ROTATE_REAR_SIDE_TO_GUTTER =bot_move_succeed=> * => GO_TO_THE_GUTTER_WALL @@ -51,6 +62,8 @@ gutter__ROTATE_REAR_SIDE_TO_GUTTER__bot_move_succeed (void) fsm_branch_t gutter__IDLE__start (void) { + /* Initialize internal data */ + gutter_wait_cycle_ = 0; /* Put the bot back to the gutter */ asserv_goto_angle (PG_GUTTER_A); return gutter_next (IDLE, start); @@ -67,7 +80,9 @@ gutter__GO_TO_THE_GUTTER_WALL__bot_move_succeed (void) { /* Open the rear panel */ trap_open_rear_panel (); - /* Wait for a while XXX TODO */ + /* Wait for GUTTER_WAIT_FOR_BALLS_TO_DROP before being calling back by the + * main loop */ + gutter_wait_cycle_ = GUTTER_WAIT_FOR_BALLS_TO_DROP; return gutter_next (GO_TO_THE_GUTTER_WALL, bot_move_succeed); } diff --git a/digital/io/src/main.c b/digital/io/src/main.c index efcd9d4e..61dc77bb 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -44,6 +44,7 @@ #include "getsamples.h" /* getsamples_start */ #include "top.h" /* top_* */ #include "chrono.h" /* chrono_end_match */ +#include "gutter.h" /* gutter_generate_wait_finished_event */ #include "io.h" @@ -136,12 +137,16 @@ main_loop (void) /* Pass it to all the FSM that need it */ fsm_handle_event (&getsamples_fsm, GETSAMPLES_EVENT_bot_move_succeed); + fsm_handle_event (&gutter_fsm, + GUTTER_EVENT_bot_move_succeed); } else if (move_status == failure) { /* Move failed */ fsm_handle_event (&getsamples_fsm, GETSAMPLES_EVENT_bot_move_failed); + fsm_handle_event (&gutter_fsm, + GUTTER_EVENT_bot_move_failed); } asserv_status_e arm_status = asserv_last_cmd_ack () ? asserv_arm_cmd_status () : none; @@ -170,6 +175,11 @@ main_loop (void) { fsm_handle_event (&top_fsm, TOP_EVENT_settings_acknowledged); } + /* Gutter wait_finished event */ + if (gutter_generate_wait_finished_event ()) + { + fsm_handle_event (&gutter_fsm, GUTTER_EVENT_wait_finished); + } /* TODO: Check other sensors */ } } -- cgit v1.2.3