[Python] IMU 9DOF modifié

Salut à tous !

Voici ma petite version modifié du script python pour l’IMU 9DOF de sparkfun. La seul grosse modification est l’ajout des graphiques pour avoir les données brut en provenance des capteurs.

Vidéo de démonstration :

Programme et firmware original :
http://code.google.com/p/sf9domahrs/

Vidéo de test officiel :

Page sparkfun :
http://www.sparkfun.com/products/9623

Le code source modifié :

from visual import *
from visual.graph import *
import serial,string,math
from time import time

grad2rad = 3.141592/180.0

# Check your COM port and baud rate
ser = serial.Serial(port='COM5',baudrate=57600, timeout=1)

# Main scene
scene=display(title="9DOF Platform Test - 3D Object")
scene.range=(1.2,1.2,1.2)
scene.forward = (1,0,-0.25)
scene.up=(0,0,1)

# Second scene (Roll, Pitch, Yaw)
scene2 = display(ytitle='9DOF Platform Test - Compensed Data',x=0, y=0, width=500, height=200,center=(0,0,0), background=(0,0,0))
scene2.range=(1,1,1)
scene.width=500
scene.y=200

# 3eme scene (G force x y z)
scene3 = gdisplay(title='9DOF Platform Test - G Force',ytitle="G force XYZ",xtitle="Time",x=500, y=0, width=800, height=400,xmax=200, xmin=0)

# 4eme scene (raw x y z)
scene4 = gdisplay(title='9DOF Platform Test - Raw Data', ytitle="Raw Data XYZ",xtitle="Time",x=500, y=400, width=800, height=400,xmax=200, xmin=0)

# Wait scene
wait = display(title="Wait Until IMU Get Ready !",x=500, y=200, width=500, height=500,center=(0,0,0), background=(0,0,0))
wait.select()
label(pos=(0,0,0),text="STATUS: IDLE",box=0,opacity=0)

ax = gcurve(gdisplay=scene3 ,color=color.cyan)
ay = gcurve(gdisplay=scene3 ,color=color.green)
az = gcurve(gdisplay=scene3 ,color=color.red)
mx = gcurve(gdisplay=scene4 ,color=color.cyan)
my = gcurve(gdisplay=scene4 ,color=color.green)
mz = gcurve(gdisplay=scene4 ,color=color.red)
gx = gcurve(gdisplay=scene4 ,color=color.yellow)
gy = gcurve(gdisplay=scene4 ,color=color.orange)
gz = gcurve(gdisplay=scene4 ,color=color.black)

scene2.select()
#Roll, Pitch, Yaw
cil_roll = cylinder(pos=(-0.4,0,0),axis=(0.2,0,0),radius=0.01,color=color.red)
cil_roll2 = cylinder(pos=(-0.4,0,0),axis=(-0.2,0,0),radius=0.01,color=color.red)
cil_pitch = cylinder(pos=(0.1,0,0),axis=(0.2,0,0),radius=0.01,color=color.green)
cil_pitch2 = cylinder(pos=(0.1,0,0),axis=(-0.2,0,0),radius=0.01,color=color.green)
arrow_course = arrow(pos=(0.6,0,0),color=color.cyan,axis=(-0.2,0,0), shaftwidth=0.02, fixedwidth=1)

#Roll,Pitch,Yaw labels
label(pos=(-0.4,0.3,0),text="Roll",box=0,opacity=0)
label(pos=(0.1,0.3,0),text="Pitch",box=0,opacity=0)
label(pos=(0.55,0.3,0),text="Yaw",box=0,opacity=0)
label(pos=(0.6,0.22,0),text="N",box=0,opacity=0,color=color.yellow)
label(pos=(0.6,-0.22,0),text="S",box=0,opacity=0,color=color.yellow)
label(pos=(0.38,0,0),text="W",box=0,opacity=0,color=color.yellow)
label(pos=(0.82,0,0),text="E",box=0,opacity=0,color=color.yellow)
label(pos=(0.75,0.15,0),height=7,text="NE",box=0,color=color.yellow)
label(pos=(0.45,0.15,0),height=7,text="NW",box=0,color=color.yellow)
label(pos=(0.75,-0.15,0),height=7,text="SE",box=0,color=color.yellow)
label(pos=(0.45,-0.15,0),height=7,text="SW",box=0,color=color.yellow)

