summaryrefslogtreecommitdiff
path: root/digital/io-hub
diff options
context:
space:
mode:
authorNicolas Schodet2012-05-12 02:02:03 +0200
committerNicolas Schodet2012-05-12 11:15:57 +0200
commit382b99e11c033f1952cbe2b9e082cd35812a13e3 (patch)
tree008571474836ee5913085089a0835911239c13cb /digital/io-hub
parentfb73dbae0dc7d6b9f117c74bb8de2a073ab4253b (diff)
digital/ui-hub: add debug draw module to debug path finding
Diffstat (limited to 'digital/io-hub')
-rw-r--r--digital/io-hub/src/common/debug_draw.host.c76
-rw-r--r--digital/io-hub/src/common/debug_draw.host.h58
-rw-r--r--digital/io-hub/src/guybrush/Makefile2
-rw-r--r--digital/io-hub/src/guybrush/path.c23
-rw-r--r--digital/io-hub/src/guybrush/simu.host.c3
-rw-r--r--digital/io-hub/tools/io_hub/mex.py26
6 files changed, 187 insertions, 1 deletions
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)