single IMU

integration/try.py

+import tkinter as tk
+from matplotlib.figure import Figure
+from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
+from mpl_toolkits.mplot3d.art3d import Poly3DCollection
+import numpy as np
+import serial
+from time import sleep
+import math
+
+class VisualizationApp:
+    def __init__(self, root):
+        self.root = root
+        self.root.title("3D Visualization")
+
+        # Create a frame
+        self.frame = tk.Frame(self.root)
+        self.frame.pack(fill=tk.BOTH, expand=True)
+
+        # Serial Port Setup
+        self.arduino = serial.Serial('COM6', 115200)
+        sleep(1)
+
+        # Conversions
+        self.toRad = 2 * np.pi / 360
+        self.toDeg = 1 / self.toRad
+
+        # Initialize Parameters
+        self.count = 0
+        self.averageroll = 0
+        self.averageyaw = 0
+        self.averagepitch = 0
+        self.averageemg = 0
+        self.iterations = 10  # EMG measurements to get average
+
+        # Create a figure for the 3D plot
+        self.fig = Figure(figsize=(8, 6), dpi=100)
+        self.ax = self.fig.add_subplot(111, projection='3d')
+
+        # Set Limits
+        self.ax.set_xlim(-2, 2)
+        self.ax.set_ylim(-2, 2)
+        self.ax.set_zlim(-2, 2)
+
+        # Set labels
+        self.ax.set_xlabel('X')
+        self.ax.set_ylabel('Y')
+        self.ax.set_zlabel('Z')
+
+        # Draw Axes
+        self.ax.quiver(0, 0, 0, 2, 0, 0, color='red', label='X-Axis', arrow_length_ratio=0.1)  # X Axis (Red)
+        self.ax.quiver(0, 0, 0, 0, -2, 0, color='green', label='Y-Axis', arrow_length_ratio=0.1)  # Y Axis (Green)
+        self.ax.quiver(0, 0, 0, 0, 0, 4, color='blue', label='Z-Axis', arrow_length_ratio=0.1)  # Z Axis (Blue)
+
+        # Draw the board as a rectangular prism (solid)
+        self.prism_vertices = np.array([
+            [-1.5, -1, 0], [1.5, -1, 0], [1.5, 1, 0], [-1.5, 1, 0],  # bottom vertices
+            [-1.5, -1, 0.1], [1.5, -1, 0.1], [1.5, 1, 0.1], [-1.5, 1, 0.1]  # top vertices (height=0.1 for visual thickness)
+        ])
+
+        self.prism_faces = [
+            [self.prism_vertices[j] for j in [0, 1, 2, 3]],  # bottom face
+            [self.prism_vertices[j] for j in [4, 5, 6, 7]],  # top face
+            [self.prism_vertices[j] for j in [0, 1, 5, 4]],  # side face
+            [self.prism_vertices[j] for j in [1, 2, 6, 5]],  # side face
+            [self.prism_vertices[j] for j in [2, 3, 7, 6]],  # side face
+            [self.prism_vertices[j] for j in [3, 0, 4, 7]]   # side face
+        ]
+
+        self.prism_collection = Poly3DCollection(self.prism_faces, facecolors='gray', linewidths=1, edgecolors='black', alpha=0.25)
+        self.ax.add_collection3d(self.prism_collection)
+
+        # Front Arrow (Purple)
+        self.front_arrow, = self.ax.plot([0, 2], [0, 0], [0, 0], color='purple', marker='o', markersize=10, label='Front Arrow')
+
+        # Up Arrow (Magenta)
+        self.up_arrow, = self.ax.plot([0, 0], [0, -1], [0, 1], color='magenta', marker='o', markersize=10, label='Up Arrow')
+
+        # Side Arrow (Orange)
+        self.side_arrow, = self.ax.plot([0, 1], [0, -1], [0, 1], color='orange', marker='o', markersize=10, label='Side Arrow')
+
+        # Create a canvas to draw on
+        self.canvas = FigureCanvasTkAgg(self.fig, master=self.frame)
+        self.canvas.draw()
+        self.canvas.get_tk_widget().pack(fill=tk.BOTH, expand=True)
+
+        # Create a label for average EMG
+        self.emg_label = tk.Label(self.frame, text="Average EMG: 0", font=("Arial", 14))
+        self.emg_label.pack(pady=10)
+
+        # Add a button to start data transmission
+        self.start_button = tk.Button(self.frame, text="Start Data Transmission", command=self.start_data_transmission)
+        self.start_button.pack(pady=10)
+
+        # Initialize the data transmission flag
+        self.transmitting = False
+
+    def start_data_transmission(self):
+        # Set the transmitting flag to True and start the update loop
+        self.transmitting = True
+        self.update_display()
+
+    def update_display(self):
+        if self.transmitting:
+            try:
+                while self.arduino.inWaiting() > 0:
+                    dataPacket = self.arduino.readline()
+                    dataPacket = dataPacket.decode()
+                    cleandata = dataPacket.replace("\r\n", "")
+                    row = cleandata.strip().split(',')
+
+                    if len(row) == 9:
+                        splitPacket = cleandata.split(',')
+
+                        emg = float(splitPacket[0])  # EMG sensor data
+                        q0 = float(splitPacket[1])  # qw
+                        q1 = float(splitPacket[2])  # qx
+                        q2 = float(splitPacket[3])  # qy
+                        q3 = float(splitPacket[4])  # qz
+
+                        # Calculate Angles
+                        roll = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))
+                        pitch = -math.asin(2 * (q0 * q2 - q3 * q1))
+                        yaw = -math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3))
+
+                        # Rotation matrices
+                        Rz = np.array([
+                            [np.cos(yaw), -np.sin(yaw), 0],
+                            [np.sin(yaw), np.cos(yaw), 0],
+                            [0, 0, 1]
+                        ])
+
+                        Ry = np.array([
+                            [np.cos(pitch), 0, np.sin(pitch)],
+                            [0, 1, 0],
+                            [-np.sin(pitch), 0, np.cos(pitch)]
+                        ])
+
+                        Rx = np.array([
+                            [1, 0, 0],
+                            [0, np.cos(roll), -np.sin(roll)],
+                            [0, np.sin(roll), np.cos(roll)]
+                        ])
+
+                        R = Rz @ Ry @ Rx  # Combined rotation matrix
+
+                        # Apply the rotation
+                        rotated_vertices = (R @ self.prism_vertices.T).T
+
+                        prism_faces_rotated = [
+                            [rotated_vertices[j] for j in [0, 1, 2, 3]],  # bottom face
+                            [rotated_vertices[j] for j in [4, 5, 6, 7]],  # top face
+                            [rotated_vertices[j] for j in [0, 1, 5, 4]],  # side face
+                            [rotated_vertices[j] for j in [1, 2, 6, 5]],  # side face
+                            [rotated_vertices[j] for j in [2, 3, 7, 6]],  # side face
+                            [rotated_vertices[j] for j in [3, 0, 4, 7]]   # side face
+                        ]
+
+                        # Update the collection
+                        self.prism_collection.set_verts(prism_faces_rotated)
+
+                        # Update Arrows
+                        k = np.array([np.cos(yaw) * np.cos(pitch), np.sin(pitch), np.sin(yaw) * np.cos(pitch)])  # X vector
+                        y = np.array([0, 1, 0])  # Y vector: pointing down
+                        s = np.cross(k, y)  # Side vector
+                        v = np.cross(s, k)  # Up vector
+                        vrot = v * np.cos(roll) + np.cross(k, v) * np.sin(roll)  # Rotated Up vector
+
+                        self.front_arrow.set_data([0, k[0] * 2], [0, k[1] * 2])
+                        self.front_arrow.set_3d_properties([0, k[2] * 2])
+                        self.up_arrow.set_data([0, vrot[0] * 1], [0, vrot[1] * 1])
+                        self.up_arrow.set_3d_properties([0, vrot[2] * 1])
+                        self.side_arrow.set_data([0, s[0] * 1], [0, s[1] * 1])
+                        self.side_arrow.set_3d_properties([0, s[2] * 1])
+
+                        # Update canvas
+                        self.canvas.draw()
+
+                        self.averageroll += roll * self.toDeg
+                        self.averageyaw += yaw * self.toDeg
+                        self.averagepitch += pitch * self.toDeg
+                        self.averageemg += emg
+
+                        if self.count == self.iterations:
+                            self.averageroll = self.averageroll / self.iterations
+                            self.averageyaw = self.averageyaw / self.iterations
+                            self.averagepitch = self.averagepitch / self.iterations
+                            self.averageemg = self.averageemg / self.iterations
+
+                            self.averageroll = round(self.averageroll)
+                            self.averageyaw = round(self.averageyaw)
+                            self.averagepitch = round(self.averagepitch)
+
+                            # Print the averaged results
+                            print("iterations:", self.iterations)
+                            print("averageroll is", self.averageroll)
+                            print("averageyaw is", self.averageyaw)
+                            print("averagepitch is", self.averagepitch)
+                            print("averageemg=", self.averageemg)
+
+                            self.count = 0
+
+                            self.averageyaw = 0
+                            self.averageroll = 0
+                            self.averagepitch = 0
+                            self.averageemg = 0
+                        else:
+                            self.count += 1
+
+                        # Update EMG Label
+                        self.emg_label.config(text=f"Average EMG: {self.averageemg:.2f}")
+
+            except Exception as e:
+                print(f"An error occurred: {e}")
+
+            # Call update_display() again after 50 milliseconds
+            self.root.after(50, self.update_display)
+
+if __name__ == "__main__":
+    root = tk.Tk()
+    app = VisualizationApp(root)
+    root.mainloop()