diff --git a/integration/integrate.py b/integration/integrate.py
index 6d69a360b540c9fb825fca05adbcdb9926aa5e02..45d2f057c2a8ab60faa71bc6848301e01d422812 100644
--- a/integration/integrate.py
+++ b/integration/integrate.py
@@ -1,3 +1,4 @@
+import threading
import tkinter as tk
from tkinter import ttk
from matplotlib.pyplot import Figure
@@ -8,6 +9,7 @@ import serial
from math import atan2, asin, cos, sin
from matplotlib.figure import Figure
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
+from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import numpy as np
from vpython import canvas
@@ -27,30 +29,31 @@ class Interface:
def create_popup(self):
global frame1, frame2, frame3, frame4,width,height,popup
- popup = tk.Tk()
- popup.title("Project")
+ self.popup = tk.Tk()
+ self.popup.title("Project")
# Set the initial size and position of the popup window
width = 1000
height = 600
- screen_width = popup.winfo_screenwidth()
- screen_height = popup.winfo_screenheight()
+ screen_width = self.popup.winfo_screenwidth()
+ screen_height = self.popup.winfo_screenheight()
x = (screen_width // 2) - (width // 2)
y = (screen_height // 2) - (height // 2)
- popup.geometry(f"{width}x{height}+{x}+{y}")
+ self.popup.geometry(f"{width}x{height}+{x}+{y}")
# Configure the grid to be expandable
- popup.columnconfigure(0, weight=1)
- popup.columnconfigure(1, weight=1)
- popup.rowconfigure(0, weight=1)
- popup.rowconfigure(1, weight=1)
+ self.popup.columnconfigure(0, weight=1)
+ self.popup.columnconfigure(1, weight=1)
+ self.popup.rowconfigure(0, weight=1)
+ self.popup.rowconfigure(1, weight=1)
# First row, two sections
- frame1 = ttk.Frame(popup, borderwidth=1, relief="solid",width=width/3,height=height/2)
- frame1.grid(row=0, column=0, padx=10, pady=10, sticky="nsew")
+ self.frame1 = ttk.Frame(self.popup, borderwidth=1, relief="solid",width=width/3,height=height/2)
+ self.frame1.grid(row=0, column=0, padx=10, pady=10, sticky="nsew")
+ self._initialise_IMU_3D_graph()
- frame2 = ttk.Frame(popup, borderwidth=1, relief="solid",width=width*2/3,height=height/2)
+ frame2 = ttk.Frame(self.popup, borderwidth=1, relief="solid",width=width*2/3,height=height/2)
frame2.grid(row=0, column=1, padx=10, pady=10, sticky="nsew")
label1 = ttk.Label(frame2, text="If IMU is connected to the laptop please click the Connect button",
wraplength=width / 2)
@@ -60,19 +63,19 @@ class Interface:
frame2.update_idletasks()
# Second row, two sections
- frame3 = ttk.Frame(popup, borderwidth=1, relief="solid",width=width/3,height=height/2)
+ frame3 = ttk.Frame(self.popup, borderwidth=1, relief="solid",width=width/3,height=height/2)
frame3.grid(row=1, column=0, padx=10, pady=10, sticky="nsew")
label3 = ttk.Label(frame3, text="If EMG is connected to the laptop please click the Connect button",wraplength=width/4)
label3.place(relx=0.5, rely=0.9, anchor='center')
label4 = ttk.Label(frame3, text="Port: None ")
label4.place(relx=0.35, rely=0.8, anchor='e')
- frame4 = ttk.Frame(popup, borderwidth=1, relief="solid",width=width*2/3,height=height/2)
+ frame4 = ttk.Frame(self.popup, borderwidth=1, relief="solid",width=width*2/3,height=height/2)
frame4.grid(row=1, column=1, padx=10, pady=10, sticky="nsew")
frame4.grid_propagate(False)
label4 = ttk.Label(frame4, text="Section 4")
label4.place(relx=0.5, rely=0.5, anchor='center')
- self._initialise_real_time_classification_graph()
+ #self._initialise_real_time_classification_graph()
# Add a button to frame1 to trigger adding widgets
add_button1 = ttk.Button(frame2, text="Connect", command=self.IMU_Connect)
@@ -85,91 +88,20 @@ class Interface:
add_button4 = ttk.Button(frame3, text="Disconnect", command=self.EMG_disconnect())
add_button4.place(relx=0.8, rely=0.8, anchor='center')
- popup.mainloop()
+
def IMU_Connect(self):
+ self.label2.config(text="Port: COM6")
try:
self._iMU_isConnected=True
- self.arduino = serial.Serial('COM6', 115200, timeout=1)
- column_limit = 9
- self.label2 = ttk.Label(frame2, text="Port: COM6 ")
- self.label2.place(relx=0.35, rely=0.8, anchor='center')
+
print("Connected to IMU")
+ self.update_thread = threading.Thread(target=self.update_display)
+ self.update_thread.daemon = True
+ self.update_thread.start()
except serial.SerialException:
print("IMU is not connected")
return
- sleep(1)
-
- scene.range = 5 # 修改场景的范围为10
- scene.forward = vector(-1, -1, -1) # 保持视角不变
- scene.width = 300 # 修改场景的宽度为800
- scene.height = 300 # 修改场景的高度为800
- scene.center = vector(0, 0, 0) # 将场景中心位置调整为 (0, 0, 0)
-
- scene.width = 600
- scene.height = 600
-
- 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])
- last_print_time = time()
-
- try:
- while (self._iMU_isConnected==True):
- while self.arduino.inWaiting() == 0:
- pass
-
- try:
- data_packet = self.arduino.readline().decode('utf-8').strip()
- data = data_packet.split(',')
-
- if len(data) == column_limit:
- emg = float(data[0]) # emg sensor data
- q0 = float(data[1]) # qw
- q1 = float(data[2]) # qx
- q2 = float(data[3]) # qy
- q3 = float(data[4]) # qz
-
- 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))
- current_time = time()
-
- if current_time - last_print_time >= 0.01:
- print(f"EMG: {emg}, Roll: {roll}, Pitch: {pitch}, Yaw: {yaw}")
- k = vector(cos(yaw) * cos(pitch), sin(pitch), sin(yaw) * cos(pitch)) # this is the 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)
- # print(k,y,s,v)
-
- 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
- last_print_time = current_time
-
- else:
- print("Data packet does not match column limit")
-
- except ValueError:
- print("Error in data conversion")
- except UnicodeDecodeError:
- print("Error in decoding data")
-
- except KeyboardInterrupt:
- self.arduino.close()
- print("Disconnected from IMU")
def IMU_disconnect(self):
self._iMU_isConnected = False
@@ -224,6 +156,84 @@ class Interface:
self.treeview.insert("", "end", values=("Gesture 1", f"{emg_data[0]:.2f}"))
self.treeview.insert("", "end", values=("Gesture 2", f"{emg_data[1]:.2f}"))
+ def _initialise_IMU_3D_graph(self):
+
+ self.arduino = serial.Serial('COM6', 115200, timeout=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=((width / 300), (height / 200)))
+ 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',labelpad=0)
+
+ # 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.frame1)
+ self.canvas.draw()
+ self.canvas.get_tk_widget().pack(fill=tk.BOTH, expand=True)
+
+ self.update_display()
+
+ # Create a label for average EMG
+ # self.emg_label = tk.Label(self.frame1, text="Average EMG: 0", font=("Arial", 14))
+ #self.emg_label.pack(pady=10)
+
+
def _initialise_real_time_classification_graph(self):
# Create a figure and axis
@@ -255,7 +265,7 @@ class Interface:
self.line2, = self.ax.plot([], [], color='blue', label='Inner Wrist Muscle (Flexor Carpi Radialis)')
self.ax.legend(fontsize=9, loc='upper right')
-
+
# Embed the plot in the tkinter frame
@@ -263,6 +273,121 @@ class Interface:
self.canvas.draw()
self.canvas.get_tk_widget().pack(fill=tk.BOTH, expand=True)
+ def update_display(self):
+ 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 = 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))
+
+ # 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.popup.after(50, self.update_display)
+
@@ -270,6 +395,7 @@ class Interface:
if __name__ == "__main__":
windows = Interface()
windows.create_popup()
+ windows.popup.mainloop()