From 4cad0f3f5d037062aad81a88514e04336e473829 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 10 May 2010 08:32:39 +0200 Subject: digital/mimot: add motor limit mex message --- digital/mimot/src/dirty/simu.host.c | 33 +++++++++++++++++++++++++++ digital/mimot/tools/mimot/mex.py | 45 +++++++++++++++++++++++++++++++++++++ 2 files changed, 78 insertions(+) diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c index 1f0d6394..c773f5a2 100644 --- a/digital/mimot/src/dirty/simu.host.c +++ b/digital/mimot/src/dirty/simu.host.c @@ -22,6 +22,7 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#define _GNU_SOURCE 1 /* Need ISO C99 features as well. */ #include "common.h" #include "simu.host.h" @@ -76,6 +77,37 @@ int simu_mex; /** Counter to limit the interval between information is sent. */ int simu_send_cpt; +static void +simu_handle_limits__update_limit (double *th_limit, int32_t limit_int, + double i_G, int sign) +{ + if (limit_int == -1) + /* No change. */ + ; + else if (limit_int == -2) + /* INFINITY. */ + *th_limit = sign > 0 ? INFINITY : -INFINITY; + else + *th_limit = (double) limit_int / 1024.0 * i_G; +} + +static void +simu_handle_limits (void *user, mex_msg_t *msg) +{ + int i; + int32_t limit_min_int, limit_max_int; + for (i = 0; i < AC_ASSERV_AUX_NB; i++) + { + mex_msg_pop (msg, "ll", &limit_min_int, &limit_max_int); + simu_handle_limits__update_limit (&simu_aux_model[i].m.th_min, + limit_min_int, + simu_aux_model[i].m.i_G, -1); + simu_handle_limits__update_limit (&simu_aux_model[i].m.th_max, + limit_max_int, + simu_aux_model[i].m.i_G, +1); + } +} + /** Initialise simulation. */ static void simu_init (void) @@ -89,6 +121,7 @@ simu_init (void) if (!simu_mex) simu_mex = 1; simu_send_cpt = simu_mex; mex_node_connect (); + mex_node_register (0xcc, simu_handle_limits, 0); argc--; argv++; } if (argc != 1) diff --git a/digital/mimot/tools/mimot/mex.py b/digital/mimot/tools/mimot/mex.py index b32f2925..a190c1cc 100644 --- a/digital/mimot/tools/mimot/mex.py +++ b/digital/mimot/tools/mimot/mex.py @@ -24,8 +24,10 @@ """Mex interface to mimot.""" from utils.observable import Observable +import simu.mex.msg ID_AUX = 0xc8 +ID_LIMITS = 0xcc class Mex: """Handle communications with simulated mimot.""" @@ -54,7 +56,50 @@ class Mex: aux.angle = float (angle) / 1024 aux.notify () + class Limits (Observable): + """Motor limits. + + - min, max: limits in radian. + + """ + + def __init__ (self, pack, index): + Observable.__init__ (self) + self.pack = pack + self.index = index + self.min = None + self.max = None + self.register (self.__notified) + + def __notified (self): + self.pack.set (self.index, self.min, self.max) + + class Pack: + """Handle emission of several limits for one message.""" + + def __init__ (self, node): + self.node = node + self.limits = [ None, None, None, None ] + + def set (self, index, min, max): + self.limits[index * 2] = min + self.limits[index * 2 + 1] = max + self.__send () + + def __send (self): + m = simu.mex.msg.Msg (ID_LIMITS) + for l in self.limits: + if l is None: + li = -1 + else: + li = int (l * 1024) + m.push ('l', li) + self.node.send (m) + def __init__ (self, node): self.aux = (self.Aux (), self.Aux ()) self.__aux_pack = self.Aux.Pack (node, self.aux) + self.__limits_pack = self.Limits.Pack (node) + for index, aux in enumerate (self.aux): + aux.limits = self.Limits (self.__limits_pack, index) -- cgit v1.2.3