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.
The code was moved in a dedicated function.

Author: jarobase

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 = "<b>Pololu MinIMU-9 + Arduino AHRS</b>"
+    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()