Skip to content
Snippets Groups Projects
Commit cb91d80e authored by ym13n22's avatar ym13n22
Browse files

half EMG

parent 55d51c78
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
integration/integrate.py
+ 219
− 93
View file @ cb91d80e
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()
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment