From 5e074e1df70ce948dbc72c13610a12d925af5a00 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 8 May 2009 23:07:28 +0200 Subject: * digital/io: - added fsm timeout handling. --- digital/io/src/cylinder.fsm | 2 +- digital/io/src/elevator.fsm | 1 + digital/io/src/filterbridge.fsm | 2 +- digital/io/src/fsm.c | 20 ++++++++++++++++++++ digital/io/src/fsm.h | 10 ++++++++++ digital/io/src/fsm_templates/template_fsm.c | 8 ++++++++ digital/io/src/main.c | 12 ++++++++++++ digital/io/src/move.fsm | 1 + digital/io/src/top.fsm | 1 + 9 files changed, 55 insertions(+), 2 deletions(-) diff --git a/digital/io/src/cylinder.fsm b/digital/io/src/cylinder.fsm index 5b4827ae..016c5410 100644 --- a/digital/io/src/cylinder.fsm +++ b/digital/io/src/cylinder.fsm @@ -43,7 +43,7 @@ Events: Optical fence probe finished lift_ready lift ready to get pucks - + state_timeout IDLE: jack_in -> INIT diff --git a/digital/io/src/elevator.fsm b/digital/io/src/elevator.fsm index 70d1e297..b35ce04e 100644 --- a/digital/io/src/elevator.fsm +++ b/digital/io/src/elevator.fsm @@ -37,6 +37,7 @@ Events: doors are opened doors_closed doors are closed + state_timeout IDLE: started -> GO_TO_POS_X diff --git a/digital/io/src/filterbridge.fsm b/digital/io/src/filterbridge.fsm index 13027069..97bcfa54 100644 --- a/digital/io/src/filterbridge.fsm +++ b/digital/io/src/filterbridge.fsm @@ -35,7 +35,7 @@ Events: the result of the investigation will be revealed (suspens!!) door_closed ready to test a new puck - + state_timeout IDLE: lift_ready -> WAIT_A_PUCK diff --git a/digital/io/src/fsm.c b/digital/io/src/fsm.c index 945896a6..5e83f550 100644 --- a/digital/io/src/fsm.c +++ b/digital/io/src/fsm.c @@ -37,6 +37,25 @@ fsm_init (fsm_t *fsm) fsm->state_current = fsm->state_init; } +/** Handle state timeout, return 1 if a event was handled. */ +uint8_t +fsm_handle_timeout (fsm_t *fsm) +{ + assert (fsm); + /* If there is a timeout for this state. */ + if (fsm->state_timeout != 0xffff) + { + if (fsm->state_timeout) + fsm->state_timeout--; + else + { + /* Timeout expired, generate corresponding event. */ + return fsm_handle_event (fsm, fsm->state_timeout_event); + } + } + return 0; +} + /** Handle an event on the given FSM. */ uint8_t fsm_handle_event (fsm_t *fsm, u8 event) @@ -62,6 +81,7 @@ fsm_handle_event (fsm_t *fsm, u8 event) #else fsm->state_current = br; #endif + fsm->state_timeout = fsm->state_timeout_table[fsm->state_current]; return 1; } return 0; diff --git a/digital/io/src/fsm.h b/digital/io/src/fsm.h index 8700cadc..cf5ce15e 100644 --- a/digital/io/src/fsm.h +++ b/digital/io/src/fsm.h @@ -74,6 +74,12 @@ struct fsm_t u8 state_init; /** Current active state. */ u8 state_current; + /** State timeout table. */ + const u16 *state_timeout_table; + /** Current state timeout if not 0xffff. */ + u16 state_timeout; + /** Event to generate on state timeout. */ + u8 state_timeout_event; #ifdef HOST /** FSM name. */ const char *name; @@ -89,6 +95,10 @@ typedef struct fsm_t fsm_t; void fsm_init (fsm_t *fsm); +/** Handle state timeout, return 1 if a event was handled. */ +uint8_t +fsm_handle_timeout (fsm_t *fsm); + /** Handle an event on the given FSM. * @return * - 0 if this FSM does not handle this event ; diff --git a/digital/io/src/fsm_templates/template_fsm.c b/digital/io/src/fsm_templates/template_fsm.c index dfbdd4b4..87367189 100644 --- a/digital/io/src/fsm_templates/template_fsm.c +++ b/digital/io/src/fsm_templates/template_fsm.c @@ -15,6 +15,11 @@ static const fsm_transition_t %(prefix)s_transition_table[%(PREFIX)s_STATE_NB][%(PREFIX)s_EVENT_NB] = { %(transition_table)s}; +/* %(name)s state timeout table. */ +static const uint16_t +%(prefix)s_state_timeout_table[%(PREFIX)s_STATE_NB] = { +%(states,%(@timeout|0xffff)s)s}; + #ifdef HOST /* %(name)s state names. */ @@ -35,6 +40,9 @@ fsm_t %(prefix)s_fsm = { %(PREFIX)s_EVENT_NB, %(PREFIX)s_STATE_%(initial)s, %(PREFIX)s_STATE_%(initial)s, + &%(prefix)s_state_timeout_table[0], + 0, + %(PREFIX)s_EVENT_state_timeout, #ifdef HOST "%(name)s", %(prefix)s_states_names, diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 760dc5d3..13b02353 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -164,6 +164,12 @@ main_loop (void) continue; \ } \ } +#define FSM_HANDLE_TIMEOUT(fsm) \ + { if (fsm_handle_timeout (fsm)) \ + { \ + continue; \ + } \ + } /* Infinite loop */ while (1) @@ -226,6 +232,12 @@ main_loop (void) /* Reset counter */ main_sharp_freq_counter_ = 0; } + /* Update FSM timeouts. */ + FSM_HANDLE_TIMEOUT (&move_fsm); + FSM_HANDLE_TIMEOUT (&top_fsm); + FSM_HANDLE_TIMEOUT (&filterbridge_fsm); + FSM_HANDLE_TIMEOUT (&elevator_fsm); + FSM_HANDLE_TIMEOUT (&cylinder_fsm); /* Update main */ asserv_status_e move_status = asserv_last_cmd_ack () diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index c08d0454..581378ed 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -27,6 +27,7 @@ Events: the bot has seen something (with the sharps) wait_finished we have wait the desired time + state_timeout IDLE: start -> MOVING_TO_FINAL_POSITION diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index c3a492ec..3e1fb456 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -32,6 +32,7 @@ Events: the jack is removed from the bot settings_acknowledged the asserv board has acknowledged the settings sent + state_timeout IDLE: start -> WAIT_JACK_IN -- cgit v1.2.3