summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/guybrush/main.c
diff options
context:
space:
mode:
authorNicolas Schodet2012-06-01 22:41:57 +0200
committerNicolas Schodet2012-06-02 00:32:07 +0200
commitf4fdf2e829ee8e1e5d3167a624280acffa09d7de (patch)
tree5c34b90238e538202aa748c46ec3636e2589e484 /digital/io-hub/src/guybrush/main.c
parenta1c8880ef806ba64f5f985cec8e845f01ebae228 (diff)
digital/io-hub/src/guybrush: integrate beacon system
Diffstat (limited to 'digital/io-hub/src/guybrush/main.c')
-rw-r--r--digital/io-hub/src/guybrush/main.c50
1 files changed, 45 insertions, 5 deletions
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);
}