imuboard/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 import re\r
  16 \r
  17 from time import time\r
  18 \r
  19 #ev = scene.waitfor('click keydown')\r
  20 \r
  21 grad2rad = 3.141592/180.0\r
  22 \r
  23 # Check your COM port and baud rate\r
  24 ser = serial.Serial(port='/dev/ttyUSB0',baudrate=57600, timeout=1)\r
  25 \r
  26 # Main scene\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
  31 scene.up=(0,0,1)\r
  32 \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
  36 scene.width=500\r
  37 scene.y=200\r
  38 \r
  39 scene2.select()\r
  40 #Roll, Pitch, Yaw\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
  48 \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
  61 \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
  65 \r
  66 # Main scene objects\r
  67 scene.select()\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
  72 # labels\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
  77 # IMU object\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
  81 \r
  82 INT = "([-+]?[0-9][0-9]*)"\r
  83 FLOAT = "([-+]?[0-9]*\.?[0-9]+)"\r
  84 \r
  85 f = open("Serial"+str(time())+".txt", 'w')\r
  86 \r
  87 roll=0\r
  88 pitch=0\r
  89 yaw=0\r
  90 cpt = 0\r
  91 t_start = time()\r
  92 while 1:\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
  96     if m:\r
  97         roll, pitch, yaw = map(lambda x: float(x)*(math.pi/18000), m.groups())\r
  98         print roll, pitch, yaw\r
  99     if not m:\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
 102     if m:\r
 103         #print m.groups()\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
 108         mx /= norm\r
 109         my /= norm\r
 110         mz /= norm\r
 111         print ("%4.4f %4.4f %4.4f"%(mx * 1000, my * 1000, mz * 1000))\r
 112     if not m:\r
 113         print line\r
 114         continue\r
 115 \r
 116     cpt += 1\r
 117     if cpt > 10:\r
 118         t = time() - t_start\r
 119         t_start = time()\r
 120         #print "XXX", cpt/t\r
 121         cpt = 0\r
 122 \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
 125     platform.axis=axis\r
 126     platform.up=up\r
 127     platform.length=1.0\r
 128     platform.width=0.65\r
 129     plat_arrow.axis=axis\r
 130     plat_arrow.up=up\r
 131     plat_arrow.length=0.8\r
 132     p_line.axis=axis\r
 133     p_line.up=up\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
 141     L3.text = str(yaw)\r
 142 ser.close()\r
 143 f.close()\r