robotsim first test

diff --git a/projects/microb2010/tests/hostsim/cs.c b/projects/microb2010/tests/hostsim/cs.c
index 7fb39f5..cbb65d6 100644 (file)
--- a/projects/microb2010/tests/hostsim/cs.c
+++ b/projects/microb2010/tests/hostsim/cs.c
@@ -113,6 +113,9 @@ static void do_cs(void *dummy)
                BRAKE_ON();
 #endif
        cpt++;
                BRAKE_ON();
 #endif
        cpt++;

+
+       if ((cpt & 8) == 0)
+               robotsim_dump();

        //dump_cs("distance", &mainboard.distance.cs);
        //dump_cs("angle", &mainboard.angle.cs);
 }
        //dump_cs("distance", &mainboard.distance.cs);
        //dump_cs("angle", &mainboard.angle.cs);
 }

diff --git a/projects/microb2010/tests/hostsim/display.py b/projects/microb2010/tests/hostsim/display.py
new file mode 100644 (file)
index 0000000..412f936
--- /dev/null
+++ b/projects/microb2010/tests/hostsim/display.py
@@ -0,0 +1,50 @@
+import math, sys, time, os
+from visual import *
+
+AREA_X = 3000.
+AREA_Y = 2100.
+
+area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ]
+areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area)
+area_box = box(size=areasize, color=(0.0, 1.0, 0.0))
+
+scene.autoscale=0
+
+robot = box(pos = (0, 0, 150),
+            size = (300,300,300),
+            color = (1, 0, 0) )
+
+def set_robot(x, y, a):
+    global robot
+    robot.pos = (x - AREA_X/2, y - AREA_Y/2, 150)
+    robot.axis = (math.cos(a*math.pi/180) * 300,
+                  math.sin(a*math.pi/180) * 300,
+                  0)
+
+while True:
+    try:
+        os.mkfifo("/tmp/.robot")
+    except:
+        pass
+    while True:
+        f = open("/tmp/.robot")
+        while True:
+            l = f.readline()
+            if l == "":
+                break
+            x,y,a = map(lambda x:int(x), l[:-1].split(" "))
+            set_robot(x,y,a)
+        f.close()
+
+    """
+    k = scene.kb.getkey()
+    x,y,z = scene.center
+    if k == "left":
+        scene.center = x-10,y,z
+    elif k == "right":
+        scene.center = x+10,y,z
+    elif k == "up":
+        scene.center = x,y+10,z
+    elif k == "down":
+        scene.center = x,y-10,z
+    """

diff --git a/projects/microb2010/tests/hostsim/main.c b/projects/microb2010/tests/hostsim/main.c
index 9f5a15f..ec70479 100644 (file)
--- a/projects/microb2010/tests/hostsim/main.c
+++ b/projects/microb2010/tests/hostsim/main.c
@@ -69,6 +69,7 @@ int main(void)
 \r
 #ifdef HOST_VERSION\r
        hostsim_init();\r
 \r
 #ifdef HOST_VERSION\r
        hostsim_init();\r

+       robotsim_init();\r

 #endif\r
        time_init(TIME_PRIO);\r
 \r
 #endif\r
        time_init(TIME_PRIO);\r
 \r


@@ -81,9 +82,10 @@ int main(void)
        mainboard.flags = DO_ENCODERS | DO_RS |\r
                DO_POS | DO_POWER | DO_BD | DO_CS;\r
 \r
        mainboard.flags = DO_ENCODERS | DO_RS |\r
                DO_POS | DO_POWER | DO_BD | DO_CS;\r
 \r

-       trajectory_d_rel(&mainboard.traj, 100);\r
-\r
-       time_wait_ms(1000);\r
+       trajectory_d_rel(&mainboard.traj, 1000);\r
+       time_wait_ms(2000);\r
+       trajectory_goto_xy_abs(&mainboard.traj, 1500, 2000);\r
+       time_wait_ms(2000);\r

        return 0;\r
 }\r
 \r
        return 0;\r
 }\r
 \r

diff --git a/projects/microb2010/tests/hostsim/robotsim.c b/projects/microb2010/tests/hostsim/robotsim.c
index 6f4dbbf..7ed49f4 100644 (file)
--- a/projects/microb2010/tests/hostsim/robotsim.c
+++ b/projects/microb2010/tests/hostsim/robotsim.c
@@ -22,6 +22,9 @@
 #include <stdio.h>
 #include <string.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <string.h>
 #include <stdint.h>

+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>

 
 #include <aversive.h>
 #include <aversive/error.h>
 
 #include <aversive.h>
 #include <aversive/error.h>


@@ -46,8 +49,10 @@
 static int32_t l_pwm, r_pwm;
 static int32_t l_enc, r_enc;
 
 static int32_t l_pwm, r_pwm;
 static int32_t l_enc, r_enc;
 

+static int fd;
+

 /* */
 /* */

-#define FILTER  98
+#define FILTER  97

 #define FILTER2 (100-FILTER)
 
 /* must be called periodically */
 #define FILTER2 (100-FILTER)
 
 /* must be called periodically */


@@ -62,6 +67,18 @@ void robotsim_update(void)
        r_enc += (r_speed / 1000);
 }
 
        r_enc += (r_speed / 1000);
 }
 

+void robotsim_dump(void)
+{
+       char buf[BUFSIZ];
+       int len;
+
+       len =snprintf(buf, sizeof(buf), "%d %d %d\n",
+                     position_get_x_s16(&mainboard.pos),
+                     position_get_y_s16(&mainboard.pos),
+                     position_get_a_deg_s16(&mainboard.pos));
+       write(fd, buf, len);
+}
+

 void robotsim_pwm(void *arg, int32_t val)
 {
        //      printf("%p, %d\n", arg, val);
 void robotsim_pwm(void *arg, int32_t val)
 {
        //      printf("%p, %d\n", arg, val);


@@ -79,3 +96,12 @@ int32_t robotsim_encoder_get(void *arg)
                return r_enc;
        return 0;
 }
                return r_enc;
        return 0;
 }

+
+int robotsim_init(void)
+{
+       mkfifo("/tmp/.robot", 0600);
+       fd = open("/tmp/.robot", O_WRONLY, 0);
+       if (fd < 0)
+               return -1;
+       return 0;
+}

diff --git a/projects/microb2010/tests/hostsim/robotsim.h b/projects/microb2010/tests/hostsim/robotsim.h
index 56415f9..8777de8 100644 (file)
--- a/projects/microb2010/tests/hostsim/robotsim.h
+++ b/projects/microb2010/tests/hostsim/robotsim.h
@@ -22,3 +22,5 @@
 void robotsim_update(void);
 void robotsim_pwm(void *arg, int32_t val);
 int32_t robotsim_encoder_get(void *arg);
 void robotsim_update(void);
 void robotsim_pwm(void *arg, int32_t val);
 int32_t robotsim_encoder_get(void *arg);

+int robotsim_init(void);
+void robotsim_dump(void);