From 382b99e11c033f1952cbe2b9e082cd35812a13e3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 12 May 2012 02:02:03 +0200 Subject: digital/ui-hub: add debug draw module to debug path finding --- digital/io-hub/src/guybrush/Makefile | 2 +- digital/io-hub/src/guybrush/path.c | 23 +++++++++++++++++++++++ digital/io-hub/src/guybrush/simu.host.c | 3 +++ 3 files changed, 27 insertions(+), 1 deletion(-) (limited to 'digital/io-hub/src/guybrush') diff --git a/digital/io-hub/src/guybrush/Makefile b/digital/io-hub/src/guybrush/Makefile index ecb7ff28..a26a05a4 100644 --- a/digital/io-hub/src/guybrush/Makefile +++ b/digital/io-hub/src/guybrush/Makefile @@ -10,7 +10,7 @@ io_hub_SOURCES = main.c top.c strat.c \ contact.avr.c contact.host.c \ output.c output.host.c \ twi_master.c asserv.c mimot.c beacon.c \ - chrono.c timer.avr.c simu.host.c \ + chrono.c timer.avr.c simu.host.c debug_draw.host.c \ bottom_clamp.c # Modules needed for IO. MODULES = proto uart twi utils \ diff --git a/digital/io-hub/src/guybrush/path.c b/digital/io-hub/src/guybrush/path.c index 5696a2f8..47427a93 100644 --- a/digital/io-hub/src/guybrush/path.c +++ b/digital/io-hub/src/guybrush/path.c @@ -39,6 +39,12 @@ #include "debug.host.h" #endif +#define PATH_DEBUG_DRAW 0 + +#if PATH_DEBUG_DRAW +#include "debug_draw.host.h" +#endif + /** * This year, due to the large number of obstacles, a grid like structure is * used for path finding on the playground. The A* algorithm is used to find @@ -425,6 +431,23 @@ path_update (void) PATH_OBSTACLES_NB); } #endif +#if PATH_DEBUG_DRAW + uint8_t i; + debug_draw_start (); + for (i = 0; i < UTILS_COUNT (path_blocking_point); i++) + debug_draw_circle (&path_blocking_point[i].pos, + path_blocking_point[i].radius, 0); + for (i = 0; i < UTILS_COUNT (path_blocking_segment); i++) + debug_draw_segment (&path_blocking_segment[i][0], + &path_blocking_segment[i][1], 0); + for (i = 0; i < PATH_NODES_NB; i++) + { + vect_t pos; + path_pos (i, &pos); + debug_draw_point (&pos, 1); + } + debug_draw_send (); +#endif } uint8_t diff --git a/digital/io-hub/src/guybrush/simu.host.c b/digital/io-hub/src/guybrush/simu.host.c index d9830575..e4cd61f0 100644 --- a/digital/io-hub/src/guybrush/simu.host.c +++ b/digital/io-hub/src/guybrush/simu.host.c @@ -33,6 +33,8 @@ #include "modules/path/path.h" #include "io.h" +#include "debug_draw.host.h" + /** AVR registers. */ uint8_t PORTA, PORTB, PORTC, PORTD, PORTF; uint8_t DDRA, DDRB, DDRC, DDRD, DDRF; @@ -63,6 +65,7 @@ simu_init (void) simu_mex_pos_report = mex_node_reservef ("%s:pos-report", mex_instance); simu_mex_path = mex_node_reservef ("%s:path", mex_instance); output_host_init (); + debug_draw_init (); } /** Make a simulation step. */ -- cgit v1.2.3