summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/path.c13
-rw-r--r--digital/io/src/simu.host.c14
-rw-r--r--digital/io/src/simu.host.h4
-rw-r--r--digital/io/src/test_path.c5
-rw-r--r--host/inter/inter_node.py14
-rw-r--r--host/inter/path.py41
6 files changed, 90 insertions, 1 deletions
diff --git a/digital/io/src/path.c b/digital/io/src/path.c
index 959f299e..50724f59 100644
--- a/digital/io/src/path.c
+++ b/digital/io/src/path.c
@@ -26,9 +26,11 @@
#include "path.h"
#include "playground.h"
+#include "simu.host.h"
#include "modules/math/fixed/fixed.h"
#include "modules/utils/utils.h"
+#include "modules/host/mex.h"
/** Number of possible obstacles. */
#define PATH_OBSTACLES_NB 2
@@ -288,6 +290,17 @@ path_update (void)
path_compute_points ();
path_compute_arcs ();
path_dijkstra ();
+#if defined (HOST)
+ uint8_t len, i;
+ uint16_t points[PATH_POINTS_NB * 2];
+ len = 0;
+ for (i = 1; i != 0xff; i = path.points[i].next)
+ {
+ points[len++] = path.points[i].x;
+ points[len++] = path.points[i].y;
+ }
+ simu_send_path (len, points);
+#endif
}
/** Retrieve first path point coordinates. Return 0 on failure. */
diff --git a/digital/io/src/simu.host.c b/digital/io/src/simu.host.c
index 03eed76a..7a497bff 100644
--- a/digital/io/src/simu.host.c
+++ b/digital/io/src/simu.host.c
@@ -39,6 +39,7 @@ enum
MSG_SIMU_IO_COLOR = 0xb1,
MSG_SIMU_IO_SERVO = 0xb2,
MSG_SIMU_IO_SHARPS = 0xb3,
+ MSG_SIMU_IO_PATH = 0xb4,
};
/** Requested servo position. */
@@ -194,3 +195,16 @@ void
eeprom_clear_param (void)
{
}
+
+/** Send computed path. */
+void
+simu_send_path (uint8_t len, uint16_t *points)
+{
+ int i;
+ mex_msg_t *m;
+ m = mex_msg_new (MSG_SIMU_IO_PATH);
+ for (i = 0; i < len; i++)
+ mex_msg_push (m, "h", points[i]);
+ mex_node_send (m);
+}
+
diff --git a/digital/io/src/simu.host.h b/digital/io/src/simu.host.h
index efd3bb2a..91203fce 100644
--- a/digital/io/src/simu.host.h
+++ b/digital/io/src/simu.host.h
@@ -51,6 +51,10 @@ switch_get_color (void);
uint8_t
switch_get_jack (void);
+/** Send computed path. */
+void
+simu_send_path (uint8_t len, uint16_t *points);
+
#endif /* defined (HOST) */
#endif /* simu_host_h */
diff --git a/digital/io/src/test_path.c b/digital/io/src/test_path.c
index 0ec2beb6..3d25e0b7 100644
--- a/digital/io/src/test_path.c
+++ b/digital/io/src/test_path.c
@@ -47,3 +47,8 @@ main (void)
return 0;
}
+void
+simu_send_path (uint8_t len, uint16_t *points)
+{
+}
+
diff --git a/host/inter/inter_node.py b/host/inter/inter_node.py
index c238a4d5..403e2cb9 100644
--- a/host/inter/inter_node.py
+++ b/host/inter/inter_node.py
@@ -28,6 +28,7 @@ if __name__ == '__main__':
from inter import Inter, Obstacle
from dist_sensor import DistSensor
+from path import Path
from Tkinter import *
from mex.node import Node
from mex.msg import Msg
@@ -45,6 +46,7 @@ class InterNode (Inter):
IO_COLOR = 0xb1
IO_SERVO = 0xb2
IO_SHARPS = 0xb3
+ IO_PATH = 0xb4
def __init__ (self):
Inter.__init__ (self)
@@ -55,6 +57,7 @@ class InterNode (Inter):
self.node.register (self.IO_COLOR, self.handle_IO_COLOR)
self.node.register (self.IO_SERVO, self.handle_IO_SERVO)
self.node.register (self.IO_SHARPS, self.handle_IO_SHARPS)
+ self.node.register (self.IO_PATH, self.handle_IO_PATH)
self.tk.createfilehandler (self.node, READABLE, self.read)
self.date = 0
self.synced = True
@@ -72,6 +75,9 @@ class InterNode (Inter):
s.obstacles = self.obstacles
s.hide = True
self.tableview.robot.drawn.extend (self.dist_sensors)
+ self.path = Path (self.tableview.table)
+ self.tableview.drawn.append (self.path)
+ self.tableview
def createWidgets (self):
Inter.createWidgets (self)
@@ -174,12 +180,18 @@ class InterNode (Inter):
assert v >= 0 and v < 1024
self.node.response (m)
+ def handle_IO_PATH (self, msg):
+ self.path.path = [ ]
+ while len (msg) > 4:
+ self.path.path.append (msg.pop ('hh'))
+ self.update (self.path)
+
def place_obstacle (self, ev):
pos = self.tableview.screen_coord ((ev.x, ev.y))
if self.obstacles:
self.obstacles[0].pos = pos
else:
- self.obstacles.append (Obstacle (self.tableview, pos, 150))
+ self.obstacles.append (Obstacle (self.tableview.table, pos, 150))
self.tableview.drawn.append (self.obstacles[0])
self.update (*self.obstacles)
self.update (*self.dist_sensors)
diff --git a/host/inter/path.py b/host/inter/path.py
new file mode 100644
index 00000000..f1463f33
--- /dev/null
+++ b/host/inter/path.py
@@ -0,0 +1,41 @@
+# inter - Robot simulation interface. {{{
+#
+# Copyright (C) 2008 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.
+#
+# }}}
+"""Computed path drawing."""
+
+from Tkinter import *
+from drawable import *
+
+from math import pi, cos, sin, sqrt
+
+class Path (Drawable):
+ """Computed path drawing."""
+
+ def __init__ (self, onto):
+ Drawable.__init__ (self, onto)
+ self.path = [ ]
+
+ def draw (self):
+ self.reset ()
+ if len (self.path) > 2:
+ self.draw_line (self.path, fill = 'green', arrow = LAST)