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/common/debug_draw.host.c | 76 +++++++++++++++++++++++++++++ digital/io-hub/src/common/debug_draw.host.h | 58 ++++++++++++++++++++++ digital/io-hub/src/guybrush/Makefile | 2 +- digital/io-hub/src/guybrush/path.c | 23 +++++++++ digital/io-hub/src/guybrush/simu.host.c | 3 ++ digital/io-hub/tools/io_hub/mex.py | 26 ++++++++++ 6 files changed, 187 insertions(+), 1 deletion(-) create mode 100644 digital/io-hub/src/common/debug_draw.host.c create mode 100644 digital/io-hub/src/common/debug_draw.host.h (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/common/debug_draw.host.c b/digital/io-hub/src/common/debug_draw.host.c new file mode 100644 index 00000000..ea743bad --- /dev/null +++ b/digital/io-hub/src/common/debug_draw.host.c @@ -0,0 +1,76 @@ +/* debug_draw.host.c */ +/* io-hub - Modular Input/Output. {{{ + * + * Copyright (C) 2012 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" +#include "debug_draw.host.h" + +#include "modules/host/host.h" +#include "modules/host/mex.h" + +/** Message type. */ +uint8_t debug_draw_mtype; + +/** Message being composed. */ +mex_msg_t *debug_draw_msg; + +void +debug_draw_init (void) +{ + const char *mex_instance = host_get_instance ("io-hub0", 0); + debug_draw_mtype = mex_node_reservef ("%s:debug-draw", mex_instance); +} + +void +debug_draw_start (void) +{ + assert (!debug_draw_msg); + debug_draw_msg = mex_msg_new (debug_draw_mtype); +} + +void +debug_draw_send (void) +{ + mex_node_send (debug_draw_msg); + debug_draw_msg = 0; +} + +void +debug_draw_circle (const vect_t *p, int16_t radius, uint8_t color) +{ + mex_msg_push (debug_draw_msg, "Bhhhb", 'c', p->x, p->y, radius, color); +} + +void +debug_draw_segment (const vect_t *p1, const vect_t *p2, uint8_t color) +{ + mex_msg_push (debug_draw_msg, "Bhhhhb", 's', p1->x, p1->y, p2->x, p2->y, + color); +} + +void +debug_draw_point (const vect_t *p, uint8_t color) +{ + mex_msg_push (debug_draw_msg, "Bhhb", 'p', p->x, p->y, color); +} + diff --git a/digital/io-hub/src/common/debug_draw.host.h b/digital/io-hub/src/common/debug_draw.host.h new file mode 100644 index 00000000..fe46e80a --- /dev/null +++ b/digital/io-hub/src/common/debug_draw.host.h @@ -0,0 +1,58 @@ +#ifndef debug_draw_host_h +#define debug_draw_host_h +/* debug_draw.host.h */ +/* io-hub - Modular Input/Output. {{{ + * + * Copyright (C) 2012 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "defs.h" + +#ifdef HOST + +/** Initialise module. */ +void +debug_draw_init (void); + +/** Start a debug draw message, to be filled with debug draw primitives, then + * sent. */ +void +debug_draw_start (void); + +/** Send prepared message. */ +void +debug_draw_send (void); + +/** Append a debug circle. */ +void +debug_draw_circle (const vect_t *p, int16_t radius, uint8_t color); + +/** Append a debug segment. */ +void +debug_draw_segment (const vect_t *p1, const vect_t *p2, uint8_t color); + +/** Append a debug point. */ +void +debug_draw_point (const vect_t *p, uint8_t color); + +#endif + +#endif /* debug_draw_host_h */ 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. */ diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index dd383dbc..989a992c 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -224,6 +224,31 @@ class Mex: self.pos[id] = p self.notify () + class DebugDraw (Observable): + """General purpose debug drawing.""" + + def __init__ (self, node, instance): + Observable.__init__ (self) + self.drawing = [ ] + node.register (instance + ':debug-draw', self.__handle) + + def __handle (self, msg): + self.drawing = [ ] + while len (msg): + t, = msg.pop ('c') + if t == 'c': + x, y, r, c = msg.pop ('hhhb') + self.drawing.append (['circle', x, y, r, c]) + elif t == 's': + x1, y1, x2, y2, c = msg.pop ('hhhhb') + self.drawing.append (['segment', x1, y1, x2, y2, c]) + elif t == 'p': + x, y, c = msg.pop ('hhb') + self.drawing.append (['point', x, y, c]) + else: + raise ValueError + self.notify () + def __init__ (self, node, instance = 'io-hub0', pwm_nb = 0, contact_nb = 0, output_nb = 0, codebar = False): self.adc = tuple (self.ADC (node, instance, i) for i in range (0, ADC_NB)) @@ -244,4 +269,5 @@ class Mex: for i in (0, 1)) self.path = self.Path (node, instance) self.pos_report = self.PosReport (node, instance) + self.debug_draw = self.DebugDraw (node, instance) -- cgit v1.2.3