From 145b19e73ce5751b365bc53769189addeecee81b Mon Sep 17 00:00:00 2001
From: zer0 <zer0@carbon.local>
Date: Thu, 21 Jan 2010 00:31:54 +0100
Subject: [PATCH] robotsim first test

---
 projects/microb2010/tests/hostsim/cs.c       |  3 ++
 projects/microb2010/tests/hostsim/display.py | 50 ++++++++++++++++++++
 projects/microb2010/tests/hostsim/main.c     |  8 ++--
 projects/microb2010/tests/hostsim/robotsim.c | 28 ++++++++++-
 projects/microb2010/tests/hostsim/robotsim.h |  2 +
 5 files changed, 87 insertions(+), 4 deletions(-)
 create mode 100644 projects/microb2010/tests/hostsim/display.py

diff --git a/projects/microb2010/tests/hostsim/cs.c b/projects/microb2010/tests/hostsim/cs.c
index 7fb39f5..cbb65d6 100644
--- 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++;
+
+	if ((cpt & 8) == 0)
+		robotsim_dump();
 	//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
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
--- a/projects/microb2010/tests/hostsim/main.c
+++ b/projects/microb2010/tests/hostsim/main.c
@@ -69,6 +69,7 @@ int main(void)
 
 #ifdef HOST_VERSION
 	hostsim_init();
+	robotsim_init();
 #endif
 	time_init(TIME_PRIO);
 
@@ -81,9 +82,10 @@ int main(void)
 	mainboard.flags = DO_ENCODERS | DO_RS |
 		DO_POS | DO_POWER | DO_BD | DO_CS;
 
-	trajectory_d_rel(&mainboard.traj, 100);
-
-	time_wait_ms(1000);
+	trajectory_d_rel(&mainboard.traj, 1000);
+	time_wait_ms(2000);
+	trajectory_goto_xy_abs(&mainboard.traj, 1500, 2000);
+	time_wait_ms(2000);
 	return 0;
 }
 
diff --git a/projects/microb2010/tests/hostsim/robotsim.c b/projects/microb2010/tests/hostsim/robotsim.c
index 6f4dbbf..7ed49f4 100644
--- 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 <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.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 int fd;
+
 /* */
-#define FILTER  98
+#define FILTER  97
 #define FILTER2 (100-FILTER)
 
 /* must be called periodically */
@@ -62,6 +67,18 @@ void robotsim_update(void)
 	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);
@@ -79,3 +96,12 @@ int32_t robotsim_encoder_get(void *arg)
 		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
--- 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);
+int robotsim_init(void);
+void robotsim_dump(void);
-- 
2.39.5