From d3c6012abe3b59ba88c8c83b2ffcb467b472f00d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 7 Apr 2010 23:51:42 +0200 Subject: digital/io, host/simu: add position report tool --- digital/io/src/simu.host.c | 12 +++++++++ digital/io/src/simu.host.h | 14 ++++++++++- digital/io/tools/io/mex.py | 23 ++++++++++++++++++ host/simu/robots/marcel/model/bag.py | 1 + host/simu/robots/marcel/view/bag.py | 2 ++ host/simu/view/pos_report.py | 47 ++++++++++++++++++++++++++++++++++++ 6 files changed, 98 insertions(+), 1 deletion(-) create mode 100644 host/simu/view/pos_report.py diff --git a/digital/io/src/simu.host.c b/digital/io/src/simu.host.c index 5b1da61f..134d0ba4 100644 --- a/digital/io/src/simu.host.c +++ b/digital/io/src/simu.host.c @@ -44,6 +44,7 @@ enum MSG_SIMU_IO_PATH = 0xb4, MSG_SIMU_IO_PWM = 0xb5, MSG_SIMU_IO_CONTACT = 0xb6, + MSG_SIMU_IO_POS_REPORT = 0xb7, }; /** Requested servo position. */ @@ -243,3 +244,14 @@ simu_send_path (uint16_t *points, uint8_t len, mex_node_send (m); } +void +simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id) +{ + mex_msg_t *m; + m = mex_msg_new (MSG_SIMU_IO_POS_REPORT); + mex_msg_push (m, "b", id); + for (; pos_nb; pos++, pos_nb--) + mex_msg_push (m, "hh", pos->x, pos->y); + mex_node_send (m); +} + diff --git a/digital/io/src/simu.host.h b/digital/io/src/simu.host.h index f314db87..f885a1c5 100644 --- a/digital/io/src/simu.host.h +++ b/digital/io/src/simu.host.h @@ -26,6 +26,7 @@ * }}} */ #include "common.h" +#include "modules/math/geometry/vect.h" #ifdef HOST @@ -47,6 +48,17 @@ switch_get_color (void); uint8_t switch_get_jack (void); -#endif /* defined (HOST) */ +/** Send general purpose positions to indicate computation results. + * - pos: array of positions to report. + * - pos_nb: number of elements in the array. + * - id: identifier so that several unrelated positions could be reported. */ +void +simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id); + +#else /* !defined (HOST) */ + +#define simu_send_pos_report(pos, pos_nb, id) ((void) 0) + +#endif /* !defined (HOST) */ #endif /* simu_host_h */ diff --git a/digital/io/tools/io/mex.py b/digital/io/tools/io/mex.py index 8f951f91..16c8eef6 100644 --- a/digital/io/tools/io/mex.py +++ b/digital/io/tools/io/mex.py @@ -33,6 +33,7 @@ ID_ADC = 0xb3 ID_PATH = 0xb4 ID_PWM = 0xb5 ID_CONTACT = 0xb6 +ID_POS_REPORT = 0xb7 SERVO_NB = 6 SERVO_VALUE_MAX = 255 @@ -198,6 +199,27 @@ class Mex: m.push ('B', self.contacts) self.node.send (m) + class PosReport (Observable): + """General purpose position report. + + - pos: dict of sequence of (x, y) coordinates (millimeters). The dict + is indexed by position identifier. + + """ + + def __init__ (self, node): + Observable.__init__ (self) + self.pos = { } + node.register (ID_POS_REPORT, self.__handle) + + def __handle (self, msg): + p = [ ] + id, = msg.pop ('b') + while len (msg) >= 4: + p.append (msg.pop ('hh')) + self.pos[id] = p + self.notify () + def __init__ (self, node): self.jack = self.Switch (node, ID_JACK) self.color_switch = self.Switch (node, ID_COLOR) @@ -211,4 +233,5 @@ class Mex: self.__contact_pack = self.Contact.Pack (node) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) + self.pos_report = self.PosReport (node) diff --git a/host/simu/robots/marcel/model/bag.py b/host/simu/robots/marcel/model/bag.py index ab4814e0..563a16b4 100644 --- a/host/simu/robots/marcel/model/bag.py +++ b/host/simu/robots/marcel/model/bag.py @@ -47,4 +47,5 @@ class Bag: link_bag.io.adc[4].value = 0 link_bag.io.adc[5].value = 0 self.path = link_bag.io.path + self.pos_report = link_bag.io.pos_report diff --git a/host/simu/robots/marcel/view/bag.py b/host/simu/robots/marcel/view/bag.py index e8666749..75f7ff50 100644 --- a/host/simu/robots/marcel/view/bag.py +++ b/host/simu/robots/marcel/view/bag.py @@ -25,6 +25,7 @@ from simu.view.switch import Switch from simu.view.distance_sensor_us import DistanceSensorUS from simu.view.path import Path +from simu.view.pos_report import PosReport from simu.robots.marcel.view.robot import Robot class Bag: @@ -37,4 +38,5 @@ class Bag: self.distance_sensor = [DistanceSensorUS (self.robot, ds) for ds in model_bag.distance_sensor] self.path = Path (table, model_bag.path) + self.pos_report = PosReport (table, model_bag.pos_report) diff --git a/host/simu/view/pos_report.py b/host/simu/view/pos_report.py new file mode 100644 index 00000000..2103997f --- /dev/null +++ b/host/simu/view/pos_report.py @@ -0,0 +1,47 @@ +# simu - Robot simulation. {{{ +# +# Copyright (C) 2010 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. +# +# }}} +"""Display general purpose position reports.""" +from simu.inter.drawable import Drawable + +class PosReport (Drawable): + + def __init__ (self, onto, model): + Drawable.__init__ (self, onto) + self.model = model + self.model.register (self.__notified) + self.__colors = ('red', 'blue', 'green', 'yellow') + + def __notified (self): + self.update () + + def draw (self): + self.reset () + for id in self.model.pos.iterkeys (): + for p in self.model.pos[id]: + s = 20 + self.draw_line ( + (p[0] - s, p[1] - s), (p[0] + s, p[1] + s), + (p[0] - s, p[1] + s), (p[0] + s, p[1] - s), + (p[0] - s, p[1] - s), fill = self.__colors[id]) + -- cgit v1.2.3