1 # Test for Razor 9DOF IMU
\r
3 # This script needs VPhyton, pyserial and pywin modules
\r
5 # First Install Python 2.6.4
\r
6 # Install pywin from http://sourceforge.net/projects/pywin32/
\r
7 # Install pyserial from http://sourceforge.net/projects/pyserial/files/
\r
8 # Install Vphyton from http://vpython.org/contents/download_windows.html
\r
10 from visual import *
\r
17 from time import time
\r
19 #ev = scene.waitfor('click keydown')
\r
21 grad2rad = 3.141592/180.0
\r
23 # Check your COM port and baud rate
\r
24 ser = serial.Serial(port='/dev/ttyUSB0',baudrate=57600, timeout=1)
\r
27 scene=display(title="9DOF Razor IMU test")
\r
28 scene.range=(1.2,1.2,1.2)
\r
29 #scene.forward = (0,-1,-0.25)
\r
30 scene.forward = (1,0,-0.25)
\r
33 # Second scene (Roll, Pitch, Yaw)
\r
34 scene2 = display(title='9DOF Razor IMU test',x=0, y=0, width=500, height=200,center=(0,0,0), background=(0,0,0))
\r
35 scene2.range=(1,1,1)
\r
41 cil_roll = cylinder(pos=(-0.4,0,0),axis=(0.2,0,0),radius=0.01,color=color.red)
\r
42 cil_roll2 = cylinder(pos=(-0.4,0,0),axis=(-0.2,0,0),radius=0.01,color=color.red)
\r
43 cil_pitch = cylinder(pos=(0.1,0,0),axis=(0.2,0,0),radius=0.01,color=color.green)
\r
44 cil_pitch2 = cylinder(pos=(0.1,0,0),axis=(-0.2,0,0),radius=0.01,color=color.green)
\r
45 #cil_course = cylinder(pos=(0.6,0,0),axis=(0.2,0,0),radius=0.01,color=color.blue)
\r
46 #cil_course2 = cylinder(pos=(0.6,0,0),axis=(-0.2,0,0),radius=0.01,color=color.blue)
\r
47 arrow_course = arrow(pos=(0.6,0,0),color=color.cyan,axis=(-0.2,0,0), shaftwidth=0.02, fixedwidth=1)
\r
49 #Roll,Pitch,Yaw labels
\r
50 label(pos=(-0.4,0.3,0),text="Roll",box=0,opacity=0)
\r
51 label(pos=(0.1,0.3,0),text="Pitch",box=0,opacity=0)
\r
52 label(pos=(0.55,0.3,0),text="Yaw",box=0,opacity=0)
\r
53 label(pos=(0.6,0.22,0),text="N",box=0,opacity=0,color=color.yellow)
\r
54 label(pos=(0.6,-0.22,0),text="S",box=0,opacity=0,color=color.yellow)
\r
55 label(pos=(0.38,0,0),text="W",box=0,opacity=0,color=color.yellow)
\r
56 label(pos=(0.82,0,0),text="E",box=0,opacity=0,color=color.yellow)
\r
57 label(pos=(0.75,0.15,0),height=7,text="NE",box=0,color=color.yellow)
\r
58 label(pos=(0.45,0.15,0),height=7,text="NW",box=0,color=color.yellow)
\r
59 label(pos=(0.75,-0.15,0),height=7,text="SE",box=0,color=color.yellow)
\r
60 label(pos=(0.45,-0.15,0),height=7,text="SW",box=0,color=color.yellow)
\r
62 L1 = label(pos=(-0.4,0.22,0),text="-",box=0,opacity=0)
\r
63 L2 = label(pos=(0.1,0.22,0),text="-",box=0,opacity=0)
\r
64 L3 = label(pos=(0.7,0.3,0),text="-",box=0,opacity=0)
\r
66 # Main scene objects
\r
68 # Reference axis (x,y,z)
\r
69 arrow(color=color.green,axis=(1,0,0), shaftwidth=0.02, fixedwidth=1)
\r
70 arrow(color=color.green,axis=(0,-1,0), shaftwidth=0.02 , fixedwidth=1)
\r
71 arrow(color=color.green,axis=(0,0,-1), shaftwidth=0.02, fixedwidth=1)
\r
73 label(pos=(0,0,0.8),text="9DOF Razor IMU test",box=0,opacity=0)
\r
74 label(pos=(1,0,0),text="X",box=0,opacity=0)
\r
75 label(pos=(0,-1,0),text="Y",box=0,opacity=0)
\r
76 label(pos=(0,0,-1),text="Z",box=0,opacity=0)
\r
78 platform = box(length=1, height=0.05, width=1, color=color.red)
\r
79 p_line = box(length=1,height=0.08,width=0.1,color=color.yellow)
\r
80 plat_arrow = arrow(color=color.green,axis=(1,0,0), shaftwidth=0.06, fixedwidth=1)
\r
82 INT = "([-+]?[0-9][0-9]*)"
\r
83 FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
\r
85 f = open("Serial"+str(time())+".txt", 'w')
\r
93 line = ser.readline()
\r
94 #line = line.replace("!ANG:","") # Delete "!ANG:"
\r
95 m = re.match(".*IMU received %s %s %s"%(INT, INT, INT), line)
\r
97 roll, pitch, yaw = map(lambda x: float(x)*(math.pi/18000), m.groups())
\r
98 print roll, pitch, yaw
\r
100 m = re.match("%s \| gyro %s %s %s \| accel %s %s %s \| magnet %s %s %s \| angles %s %s %s"%(
\r
101 INT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT, FLOAT), line)
\r
104 roll, pitch, yaw = map(lambda x: float(x), m.groups()[10:13])
\r
105 #print roll, pitch, yaw
\r
106 mx, my, mz = map(lambda x: float(x), m.groups()[7:10])
\r
107 norm = mx * mx + my * my + mz * mz
\r
111 print ("%4.4f %4.4f %4.4f"%(mx * 1000, my * 1000, mz * 1000))
\r
118 t = time() - t_start
\r
120 #print "XXX", cpt/t
\r
123 axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch))
\r
124 up=(sin(roll)*sin(yaw)+cos(roll)*sin(pitch)*cos(yaw),sin(roll)*cos(yaw)-cos(roll)*sin(pitch)*sin(yaw),-cos(roll)*cos(pitch))
\r
127 platform.length=1.0
\r
128 platform.width=0.65
\r
129 plat_arrow.axis=axis
\r
131 plat_arrow.length=0.8
\r
134 cil_roll.axis=(0.2*cos(roll),0.2*sin(roll),0)
\r
135 cil_roll2.axis=(-0.2*cos(roll),-0.2*sin(roll),0)
\r
136 cil_pitch.axis=(0.2*cos(pitch),0.2*sin(pitch),0)
\r
137 cil_pitch2.axis=(-0.2*cos(pitch),-0.2*sin(pitch),0)
\r
138 arrow_course.axis=(0.2*sin(yaw),0.2*cos(yaw),0)
\r
139 L1.text = str(roll)
\r
140 L2.text = str(pitch)
\r