From 82783f389d48bfaef2a8a96c178763ad55b1e65b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 4 Apr 2008 17:14:38 +0200 Subject: * digital/asserv/src/asserv, host/inter: - added asserv simu messages. --- digital/asserv/src/asserv/simu.host.c | 40 +++++++++++++++++++++++++++++++++-- host/inter/inter_node.py | 13 ++++++++++++ 2 files changed, 51 insertions(+), 2 deletions(-) diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 417ffea1..2264e74c 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -26,11 +26,13 @@ #include "simu.host.h" #include "modules/host/host.h" +#include "modules/host/mex.h" #include "modules/utils/utils.h" #include #include #include +#include #include "pwm.h" @@ -65,6 +67,9 @@ double simu_pos_x, simu_pos_y, simu_pos_a; uint32_t simu_counter_left, simu_counter_right, simu_counter_aux0; double simu_counter_left_th, simu_counter_right_th; +/** Use mex. */ +int simu_mex; + /** Initialise simulation. */ static void simu_init (void) @@ -72,9 +77,15 @@ simu_init (void) int argc; char **argv; host_get_program_arguments (&argc, &argv); + if (argc == 2 && strcmp (argv[0], "-m") == 0) + { + simu_mex = 1; + mex_node_connect (); + argc--; argv++; + } if (argc != 1) { - fprintf (stderr, "need model name as first argument\n"); + fprintf (stderr, "Syntax: asserv.host [-m] model\n"); exit (1); } simu_robot = models_get (argv[0]); @@ -146,7 +157,7 @@ simu_step (void) } else { - /* Thanks Thales. */ + /* Thanks Thalès. */ double left_diff = (simu_left_model.th - old_left_th) / simu_left_model.m.i_G * simu_robot->wheel_r; double right_diff = (simu_right_model.th - old_right_th) @@ -187,6 +198,27 @@ simu_step (void) simu_robot->footing * 1000); } +/** Send information to the other nodes. */ +void +simu_send (void) +{ + mex_msg_t *m; + /* Send position. */ + m = mex_msg_new (0xa0); + mex_msg_push (m, "hhl", (int16_t) simu_pos_x, (int16_t) simu_pos_y, + (int32_t) (1024.0 * simu_pos_a)); + mex_node_send (m); + /* Send PWM. */ + m = mex_msg_new (0xa1); + mex_msg_push (m, "hhh", pwm_left, pwm_right, pwm_aux0); + mex_node_send (m); + /* Send Arm position. */ + m = mex_msg_new (0xa8); + mex_msg_push (m, "l", (int32_t) (1024.0 * simu_aux0_model.th / + simu_aux0_model.m.i_G)); + mex_node_send (m); +} + /** Initialise the timer. */ void timer_init (void) @@ -198,7 +230,11 @@ timer_init (void) void timer_wait (void) { + if (simu_mex) + mex_node_wait_date (mex_node_date () + 4); simu_step (); + if (simu_mex) + simu_send (); } /** Read timer value. Used for performance analysis. */ diff --git a/host/inter/inter_node.py b/host/inter/inter_node.py index 44823eb7..14be6908 100644 --- a/host/inter/inter_node.py +++ b/host/inter/inter_node.py @@ -40,6 +40,8 @@ class InterNode (Inter): def __init__ (self): Inter.__init__ (self) self.node = Node () + self.node.register (0xa0, self.handle_asserv_0) + self.node.register (0xa8, self.handle_asserv_8) self.tk.createfilehandler (self.node, READABLE, self.read) self.step_after = None @@ -89,6 +91,17 @@ class InterNode (Inter): self.step_after = self.after (int (self.stepSizeScale.get () * self.TICK), self.step) + def handle_asserv_0 (self, msg): + x, y, a = msg.pop ('hhl') + self.tableview.robot.pos = (x, y) + self.tableview.robot.angle = float (a) / 1024 + self.update (self.tableview.robot) + + def handle_asserv_8 (self, msg): + a, = msg.pop ('l') + self.actuatorview.arm.angle = float (a) / 1024 + self.update (self.actuatorview.arm) + if __name__ == '__main__': import mex.hub import mex.forked -- cgit v1.2.3