IMU_Razor9DOF.py: adapt to new log format

diff --git a/imuboard/IMU_Razor9DOF.py b/imuboard/IMU_Razor9DOF.py
index 91351ae14e4f807983d942b0066b01439ffbd922..97ae27f5d0c6774b30b8f742dd15d52ce5401931 100644 (file)
--- a/imuboard/IMU_Razor9DOF.py
+++ b/imuboard/IMU_Razor9DOF.py
@@ -12,6 +12,7 @@ import serial
 import string\r
 import math\r
 import sys\r
+import re\r
 \r
 from time import time\r
 \r
@@ -78,6 +79,7 @@ platform = box(length=1, height=0.05, width=1, color=color.red)
 p_line = box(length=1,height=0.08,width=0.1,color=color.yellow)\r
 plat_arrow = arrow(color=color.green,axis=(1,0,0), shaftwidth=0.06, fixedwidth=1)\r
 \r
+INT = "([-+]?[0-9][0-9]*)"\r
 \r
 f = open("Serial"+str(time())+".txt", 'w')\r
 \r
@@ -88,33 +90,13 @@ cpt = 0
 t_start = time()\r
 while 1:\r
     line = ser.readline()\r
-    line = line.replace("!ANG:","")   # Delete "!ANG:"\r
-    #print line\r
-    f.write(line)                     # Write to the output log file\r
-    words = string.split(line,"\t")    # Fields split\r
-    #print words\r
-    print words\r
-    if len(words) != 3:\r
-        continue\r
-\r
-    """\r
-    a = float(words[0])#*grad2rad\r
-    b = float(words[1])#*grad2rad\r
-    c = float(words[2])#*grad2rad\r
-    d = float(words[3])#*grad2rad\r
-\r
-    platform.axis = (1, 1, 1)\r
-    #print a, b, c, d\r
-    platform.rotate(angle=a, axis = (b, c, d))\r
-    continue\r
-    """\r
-    try:\r
-        roll = float(words[0])#*grad2rad\r
-        pitch = float(words[1])#*grad2rad\r
-        yaw = float(words[2])#*grad2rad\r
-    except:\r
-        print "Invalid line"\r
+    #line = line.replace("!ANG:","")   # Delete "!ANG:"\r
+    m = re.match(".*IMU received %s %s %s"%(INT, INT, INT), line)\r
+    if m == None:\r
+        print line\r
         continue\r
+    roll, pitch, yaw = map(lambda x: float(x)*(math.pi/18000), m.groups())\r
+    print roll, pitch, yaw\r
 \r
     cpt += 1\r
     if cpt > 10:\r
@@ -139,8 +121,8 @@ while 1:
     cil_pitch.axis=(0.2*cos(pitch),0.2*sin(pitch),0)\r
     cil_pitch2.axis=(-0.2*cos(pitch),-0.2*sin(pitch),0)\r
     arrow_course.axis=(0.2*sin(yaw),0.2*cos(yaw),0)\r
-    L1.text = str(float(words[0]))\r
-    L2.text = str(float(words[1]))\r
-    L3.text = str(float(words[2]))\r
+    L1.text = str(roll)\r
+    L2.text = str(pitch)\r
+    L3.text = str(yaw)\r
 ser.close()\r
 f.close()\r