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 +++++++++++++++++++++++ 3 files changed, 48 insertions(+), 1 deletion(-) (limited to 'digital/io') 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) -- cgit v1.2.3