L1 = label(pos=(-0.4,0.22,0),text="-",box=0,opacity=0)
L2 = label(pos=(0.1,0.22,0),text="-",box=0,opacity=0)
L3 = label(pos=(0.7,0.3,0),text="-",box=0,opacity=0)

# Main scene objects
scene.select()
# Reference axis (x,y,z)
arrow(color=color.green,axis=(1,0,0), shaftwidth=0.02, fixedwidth=1)
arrow(color=color.green,axis=(0,-1,0), shaftwidth=0.02 , fixedwidth=1)
arrow(color=color.green,axis=(0,0,-1), shaftwidth=0.02, fixedwidth=1)
# labels
label(pos=(1,0,0),text="X",box=0,opacity=0)
label(pos=(0,-1,0),text="Y",box=0,opacity=0)
label(pos=(0,0,-1),text="Z",box=0,opacity=0)
# IMU object
platform = box(length=1, height=0.05, width=1, color=color.red)
p_line = box(length=1,height=0.08,width=0.1,color=color.yellow)
plat_arrow = arrow(color=color.green,axis=(1,0,0), shaftwidth=0.06, fixedwidth=1)

x = 0
l = 0
roll=0
pitch=0
yaw=0

i = 6
while i > 0:
    ser.readline()
    i = i - 1
    
wait.visible = False

while 1:
    line = ser.readline()

    line = line.replace("!ANG:","") 
    line = line.replace("AN:","")   
             
    words = string.split(line,",")
    if len(words) > 10:
        try:
            roll = float(words[0])*grad2rad
            pitch = float(words[1])*grad2rad
            yaw = float(words[2])*grad2rad
        except:
            print "Invalid line"

        axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch)) 
        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))
        platform.axis=axis
        platform.up=up
        platform.length=1.0
        platform.width=0.65
        plat_arrow.axis=axis
        plat_arrow.up=up
        plat_arrow.length=0.8
        p_line.axis=axis
        p_line.up=up
        cil_roll.axis=(0.2*cos(roll),0.2*sin(roll),0)
        cil_roll2.axis=(-0.2*cos(roll),-0.2*sin(roll),0)
        cil_pitch.axis=(0.2*cos(pitch),0.2*sin(pitch),0)
        cil_pitch2.axis=(-0.2*cos(pitch),-0.2*sin(pitch),0)
        arrow_course.axis=(0.2*sin(yaw),0.2*cos(yaw),0)
        L1.text = str(float(words[0]))
        L2.text = str(float(words[1]))
        L3.text = str(float(words[2]))

        gx.plot(pos=(x,int(words[3])))
        gy.plot(pos=(x,int(words[4])))
        gz.plot(pos=(x,int(words[5])))
        ax.plot(pos=(x,int(words[6])))
        ay.plot(pos=(x,int(words[7])))
        az.plot(pos=(x,int(words[8])))
        mx.plot(pos=(x,int(words[9])))
        my.plot(pos=(x,int(words[10])))
        mz.plot(pos=(x,int(words[11])))
        
        x = x + 1
        if x > 200:
            x = 0
            scene3.display.visible = False
            scene4.display.visible = False
            scene3 = gdisplay(title='9DOF Platform Test - G Force',ytitle="G force XYZ",xtitle="Time",x=500, y=0, width=800, height=400,xmax=200, xmin=0)
            scene4 = gdisplay(title='9DOF Platform Test - Raw Data', ytitle="Raw Data XYZ",xtitle="Time",x=500, y=400, width=800, height=400,xmax=200, xmin=0)
            ax = gcurve(gdisplay=scene3 ,color=color.cyan)
            ay = gcurve(gdisplay=scene3 ,color=color.green)
            az = gcurve(gdisplay=scene3 ,color=color.red)
            mx = gcurve(gdisplay=scene4 ,color=color.cyan)
            my = gcurve(gdisplay=scene4 ,color=color.green)
            mz = gcurve(gdisplay=scene4 ,color=color.red)
            gx = gcurve(gdisplay=scene4 ,color=color.yellow)
            gy = gcurve(gdisplay=scene4 ,color=color.orange)
            gz = gcurve(gdisplay=scene4 ,color=color.black)


ser.close