From 78085e4d2c0b4c7c2bc762929311b3ce6572375f Mon Sep 17 00:00:00 2001
From: Olivier Matz <zer0@droids-corp.org>
Date: Tue, 25 Nov 2014 20:44:56 +0100
Subject: [PATCH] IMU_Razor9DOF.py: adapt to new log format

---
 imuboard/IMU_Razor9DOF.py | 40 +++++++++++----------------------------
 1 file changed, 11 insertions(+), 29 deletions(-)

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