From d1970dba0d168fdf9b89f3f7446ea2300fac9d30 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 30 Apr 2008 01:37:18 +0200 Subject: * digital/io/src - integrate sharp to generate obstacle event for move FSM. --- digital/io/src/asserv.c | 14 ++++++++++++++ digital/io/src/asserv.h | 10 ++++++++++ digital/io/src/main.c | 22 ++++++++++++++++++++++ 3 files changed, 46 insertions(+) (limited to 'digital/io/src') diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index 4b3a5795..2505da88 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -307,6 +307,20 @@ asserv_get_arm_position (void) return asserv_status.arm_position; } +/* Are we moving forward/backward? */ +uint8_t +asserv_get_moving_direction (void) +{ + /* Foward move? */ + if (asserv_status.status & _BV (4)) + return 1; + /* Backward move? */ + if (asserv_status.status & _BV (5)) + return 2; + /* Not moving */ + return 0; +} + /* Reset the asserv board. */ void asserv_reset (void) diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index 686cddca..ebadf188 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -139,6 +139,16 @@ asserv_get_position (asserv_position_t *current_position); uint16_t asserv_get_arm_position (void); +/** + * Are we moving forward/backward? + * @return + * - 0 we are not moving; + * - 1 we are moving forward; + * - 2 we are moving backward. + */ +uint8_t +asserv_get_moving_direction (void); + /** * Reset the asserv board. * Other class command. diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 46b171c6..6a709d0c 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -243,6 +243,28 @@ main_loop (void) /* Post the event */ FSM_HANDLE_EVENT (&top_fsm, save_event); } + /* Sharps event for move FSM */ + uint8_t moving_direction = asserv_get_moving_direction (); + if (moving_direction) + { + if (moving_direction == 1) + { + /* Front only */ + if (sharp_get_interpreted (SHARP_FRONT_LEFT) || + sharp_get_interpreted (SHARP_FRONT_RIGHT)) + /* Generate an event for move FSM */ + FSM_HANDLE_EVENT (&move_fsm, + MOVE_EVENT_bot_move_obstacle); + } + else + { + /* Back only */ + if (sharp_get_interpreted (SHARP_BACK_LEFT) || + sharp_get_interpreted (SHARP_BACK_RIGHT)) + /* Generate an event for move FSM */ + FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_bot_move_obstacle); + } + } /* TODO: Check other sensors */ } -- cgit v1.2.3