From 1ff9a0a61718a52f60a770a40aaf0915f77cb707 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 12 May 2010 17:37:28 +0200 Subject: digital/io/src: generate an event when in field --- digital/io/src/main.c | 10 +++++++++- digital/io/src/move.c | 6 ++++-- digital/io/src/move.h | 4 ++-- digital/io/src/playground.h | 3 +++ digital/io/src/top.fsm | 2 ++ 5 files changed, 20 insertions(+), 5 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/main.c b/digital/io/src/main.c index f03d0ea6..f33afbb5 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -192,7 +192,15 @@ main_event_to_fsm (void) } /* Check obstacles. */ - move_check_obstacles (); + if (move_check_obstacles ()) + return; + + /* Top events. */ + position_t robot_pos; + asserv_get_position (&robot_pos); + if (robot_pos.v.y < PG_FIELD_Y_MAX) + FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_in_field); + /* TODO: Check other sensors */ } diff --git a/digital/io/src/move.c b/digital/io/src/move.c index c0676c3d..24b8848d 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -58,7 +58,7 @@ move_obstacles_update (void) MOVE_OBSTACLE_VALIDITY); } -void +uint8_t move_check_obstacles (void) { if (fsm_can_handle_event (&ai_fsm, AI_EVENT_obstacle_in_front)) @@ -67,7 +67,9 @@ move_check_obstacles (void) asserv_get_position (&robot_pos); if (radar_blocking (&robot_pos.v, &move_data.step, main_obstacles_pos, main_obstacles_nb)) - fsm_handle_event (&ai_fsm, AI_EVENT_obstacle_in_front); + if (fsm_handle_event (&ai_fsm, AI_EVENT_obstacle_in_front)) + return 1; } + return 0; } diff --git a/digital/io/src/move.h b/digital/io/src/move.h index 173de27e..c504a0b2 100644 --- a/digital/io/src/move.h +++ b/digital/io/src/move.h @@ -75,8 +75,8 @@ move_start (position_t position, uint8_t backward); void move_obstacles_update (void); -/** Check for blocking obstacles. */ -void +/** Check for blocking obstacles, return non zero if an event is handled. */ +uint8_t move_check_obstacles (void); #endif /* move_h */ diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h index 1ec6e47e..68e17ba0 100644 --- a/digital/io/src/playground.h +++ b/digital/io/src/playground.h @@ -86,4 +86,7 @@ #define PG_SLOPE_WIDTH (500 + 519 + 500) #define PG_SLOPE_LENGTH (500 + 22) +/** Start of field zone. */ +#define PG_FIELD_Y_MAX 1128 + #endif // playground_h diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm index 5a5df1ad..c3581c54 100644 --- a/digital/io/src/top.fsm +++ b/digital/io/src/top.fsm @@ -25,6 +25,8 @@ Events: when the bot has failed its asserv movement. init_match_is_started when the init FSM has finished and the match start. + in_field + the robot is in the zone containing interesting elements. state_timeout timeout in cycle count. asserv_last_cmd_ack -- cgit v1.2.3