summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--digital/ai/src/utils/chrono.c6
-rw-r--r--digital/io-hub/src/guybrush/avrconfig.h4
-rw-r--r--digital/io-hub/src/guybrush/main.c50
-rw-r--r--digital/io-hub/src/guybrush/top.c2
4 files changed, 55 insertions, 7 deletions
diff --git a/digital/ai/src/utils/chrono.c b/digital/ai/src/utils/chrono.c
index 8560c3e0..d438d4b8 100644
--- a/digital/ai/src/utils/chrono.c
+++ b/digital/ai/src/utils/chrono.c
@@ -26,6 +26,7 @@
#include "timer.h"
#include "asserv.h"
+#include "beacon.h"
#include "twi_master.h"
#include "modules/utils/utils.h"
@@ -93,6 +94,11 @@ chrono_remaining_time (void)
void
chrono_end_match (uint8_t block)
{
+ vect_t v = { 0, 0 };
+ /* Stop beacon system. */
+ beacon_on (0);
+ if (AC_AI_TWI_MASTER_BEACON)
+ beacon_send_position (&v);
/* Make the bot stop moving */
asserv_stop_motor ();
/* Wait until complete */
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);
}