diff --git a/MinIMU-9-test.py b/MinIMU-9-test.py index 993c8e8..3d23aa5 100644 --- a/MinIMU-9-test.py +++ b/MinIMU-9-test.py @@ -31,118 +31,148 @@ # This script needs VPython, pyserial and pywin modules -# First Install Python 2.6.4 (Python 2.7 also works) +# First Install Python 3 (tested with Python 3.9 on linux but earlier versions on Windows should work) +# This can be used in a Python/Conda virtualenv # Install pywin from http://sourceforge.net/projects/pywin32/ # Install pyserial from http://sourceforge.net/projects/pyserial/files/ # Install VPython from http://vpython.org/contents/download_windows.html -from visual import * +from vpython import * import serial -import string import math - -from time import time - -grad2rad = 3.141592/180.0 - -# Check your COM port and baud rate -ser = serial.Serial(port='COM9',baudrate=115200, timeout=1) - -# Main scene -scene=display(title="Pololu MinIMU-9 + Arduino AHRS") -scene.range=(1.2,1.2,1.2) -#scene.forward = (0,-1,-0.25) -scene.forward = (1,0,-0.25) -scene.up=(0,0,1) - -# Second scene (Roll, Pitch, Yaw) -scene2 = display(title='Pololu MinIMU-9 + Arduino AHRS',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 - -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) -#cil_course = cylinder(pos=(0.6,0,0),axis=(0.2,0,0),radius=0.01,color=color.blue) -#cil_course2 = cylinder(pos=(0.6,0,0),axis=(-0.2,0,0),radius=0.01,color=color.blue) -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=(0,0,0.8),text="Pololu MinIMU-9 + Arduino AHRS",box=0,opacity=0) -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.blue) -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) - - -f = open("Serial"+str(time())+".txt", 'w') - -roll=0 -pitch=0 -yaw=0 -while 1: - line = ser.readline() - if line.find("!ANG:") != -1: # filter out incomplete (invalid) lines - line = line.replace("!ANG:","") # Delete "!ANG:" - print line - f.write(line) # Write to the output log file - words = string.split(line,",") # Fields split - if len(words) > 2: - 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])) -ser.close -f.close +import os +import time + +DEVICE = "/dev/ttyACM" # could be "COM", "/dev/ttyUSB", ... + + +def launch(): + grad2rad = 3.141592 / 180.0 + # find the device + ser = None + maxi = 10 + it = 0 + while it < maxi: + try: + dev = DEVICE + str(it) + if os.path.exists(dev): + ser = serial.Serial(port=dev, baudrate=115200, timeout=1) + print(dev + " exists !") + break + else: + print("device %s does not exists" % dev) + except serial.serialutil.SerialException: + print("Serial device triggered an exception :-(") + it += 1 + if not ser: + return 1 + + # Main scene + scene = canvas(title="Platform") + caption = "Pololu MinIMU-9 + Arduino AHRS" + caption += """ +Click to toggle between pausing or running. +Right button drag or Ctrl-drag to rotate "camera" to view scene. +To zoom, drag with mid1e button or Alt/Option depressed, or use scroll wheel. +On a two-button mouse, midd1e is left + right. +Touch screen: pinch/extend to zoom, swipe or two-finger rotate.""" + scene.caption = caption + + scene.forward = vector(0, -1, -0.25) + + # # Second scene (Roll, Pitch, Yaw) + scene2 = canvas(title='Rotation axis') + scene2.select() + scene.width = 500 + scene.y = 200 + + # Roll, Pitch, Yaw + cil_roll = cylinder(pos=vector(-0.4, 0, 0), axis=vector(0.2, 0, 0), radius=0.01, color=color.red) + cil_roll2 = cylinder(pos=vector(-0.4, 0, 0), axis=vector(-0.2, 0, 0), radius=0.01, color=color.red) + cil_pitch = cylinder(pos=vector(0.1, 0, 0), axis=vector(0.2, 0, 0), radius=0.01, color=color.green) + cil_pitch2 = cylinder(pos=vector(0.1, 0, 0), axis=vector(-0.2, 0, 0), radius=0.01, color=color.green) + # cil_course = cylinder(pos=(0.6,0,0),axis=(0.2,0,0),radius=0.01,color=color.blue) + # cil_course2 = cylinder(pos=(0.6,0,0),axis=(-0.2,0,0),radius=0.01,color=color.blue) + arrow_course = arrow(pos=vector(0.6, 0, 0), color=color.cyan, axis=vector(-0.2, 0, 0), shaftwidth=0.02, + fixedwidth=1) + + # Roll,Pitch,Yaw labels + label(pos=vector(-0.4, 0.3, 0), text="Roll", box=0, opacity=0, billboard=True) + label(pos=vector(0.1, 0.3, 0), text="Pitch", box=0, opacity=1, billboard=True) + label(pos=vector(0.55, 0.3, 0), text="Yaw", box=0, opacity=0, billboard=True) + label(pos=vector(0.6, 0.22, 0), text="N", color=color.yellow, box=0, opacity=0) + label(pos=vector(0.6, -0.22, 0), text="S", color=color.yellow, box=0, opacity=0) + label(pos=vector(0.38, 0, 0), text="W", color=color.yellow, box=0, opacity=0) + label(pos=vector(0.82, 0, 0), text="E", box=0, opacity=0, color=color.yellow) + label(pos=vector(0.75, 0.15, 0), height=7, text="NE", box=0, color=color.yellow) + label(pos=vector(0.45, 0.15, 0), height=7, text="NW", box=0, color=color.yellow) + label(pos=vector(0.75, -0.15, 0), height=7, text="SE", box=0, color=color.yellow) + label(pos=vector(0.45, -0.15, 0), height=7, text="SW", box=0, color=color.yellow) + + l_1 = label(pos=vector(-0.4, 0.22, 0), text="-", box=0, opacity=0) + l_2 = label(pos=vector(0.1, 0.22, 0), text="-", box=0, opacity=0) + l_3 = label(pos=vector(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=vector(1, 0, 0), shaftwidth=0.02, fixedwidth=1) + arrow(color=color.green, axis=vector(0, -1, 0), shaftwidth=0.02, fixedwidth=1) + arrow(color=color.green, axis=vector(0, 0, -1), shaftwidth=0.02, fixedwidth=1) + # labels + # label(pos=vector(0, 0, 0.8), text="Pololu MinIMU-9 + Arduino AHRS", box=0, opacity=0) + label(pos=vector(1, 0, 0), text="X", box=0, opacity=0) + label(pos=vector(0, -1, 0), text="Y", box=0, opacity=0) + label(pos=vector(0, 0, -1), text="Z", box=0, opacity=0) + # IMU object + platform_mimi = box(length=1, height=0.05, width=1, color=color.blue) + p_line = box(length=1, height=0.08, width=0.1, color=color.yellow) + plat_arrow = arrow(color=color.green, axis=vector(1, 0, 0), shaftwidth=0.06, fixedwidth=1) + roll = 0 + pitch = 0 + yaw = 0 + + time.sleep(0.2) + rate(25) + while True: + line = ser.readline() + line = line.decode('utf-8') + if line.find("=ANG:") != -1: # filter out incomplete (invalid) lines + line = line.replace("=ANG:", "") # Delete "!ANG:" + words = line.split(",") # Fields split + if len(words) > 2: + try: + roll = float(words[0]) * grad2rad + pitch = float(words[1]) * grad2rad + yaw = float(words[2]) * grad2rad + except (TypeError, IndexError) as ex: + print("Invalid line") + + axis = vector(math.cos(pitch) * math.cos(yaw), -math.cos(pitch) * math.sin(yaw), math.sin(pitch)) + up = vector(math.sin(roll) * math.sin(yaw) + math.cos(roll) * math.sin(pitch) * math.cos(yaw), + math.sin(roll) * math.cos(yaw) - math.cos(roll) * math.sin(pitch) * math.sin(yaw), + - math.cos(roll) * math.cos(pitch)) + platform_mimi.axis = axis + platform_mimi.up = up + platform_mimi.length = 1.0 + platform_mimi.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 = vector(0.2 * math.cos(roll), 0.2 * math.sin(roll), 0) + cil_roll2.axis = vector(-0.2 * math.cos(roll), -0.2 * math.sin(roll), 0) + cil_pitch.axis = vector(0.2 * math.cos(pitch), 0.2 * math.sin(pitch), 0) + cil_pitch2.axis = vector(-0.2 * math.cos(pitch), -0.2 * math.sin(pitch), 0) + arrow_course.axis = vector(0.2 * math.sin(yaw), 0.2 * math.cos(yaw), 0) + l_1.text = str(float(words[0])) + l_2.text = str(float(words[1])) + l_3.text = str(float(words[2])) + sleep(0.05) + + ser.close() + + +if __name__ == '__main__': + launch()