From f4fdf2e829ee8e1e5d3167a624280acffa09d7de Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 1 Jun 2012 22:41:57 +0200 Subject: digital/io-hub/src/guybrush: integrate beacon system --- digital/io-hub/src/guybrush/avrconfig.h | 4 +-- digital/io-hub/src/guybrush/main.c | 50 +++++++++++++++++++++++++++++---- digital/io-hub/src/guybrush/top.c | 2 ++ 3 files changed, 49 insertions(+), 7 deletions(-) (limited to 'digital/io-hub/src') diff --git a/digital/io-hub/src/guybrush/avrconfig.h b/digital/io-hub/src/guybrush/avrconfig.h index b21b25ad..ea058227 100644 --- a/digital/io-hub/src/guybrush/avrconfig.h +++ b/digital/io-hub/src/guybrush/avrconfig.h @@ -118,7 +118,7 @@ /** Report function name. */ #define AC_PATH_REPORT_CALLBACK simu_send_path /** Number of possible obstacles. */ -#define AC_PATH_OBSTACLES_NB 2 +#define AC_PATH_OBSTACLES_NB 4 /* astar - A* path finding module. */ /** Neighbor callback. */ @@ -136,7 +136,7 @@ /** Use mimot board. */ #define AC_AI_TWI_MASTER_MIMOT 1 /** Use beacon board. */ -#define AC_AI_TWI_MASTER_BEACON 0 +#define AC_AI_TWI_MASTER_BEACON 1 /* beacon. */ /** Number of beacon positions. */ diff --git a/digital/io-hub/src/guybrush/main.c b/digital/io-hub/src/guybrush/main.c index 5fa1b22d..b366e54b 100644 --- a/digital/io-hub/src/guybrush/main.c +++ b/digital/io-hub/src/guybrush/main.c @@ -27,6 +27,7 @@ #include "modules/utils/utils.h" #include "modules/uart/uart.h" #include "modules/proto/proto.h" +#include "modules/math/geometry/distance.h" #include "modules/devices/usdist/usdist.h" @@ -70,7 +71,7 @@ enum team_color_e team_color; /** Obstacles positions, updated using radar module. */ -vect_t main_obstacles_pos[2]; +vect_t main_obstacles_pos[AC_PATH_OBSTACLES_NB]; /** Number of obstacles in main_obstacles_pos. */ uint8_t main_obstacles_nb; @@ -154,9 +155,8 @@ main_init (void) /* TWI communications */ asserv_init (); mimot_init (); -#if AC_AI_TWI_MASTER_BEACON - beacon_init (); -#endif + if (AC_AI_TWI_MASTER_BEACON) + beacon_init (); twi_master_init (); /* AI modules. */ path_init (); @@ -244,6 +244,29 @@ main_coin_detected_ok (void) return 1; } +static void +main_obstacles_merge (vect_t *obs_pos, uint8_t obs_nb) +{ + uint8_t i, j; + for (i = 0; i < obs_nb; i++) + { + /* Look at existing obstacles. */ + for (j = 0; j < main_obstacles_nb; j++) + { + /* If same obstacle, break. */ + if (distance_point_point (&obs_pos[i], + &main_obstacles_pos[j]) < 150) + break; + } + /* Else, add it. */ + if (j == main_obstacles_nb) + { + main_obstacles_pos[main_obstacles_nb] = obs_pos[i]; + main_obstacles_nb++; + } + } +} + /** Main events management. */ uint8_t main_event_to_fsm (void) @@ -349,7 +372,24 @@ main_loop (void) { position_t robot_pos; asserv_get_position (&robot_pos); - main_obstacles_nb = radar_update (&robot_pos, main_obstacles_pos); + /* Get obstacles from beacon system. */ + main_obstacles_nb = 0; + if (AC_AI_TWI_MASTER_BEACON) + { + uint8_t i; + for (i = 0; i < AC_BEACON_POSITION_NB; i++) + { + if (beacon_get_position + (i, &main_obstacles_pos[main_obstacles_nb]) > 70) + main_obstacles_nb++; + } + beacon_send_position (&robot_pos.v); + } + /* Get obstacles from radar. */ + vect_t radar_obs_pos[2]; + uint8_t radar_obs_nb = radar_update (&robot_pos, radar_obs_pos); + main_obstacles_merge (radar_obs_pos, radar_obs_nb); + /* Set as move obstacles. */ move_obstacles_update (); simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0); } diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c index 10f65c7f..803957e8 100644 --- a/digital/io-hub/src/guybrush/top.c +++ b/digital/io-hub/src/guybrush/top.c @@ -41,6 +41,7 @@ #include "path.h" #include "bottom_clamp.h" #include "radar_defs.h" +#include "beacon.h" #include "output_defs.h" @@ -284,6 +285,7 @@ FSM_TRANS (TOP_INIT_DOOR, init_done, TOP_INIT) FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION) { main_usdist = 1; + beacon_on (1); return FSM_NEXT (TOP_INIT, init_start_round); } -- cgit v1.2.3