import sd-reader_source_20120612
[protos/imu.git] / IMU_Razor9DOF.py
1 # Test for Razor 9DOF IMU\r
2 # Jose Julio @2009\r
3 # This script needs VPhyton, pyserial and pywin modules\r
4 \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
9 \r
10 from visual import *\r
11 import serial\r
12 import string\r
13 import math\r
14 import sys\r
15 \r
16 from time import time\r
17 \r
18 #ev = scene.waitfor('click keydown')\r
19 \r
20 grad2rad = 3.141592/180.0\r
21 \r
22 # Check your COM port and baud rate\r
23 ser = serial.Serial(port='/dev/ttyUSB0',baudrate=57600, timeout=1)\r
24 \r
25 # Main scene\r
26 scene=display(title="9DOF Razor IMU test")\r
27 scene.range=(1.2,1.2,1.2)\r
28 #scene.forward = (0,-1,-0.25)\r
29 scene.forward = (1,0,-0.25)\r
30 scene.up=(0,0,1)\r
31 \r
32 # Second scene (Roll, Pitch, Yaw)\r
33 scene2 = display(title='9DOF Razor IMU test',x=0, y=0, width=500, height=200,center=(0,0,0), background=(0,0,0))\r
34 scene2.range=(1,1,1)\r
35 scene.width=500\r
36 scene.y=200\r
37 \r
38 scene2.select()\r
39 #Roll, Pitch, Yaw\r
40 cil_roll = cylinder(pos=(-0.4,0,0),axis=(0.2,0,0),radius=0.01,color=color.red)\r
41 cil_roll2 = cylinder(pos=(-0.4,0,0),axis=(-0.2,0,0),radius=0.01,color=color.red)\r
42 cil_pitch = cylinder(pos=(0.1,0,0),axis=(0.2,0,0),radius=0.01,color=color.green)\r
43 cil_pitch2 = cylinder(pos=(0.1,0,0),axis=(-0.2,0,0),radius=0.01,color=color.green)\r
44 #cil_course = cylinder(pos=(0.6,0,0),axis=(0.2,0,0),radius=0.01,color=color.blue)\r
45 #cil_course2 = cylinder(pos=(0.6,0,0),axis=(-0.2,0,0),radius=0.01,color=color.blue)\r
46 arrow_course = arrow(pos=(0.6,0,0),color=color.cyan,axis=(-0.2,0,0), shaftwidth=0.02, fixedwidth=1)\r
47 \r
48 #Roll,Pitch,Yaw labels\r
49 label(pos=(-0.4,0.3,0),text="Roll",box=0,opacity=0)\r
50 label(pos=(0.1,0.3,0),text="Pitch",box=0,opacity=0)\r
51 label(pos=(0.55,0.3,0),text="Yaw",box=0,opacity=0)\r
52 label(pos=(0.6,0.22,0),text="N",box=0,opacity=0,color=color.yellow)\r
53 label(pos=(0.6,-0.22,0),text="S",box=0,opacity=0,color=color.yellow)\r
54 label(pos=(0.38,0,0),text="W",box=0,opacity=0,color=color.yellow)\r
55 label(pos=(0.82,0,0),text="E",box=0,opacity=0,color=color.yellow)\r
56 label(pos=(0.75,0.15,0),height=7,text="NE",box=0,color=color.yellow)\r
57 label(pos=(0.45,0.15,0),height=7,text="NW",box=0,color=color.yellow)\r
58 label(pos=(0.75,-0.15,0),height=7,text="SE",box=0,color=color.yellow)\r
59 label(pos=(0.45,-0.15,0),height=7,text="SW",box=0,color=color.yellow)\r
60 \r
61 L1 = label(pos=(-0.4,0.22,0),text="-",box=0,opacity=0)\r
62 L2 = label(pos=(0.1,0.22,0),text="-",box=0,opacity=0)\r
63 L3 = label(pos=(0.7,0.3,0),text="-",box=0,opacity=0)\r
64 \r
65 # Main scene objects\r
66 scene.select()\r
67 # Reference axis (x,y,z)\r
68 arrow(color=color.green,axis=(1,0,0), shaftwidth=0.02, fixedwidth=1)\r
69 arrow(color=color.green,axis=(0,-1,0), shaftwidth=0.02 , fixedwidth=1)\r
70 arrow(color=color.green,axis=(0,0,-1), shaftwidth=0.02, fixedwidth=1)\r
71 # labels\r
72 label(pos=(0,0,0.8),text="9DOF Razor IMU test",box=0,opacity=0)\r
73 label(pos=(1,0,0),text="X",box=0,opacity=0)\r
74 label(pos=(0,-1,0),text="Y",box=0,opacity=0)\r
75 label(pos=(0,0,-1),text="Z",box=0,opacity=0)\r
76 # IMU object\r
77 platform = box(length=1, height=0.05, width=1, color=color.red)\r
78 p_line = box(length=1,height=0.08,width=0.1,color=color.yellow)\r
79 plat_arrow = arrow(color=color.green,axis=(1,0,0), shaftwidth=0.06, fixedwidth=1)\r
80 \r
81 \r
82 f = open("Serial"+str(time())+".txt", 'w')\r
83 \r
84 roll=0\r
85 pitch=0\r
86 yaw=0\r
87 cpt = 0\r
88 t_start = time()\r
89 while 1:\r
90     line = ser.readline()\r
91     line = line.replace("!ANG:","")   # Delete "!ANG:"\r
92     #print line\r
93     f.write(line)                     # Write to the output log file\r
94     words = string.split(line,"\t")    # Fields split\r
95     #print words\r
96     print words\r
97     if len(words) != 3:\r
98         continue\r
99 \r
100     """\r
101     a = float(words[0])#*grad2rad\r
102     b = float(words[1])#*grad2rad\r
103     c = float(words[2])#*grad2rad\r
104     d = float(words[3])#*grad2rad\r
105 \r
106     platform.axis = (1, 1, 1)\r
107     #print a, b, c, d\r
108     platform.rotate(angle=a, axis = (b, c, d))\r
109     continue\r
110     """\r
111     try:\r
112         roll = float(words[0])#*grad2rad\r
113         pitch = float(words[1])#*grad2rad\r
114         yaw = float(words[2])#*grad2rad\r
115     except:\r
116         print "Invalid line"\r
117         continue\r
118 \r
119     cpt += 1\r
120     if cpt > 10:\r
121         t = time() - t_start\r
122         t_start = time()\r
123         print "XXX", cpt/t\r
124         cpt = 0\r
125 \r
126     axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch))\r
127     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
128     platform.axis=axis\r
129     platform.up=up\r
130     platform.length=1.0\r
131     platform.width=0.65\r
132     plat_arrow.axis=axis\r
133     plat_arrow.up=up\r
134     plat_arrow.length=0.8\r
135     p_line.axis=axis\r
136     p_line.up=up\r
137     cil_roll.axis=(0.2*cos(roll),0.2*sin(roll),0)\r
138     cil_roll2.axis=(-0.2*cos(roll),-0.2*sin(roll),0)\r
139     cil_pitch.axis=(0.2*cos(pitch),0.2*sin(pitch),0)\r
140     cil_pitch2.axis=(-0.2*cos(pitch),-0.2*sin(pitch),0)\r
141     arrow_course.axis=(0.2*sin(yaw),0.2*cos(yaw),0)\r
142     L1.text = str(float(words[0]))\r
143     L2.text = str(float(words[1]))\r
144     L3.text = str(float(words[2]))\r
145 ser.close()\r
146 f.close()\r