Combined done without send to Unty

integration/RP2040IMUEMG.py

+from vpython import *
+import numpy as np
+from time import *
+import math
+import serial
+
+column_limit = 10 # Adjusted to account for two EMG values plus the others
+
+arduino = serial.Serial('COM8', 115200)
+sleep(1)
+
+# Conversions
+toRad = 2 * np.pi / 360
+toDeg = 1 / toRad
+
+scene.range = 5
+scene.forward = vector(-1, -1, -1)
+scene.width = 600
+scene.height = 600
+
+Xarrow = arrow(axis=vector(1, 0, 0), length=2, shaftwidth=.1, color=color.red)
+Yarrow = arrow(axis=vector(0, 1, 0), length=2, shaftwidth=.1, color=color.green)
+Zarrow = arrow(axis=vector(0, 0, 1), length=4, shaftwidth=.1, color=color.blue)
+
+frontArrow = arrow(axis=vector(1, 0, 0), length=4, shaftwidth=.1, color=color.purple)
+upArrow = arrow(axis=vector(0, 1, 0), length=1, shaftwidth=.1, color=color.magenta)
+sideArrow = arrow(axis=vector(0, 0, 1), length=2, shaftwidth=.1, color=color.orange)
+
+bBoard = box(length=3, width=4, height=.2, color=color.white, opacity=0.8, pos=vector(0, 0, 0))
+bno = box(color=color.blue, length=1, width=.75, height=0.1, pos=vector(-0.5, 0.1 + 0.05, 0))
+nano = box(length=1.75, width=.6, height=.1, pos=vector(-2, .1 + .05, 0), color=color.green)
+
+myObj = compound([bBoard, bno, nano])
+count = 0
+
+averageroll = 0
+averageyaw = 0
+averagepitch = 0
+averageemg1 = 0
+averageemg2 = 0
+
+# Motor positions
+M1 = 90
+M2 = 90
+M3 = 45
+M4 = 90
+M5 = 90
+M6 = 10
+iterations = 10  # EMG measurements to get average
+
+while True:  # Replace with True to run forever
+    try:
+        while arduino.inWaiting() == 0:
+            pass
+
+        dataPacket = arduino.readline()
+        dataPacket = dataPacket.decode()
+        cleandata = dataPacket.replace("\r\n", "")
+        row = cleandata.strip().split(',')
+
+        if len(row) == column_limit:
+            splitPacket = cleandata.split(',')
+            
+            emg1 = float(splitPacket[0])  # First EMG sensor data
+            emg2 = float(splitPacket[1])  # Second EMG sensor data
+            print(f"emg1: {emg1}, emg2: {emg2}")
+            
+            q0 = float(splitPacket[2])  # qw
+            q1 = float(splitPacket[3])  # qx
+            q2 = float(splitPacket[4])  # qy
+            q3 = float(splitPacket[5])  # qz
+
+            # Calibration Statuses
+            aC = float(splitPacket[6])  # Accelerometer
+            gC = float(splitPacket[7])  # Gyroscope
+            mC = float(splitPacket[8])  # Magnetometer
+            sC = float(splitPacket[9])  # Whole System
+
+            roll = atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))
+            pitch = -asin(2 * (q0 * q2 - q3 * q1))
+            yaw = -atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3))
+
+            k = vector(cos(yaw) * cos(pitch), sin(pitch), sin(yaw) * cos(pitch))  # x vector
+            y = vector(0, 1, 0)  # y vector: pointing down
+            s = cross(k, y)  # this is pointing to the side
+            v = cross(s, k)  # the up vector
+            vrot = v * cos(roll) + cross(k, v) * sin(roll)
+
+            frontArrow.axis = k
+            sideArrow.axis = cross(k, vrot)
+            upArrow.axis = vrot
+            myObj.axis = k
+            myObj.up = vrot
+            sideArrow.length = 2
+            frontArrow.length = 4
+            upArrow.length = 1
+
+            averageroll += roll * toDeg
+            averageyaw += yaw * toDeg
+            averagepitch += pitch * toDeg
+            averageemg1 += emg1
+            averageemg2 += emg2
+
+            if count == iterations:
+                count = 0
+                averageroll = averageroll / iterations
+                averageyaw = averageyaw / iterations
+                averagepitch = averagepitch / iterations
+                averageemg1 = averageemg1 / iterations
+                averageemg2 = averageemg2 / iterations
+
+                averageroll = round(averageroll)
+                averageyaw = round(averageyaw)
+                averagepitch = round(averagepitch)
+
+                if 0 < averageyaw < 180:
+                    M1 = averageyaw
+                if 0 < averageroll < 180:
+                    M5 = averageroll
+
+                if averagepitch < (-20):
+                    M2 = 50
+                    M3 = 0
+                    M4 = 135
+                elif (-20) <= averagepitch <= 0:
+                    M2 = averagepitch * 2 + 90
+                    M3 = 0
+                    M4 = (averagepitch * (-2.25)) + 90
+                elif 0 < averagepitch <= 90:
+                    M2 = 90
+                    M3 = averagepitch
+                    M4 = 90 - averagepitch
+                elif averagepitch > 90:
+                    M2 = 90
+                    M3 = 90
+                    M4 = 0
+
+                if averageemg1 > 7.1 or averageemg2 > 7.1:
+                    M6 = 75
+                else:
+                    M6 = 10
+
+                data_to_send = [M1, M2, M3, M4, M5, M6]
+                
+                averageyaw = 0
+                averageroll = 0
+                averagepitch = 0
+                averageemg1 = 0
+                averageemg2 = 0
+            else:
+                count += 1
+    except Exception as e:
+        print(f"Error: {e}")
+        pass