From ca50bde47a56e063554c3e4e6bb9ab3af80f1d2b Mon Sep 17 00:00:00 2001 From: jnanar Date: Thu, 13 May 2021 10:56:21 +0200 Subject: [PATCH 1/2] Update MinIMU-9-test.py to run with VPython 7 and python3 Before this commit, the python script was only compatible with older versions of Vpython (5.74) and python 2.7. This modified script can be run with latest pip version of Vpython and python 3.X. It was tested in a virtualenv on Linux: Python: 3.9.4 vpython: 7.6.1 This new version does basically the same thing than the previous one but in the navigator. --- MinIMU-9-test.py | 184 +++++++++++++++++++++++++---------------------- 1 file changed, 97 insertions(+), 87 deletions(-) diff --git a/MinIMU-9-test.py b/MinIMU-9-test.py index 993c8e8..244e6c3 100644 --- a/MinIMU-9-test.py +++ b/MinIMU-9-test.py @@ -31,118 +31,128 @@ # 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 +import os +import time -from time import time - -grad2rad = 3.141592/180.0 +grad2rad = 3.141592 / 180.0 # Check your COM port and baud rate -ser = serial.Serial(port='COM9',baudrate=115200, timeout=1) +ser = serial.Serial(port='COM',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) +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 = 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 = canvas(title='Rotation axis') +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) +# 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=(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) +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=(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) +# 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 = 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) +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) +f = open("Serial"+str(time.time())+".txt", 'w') -f = open("Serial"+str(time())+".txt", 'w') +roll = 0 +pitch = 0 +yaw = 0 -roll=0 -pitch=0 -yaw=0 -while 1: +time.sleep(0.2) +rate(25) +while True: 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 + line = line.decode('utf-8') + if line.find("=ANG:") != -1: # filter out incomplete (invalid) lines + line = line.replace("=ANG:", "") # Delete "!ANG:" + f.write(line) # Write to the output log file + 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: - 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 + 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.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 = 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() +f.close() From eb2b180d4439c666ba20d63743c584ff969b5d55 Mon Sep 17 00:00:00 2001 From: jnanar Date: Thu, 13 May 2021 15:27:05 +0200 Subject: [PATCH 2/2] Update: move code to a dedicated method. --- MinIMU-9-test.py | 236 +++++++++++++++++++++++++---------------------- 1 file changed, 128 insertions(+), 108 deletions(-) diff --git a/MinIMU-9-test.py b/MinIMU-9-test.py index 244e6c3..3d23aa5 100644 --- a/MinIMU-9-test.py +++ b/MinIMU-9-test.py @@ -43,116 +43,136 @@ import os import time -grad2rad = 3.141592 / 180.0 - -# Check your COM port and baud rate -ser = serial.Serial(port='COM',baudrate=115200, timeout=1) - -# Main scene -scene = canvas(title="Platform") -caption = "Pololu MinIMU-9 + Arduino AHRS" -caption += """ +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') -scene.width = 500 -scene.y = 200 - -scene2.select() -# 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 = 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) - -f = open("Serial"+str(time.time())+".txt", 'w') - -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:" - f.write(line) # Write to the output log file - 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.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 = 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() -f.close() + 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()