An fully working IMU-Filter and Sensor drivers for the 10DOF-Board over I2C. All in one simple class. Include, calibrate sensors, call read, get angles. (3D Visualisation code for Python also included) Sensors: L3G4200D, ADXL345, HMC5883, BMP085
3D_IMU_10DOF.py.txt@2:ca8fd72a8f2b, 2013-08-27 (annotated)
- Committer:
- maetugr
- Date:
- Tue Aug 27 22:14:26 2013 +0000
- Revision:
- 2:ca8fd72a8f2b
Python code added
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 2:ca8fd72a8f2b | 1 | # Test for IMU_10DOF |
maetugr | 2:ca8fd72a8f2b | 2 | # MaEtUgR @2013 |
maetugr | 2:ca8fd72a8f2b | 3 | # This script needs VPhyton, pyserial and pywin modules |
maetugr | 2:ca8fd72a8f2b | 4 | |
maetugr | 2:ca8fd72a8f2b | 5 | # First Install Python 2.6.4 |
maetugr | 2:ca8fd72a8f2b | 6 | # Install pywin from http://sourceforge.net/projects/pywin32/ |
maetugr | 2:ca8fd72a8f2b | 7 | # Install pyserial from http://sourceforge.net/projects/pyserial/files/ |
maetugr | 2:ca8fd72a8f2b | 8 | # Install Vphyton from http://vpython.org/contents/download_windows.html |
maetugr | 2:ca8fd72a8f2b | 9 | |
maetugr | 2:ca8fd72a8f2b | 10 | from visual import * |
maetugr | 2:ca8fd72a8f2b | 11 | import serial |
maetugr | 2:ca8fd72a8f2b | 12 | import string |
maetugr | 2:ca8fd72a8f2b | 13 | import math |
maetugr | 2:ca8fd72a8f2b | 14 | |
maetugr | 2:ca8fd72a8f2b | 15 | from time import time |
maetugr | 2:ca8fd72a8f2b | 16 | |
maetugr | 2:ca8fd72a8f2b | 17 | grad2rad = 3.141592/180.0 |
maetugr | 2:ca8fd72a8f2b | 18 | |
maetugr | 2:ca8fd72a8f2b | 19 | # Check your COM port and baud rate |
maetugr | 2:ca8fd72a8f2b | 20 | ser = serial.Serial(port='COM3',baudrate=921600, timeout=1) |
maetugr | 2:ca8fd72a8f2b | 21 | |
maetugr | 2:ca8fd72a8f2b | 22 | # Main scene |
maetugr | 2:ca8fd72a8f2b | 23 | scene=display(title="3D Visualisation") |
maetugr | 2:ca8fd72a8f2b | 24 | scene.range=(1.2,1.2,1.2) |
maetugr | 2:ca8fd72a8f2b | 25 | #scene.forward = (0,-1,-0.25) |
maetugr | 2:ca8fd72a8f2b | 26 | scene.forward = (1,0,-0.25) |
maetugr | 2:ca8fd72a8f2b | 27 | scene.up=(0,0,1) |
maetugr | 2:ca8fd72a8f2b | 28 | |
maetugr | 2:ca8fd72a8f2b | 29 | # Second scene (Roll, Pitch, Yaw) |
maetugr | 2:ca8fd72a8f2b | 30 | scene2 = display(title='2D Visualisation',x=0, y=0, width=500, height=200,center=(0,0,0), background=(0,0,0)) |
maetugr | 2:ca8fd72a8f2b | 31 | scene2.range=(1,1,1) |
maetugr | 2:ca8fd72a8f2b | 32 | scene.width=500 |
maetugr | 2:ca8fd72a8f2b | 33 | scene.y=200 |
maetugr | 2:ca8fd72a8f2b | 34 | |
maetugr | 2:ca8fd72a8f2b | 35 | scene2.select() |
maetugr | 2:ca8fd72a8f2b | 36 | #Roll, Pitch, Yaw |
maetugr | 2:ca8fd72a8f2b | 37 | cil_roll = cylinder(pos=(-0.4,0,0),axis=(0.2,0,0),radius=0.01,color=color.red) |
maetugr | 2:ca8fd72a8f2b | 38 | cil_roll2 = cylinder(pos=(-0.4,0,0),axis=(-0.2,0,0),radius=0.01,color=color.red) |
maetugr | 2:ca8fd72a8f2b | 39 | cil_pitch = cylinder(pos=(0.1,0,0),axis=(0.2,0,0),radius=0.01,color=color.green) |
maetugr | 2:ca8fd72a8f2b | 40 | cil_pitch2 = cylinder(pos=(0.1,0,0),axis=(-0.2,0,0),radius=0.01,color=color.green) |
maetugr | 2:ca8fd72a8f2b | 41 | #cil_course = cylinder(pos=(0.6,0,0),axis=(0.2,0,0),radius=0.01,color=color.blue) |
maetugr | 2:ca8fd72a8f2b | 42 | #cil_course2 = cylinder(pos=(0.6,0,0),axis=(-0.2,0,0),radius=0.01,color=color.blue) |
maetugr | 2:ca8fd72a8f2b | 43 | arrow_course = arrow(pos=(0.6,0,0),color=color.cyan,axis=(-0.2,0,0), shaftwidth=0.02, fixedwidth=1) |
maetugr | 2:ca8fd72a8f2b | 44 | |
maetugr | 2:ca8fd72a8f2b | 45 | #Roll,Pitch,Yaw labels |
maetugr | 2:ca8fd72a8f2b | 46 | label(pos=(-0.4,0.3,0),text="Roll",box=0,opacity=0) |
maetugr | 2:ca8fd72a8f2b | 47 | label(pos=(0.1,0.3,0),text="Pitch",box=0,opacity=0) |
maetugr | 2:ca8fd72a8f2b | 48 | label(pos=(0.55,0.3,0),text="Yaw",box=0,opacity=0) |
maetugr | 2:ca8fd72a8f2b | 49 | label(pos=(0.6,0.22,0),text="N",box=0,opacity=0,color=color.yellow) |
maetugr | 2:ca8fd72a8f2b | 50 | label(pos=(0.6,-0.22,0),text="S",box=0,opacity=0,color=color.yellow) |
maetugr | 2:ca8fd72a8f2b | 51 | label(pos=(0.38,0,0),text="W",box=0,opacity=0,color=color.yellow) |
maetugr | 2:ca8fd72a8f2b | 52 | label(pos=(0.82,0,0),text="E",box=0,opacity=0,color=color.yellow) |
maetugr | 2:ca8fd72a8f2b | 53 | label(pos=(0.75,0.15,0),height=7,text="NE",box=0,color=color.yellow) |
maetugr | 2:ca8fd72a8f2b | 54 | label(pos=(0.45,0.15,0),height=7,text="NW",box=0,color=color.yellow) |
maetugr | 2:ca8fd72a8f2b | 55 | label(pos=(0.75,-0.15,0),height=7,text="SE",box=0,color=color.yellow) |
maetugr | 2:ca8fd72a8f2b | 56 | label(pos=(0.45,-0.15,0),height=7,text="SW",box=0,color=color.yellow) |
maetugr | 2:ca8fd72a8f2b | 57 | |
maetugr | 2:ca8fd72a8f2b | 58 | L1 = label(pos=(-0.4,0.22,0),text="-",box=0,opacity=0) |
maetugr | 2:ca8fd72a8f2b | 59 | L2 = label(pos=(0.1,0.22,0),text="-",box=0,opacity=0) |
maetugr | 2:ca8fd72a8f2b | 60 | L3 = label(pos=(0.7,0.3,0),text="-",box=0,opacity=0) |
maetugr | 2:ca8fd72a8f2b | 61 | |
maetugr | 2:ca8fd72a8f2b | 62 | # Main scene objects |
maetugr | 2:ca8fd72a8f2b | 63 | scene.select() |
maetugr | 2:ca8fd72a8f2b | 64 | # Reference axis (x,y,z) |
maetugr | 2:ca8fd72a8f2b | 65 | arrow(color=color.green,axis=(1,0,0), shaftwidth=0.02, fixedwidth=1) |
maetugr | 2:ca8fd72a8f2b | 66 | arrow(color=color.green,axis=(0,-1,0), shaftwidth=0.02 , fixedwidth=1) |
maetugr | 2:ca8fd72a8f2b | 67 | arrow(color=color.green,axis=(0,0,-1), shaftwidth=0.02, fixedwidth=1) |
maetugr | 2:ca8fd72a8f2b | 68 | # labels |
maetugr | 2:ca8fd72a8f2b | 69 | label(pos=(0,0,0.8),text="MaEtUgR's IMU test",box=0,opacity=0) |
maetugr | 2:ca8fd72a8f2b | 70 | label(pos=(1,0,0),text="X",box=0,opacity=0) |
maetugr | 2:ca8fd72a8f2b | 71 | label(pos=(0,-1,0),text="Y",box=0,opacity=0) |
maetugr | 2:ca8fd72a8f2b | 72 | label(pos=(0,0,-1),text="Z",box=0,opacity=0) |
maetugr | 2:ca8fd72a8f2b | 73 | # IMU object |
maetugr | 2:ca8fd72a8f2b | 74 | platform = box(length=1, height=0.05, width=1, color=color.red) |
maetugr | 2:ca8fd72a8f2b | 75 | p_line = box(length=1,height=0.08,width=0.1,color=color.yellow) |
maetugr | 2:ca8fd72a8f2b | 76 | plat_arrow = arrow(color=color.green,axis=(1,0,0), shaftwidth=0.06, fixedwidth=1) |
maetugr | 2:ca8fd72a8f2b | 77 | |
maetugr | 2:ca8fd72a8f2b | 78 | |
maetugr | 2:ca8fd72a8f2b | 79 | f = open("Serial"+str(time())+".txt", 'w') |
maetugr | 2:ca8fd72a8f2b | 80 | |
maetugr | 2:ca8fd72a8f2b | 81 | roll=0 |
maetugr | 2:ca8fd72a8f2b | 82 | pitch=0 |
maetugr | 2:ca8fd72a8f2b | 83 | yaw=0 |
maetugr | 2:ca8fd72a8f2b | 84 | while 1: |
maetugr | 2:ca8fd72a8f2b | 85 | rate(100) # for speed issues with 3D not refreshing or loosing Serial data |
maetugr | 2:ca8fd72a8f2b | 86 | line = ser.readline() |
maetugr | 2:ca8fd72a8f2b | 87 | #line = line.replace("!ANG:","") # Delete "!ANG:" |
maetugr | 2:ca8fd72a8f2b | 88 | print line |
maetugr | 2:ca8fd72a8f2b | 89 | f.write(line) # Write to the output log file |
maetugr | 2:ca8fd72a8f2b | 90 | words = string.split(line,",") # Fields split |
maetugr | 2:ca8fd72a8f2b | 91 | if len(words) > 2: |
maetugr | 2:ca8fd72a8f2b | 92 | try: |
maetugr | 2:ca8fd72a8f2b | 93 | roll = -float(words[0])*grad2rad |
maetugr | 2:ca8fd72a8f2b | 94 | pitch = float(words[1])*grad2rad |
maetugr | 2:ca8fd72a8f2b | 95 | yaw = -float(words[2])*grad2rad |
maetugr | 2:ca8fd72a8f2b | 96 | |
maetugr | 2:ca8fd72a8f2b | 97 | axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch)) |
maetugr | 2:ca8fd72a8f2b | 98 | 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)) |
maetugr | 2:ca8fd72a8f2b | 99 | platform.axis=axis |
maetugr | 2:ca8fd72a8f2b | 100 | platform.up=up |
maetugr | 2:ca8fd72a8f2b | 101 | platform.length=1.0 |
maetugr | 2:ca8fd72a8f2b | 102 | platform.width=0.65 |
maetugr | 2:ca8fd72a8f2b | 103 | plat_arrow.axis=axis |
maetugr | 2:ca8fd72a8f2b | 104 | plat_arrow.up=up |
maetugr | 2:ca8fd72a8f2b | 105 | plat_arrow.length=0.8 |
maetugr | 2:ca8fd72a8f2b | 106 | p_line.axis=axis |
maetugr | 2:ca8fd72a8f2b | 107 | p_line.up=up |
maetugr | 2:ca8fd72a8f2b | 108 | cil_roll.axis=(0.2*cos(roll),0.2*sin(roll),0) |
maetugr | 2:ca8fd72a8f2b | 109 | cil_roll2.axis=(-0.2*cos(roll),-0.2*sin(roll),0) |
maetugr | 2:ca8fd72a8f2b | 110 | cil_pitch.axis=(0.2*cos(pitch),0.2*sin(pitch),0) |
maetugr | 2:ca8fd72a8f2b | 111 | cil_pitch2.axis=(-0.2*cos(pitch),-0.2*sin(pitch),0) |
maetugr | 2:ca8fd72a8f2b | 112 | arrow_course.axis=(0.2*sin(yaw),0.2*cos(yaw),0) |
maetugr | 2:ca8fd72a8f2b | 113 | L1.text = str(float(words[0])) |
maetugr | 2:ca8fd72a8f2b | 114 | L2.text = str(float(words[1])) |
maetugr | 2:ca8fd72a8f2b | 115 | L3.text = str(float(words[2])) |
maetugr | 2:ca8fd72a8f2b | 116 | except: |
maetugr | 2:ca8fd72a8f2b | 117 | print "Invalid line" |
maetugr | 2:ca8fd72a8f2b | 118 | |
maetugr | 2:ca8fd72a8f2b | 119 | ser.close |
maetugr | 2:ca8fd72a8f2b | 120 | f.close |