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.

Author: jarobase

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 = "<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 = 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()