From 960a716bceeeb0fbf58dec1859725a070d486f74 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 30 Apr 2008 13:20:03 +0200 Subject: * host/inter, digital/io/src: - added computed path drawing. --- digital/io/src/path.c | 13 +++++++++++++ digital/io/src/simu.host.c | 14 ++++++++++++++ digital/io/src/simu.host.h | 4 ++++ digital/io/src/test_path.c | 5 +++++ host/inter/inter_node.py | 14 +++++++++++++- host/inter/path.py | 41 +++++++++++++++++++++++++++++++++++++++++ 6 files changed, 90 insertions(+), 1 deletion(-) create mode 100644 host/inter/path.py 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) -- cgit v1.2.3