summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/mimot/src/dirty/simu.host.c33
-rw-r--r--digital/mimot/tools/mimot/mex.py45
2 files changed, 78 insertions, 0 deletions
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)