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