summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/simu.host.c12
-rw-r--r--digital/io/src/simu.host.h14
-rw-r--r--digital/io/tools/io/mex.py23
-rw-r--r--host/simu/robots/marcel/model/bag.py1
-rw-r--r--host/simu/robots/marcel/view/bag.py2
-rw-r--r--host/simu/view/pos_report.py47
6 files changed, 98 insertions, 1 deletions
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])
+