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

Combined done without send to Unty

parent f7092b6f
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
integration/RP2040IMUEMG.py 0 → 100644
+ 154
− 0
View file @ 8b08e9ab
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
integration/Window2.py
+ 662
− 36
View file @ 8b08e9ab
...@@ -9,6 +9,9 @@ import serial.tools.list_ports ...@@ -9,6 +9,9 @@ import serial.tools.list_ports
from time import sleep from time import sleep
import math import math
import time import time
import socket
import os
from PIL import Image, ImageTk
class Window: class Window:
def __init__(self, root): def __init__(self, root):
...@@ -56,8 +59,8 @@ class Window: ...@@ -56,8 +59,8 @@ class Window:
self.emg_data_1 = [-1] * 41 self.emg_data_1 = [-1.0] * 41
self.emg_data_2 = [-1] * 41 self.emg_data_2 = [-1.0] * 41
#self.initial_IMU() #self.initial_IMU()
#self._initialise_EMG_graph() #self._initialise_EMG_graph()
...@@ -71,9 +74,9 @@ class Window: ...@@ -71,9 +74,9 @@ class Window:
def initial_IMU(self): def initial_IMU(self):
# Serial Port Setup # Serial Port Setup
if'COM7' in self.ports:#port maybe different on different laptop if'COM8' in self.ports:#port maybe different on different laptop
self.label2 = tk.Label(self.frame2, text="Port: COM7 ") self.label2 = tk.Label(self.frame2, text="Port: COM8 ")
self.label2.place(relx=0.35, rely=0.8, anchor='center') self.label2.place(relx=0.35, rely=0.8, anchor='center')
self.label1 = tk.Label(self.frame2, self.label1 = tk.Label(self.frame2,
text="click the Connect button to see the animation", text="click the Connect button to see the animation",
...@@ -185,9 +188,9 @@ class Window: ...@@ -185,9 +188,9 @@ class Window:
def _initialise_EMG_graph(self): def _initialise_EMG_graph(self):
if 'COM7' in self.ports:#port maybe different on different laptop if 'COM8' in self.ports:#port maybe different on different laptop
self.label2 = tk.Label(self.frame3, text="Port: COM7 ") self.label2 = tk.Label(self.frame3, text="Port: COM8 ")
self.label2.place(relx=0.23, rely=0.8, anchor='center') self.label2.place(relx=0.23, rely=0.8, anchor='center')
self.label1 = tk.Label(self.frame3, self.label1 = tk.Label(self.frame3,
text="click the Connect button to see the animation", text="click the Connect button to see the animation",
...@@ -263,6 +266,10 @@ class Window: ...@@ -263,6 +266,10 @@ class Window:
self.gesture_label = tk.Label(self.frame3, text=f"Gesture is :") self.gesture_label = tk.Label(self.frame3, text=f"Gesture is :")
self.gesture_label.config(font=("Arial", 12)) self.gesture_label.config(font=("Arial", 12))
self.gesture_label.place(relx=0.1, rely=0.6, anchor='w') self.gesture_label.place(relx=0.1, rely=0.6, anchor='w')
self.gesture_predict = tk.Label(self.frame3, text="")
self.gesture_predict.config(font=("Arial", 12))
self.gesture_predict.place(relx=0.2, rely=0.7, anchor='w')
self.a, self.b = self.load_Function()
...@@ -270,17 +277,17 @@ class Window: ...@@ -270,17 +277,17 @@ class Window:
def start_data_transmission(self): def start_data_transmission(self):
# Set the transmitting flag to True and start the update loop # Set the transmitting flag to True and start the update loop
if 'COM7' in self.ports: if 'COM8' in self.ports:
if self.arduino==None: if self.arduino==None:
self.arduino = serial.Serial('COM7', 115200) self.arduino = serial.Serial('COM8', 115200)
self.transmitting = True self.transmitting = True
self.update_display() self.update_display()
def start_EMG_data_transmission(self): def start_EMG_data_transmission(self):
# Set the transmitting flag to True and start the update loop # Set the transmitting flag to True and start the update loop
if 'COM7' in self.ports: if 'COM8' in self.ports:
if self.arduino==None: if self.arduino==None:
self.arduino = serial.Serial('COM7', 115200) self.arduino = serial.Serial('COM8', 115200)
self.EMG_transmitting = True self.EMG_transmitting = True
self.EMG_Display() self.EMG_Display()
...@@ -326,14 +333,41 @@ class Window: ...@@ -326,14 +333,41 @@ class Window:
row = cleandata.strip().split(',') row = cleandata.strip().split(',')
if len(row) == 6: if len(row) == 10:
splitPacket = cleandata.split(',') splitPacket = cleandata.split(',')
emg1 = float(splitPacket[0]) # emg sensor data
emg2 = float(splitPacket[1])
q0 = float(splitPacket[2]) # qw q0 = float(splitPacket[2]) # qw
q1 = float(splitPacket[3]) # qx q1 = float(splitPacket[3]) # qx
q2 = float(splitPacket[4]) # qy q2 = float(splitPacket[4]) # qy
q3 = float(splitPacket[5]) # qz q3 = float(splitPacket[5]) # qz
emg1 = float(splitPacket[0]) # First EMG sensor data
emg2 = float(splitPacket[1]) # Second EMG sensor data
print(f"emg1: {emg1}, emg2: {emg2}")
data = [emg1, emg2]
predictions = self.predict(data, self.a, self.b)
ges_predictions = None
if predictions is not None:
if predictions == -1:
ges_predictions = "Hand Open"
if predictions == 1:
ges_predictions = "Hand Close"
if predictions == 0:
ges_predictions = "Unknown"
self.gesture_predict.config(text=f"{ges_predictions}")
self.outer_EMG_Number.config(text=f"{emg1}")
self.inner_EMG_Number.config(text=f"{emg2}")
self.emg_data_1.append(emg1)
self.emg_data_1.pop(0)
self.emg_data_2.append(emg2)
self.emg_data_2.pop(0)
# Update the line data to shift the line from right to left
self.line1.set_data(range(len(self.emg_data_1)), self.emg_data_1)
self.line2.set_data(range(len(self.emg_data_2)), self.emg_data_2)
# Redraw the canvas
self.canvas1.draw() # Redraw the canvas
# Calculate Angles # Calculate Angles
roll = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)) roll = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))
...@@ -437,46 +471,333 @@ class Window: ...@@ -437,46 +471,333 @@ class Window:
print(f"An error occurred: {e}") print(f"An error occurred: {e}")
# Call update_display() again after 50 milliseconds # Call update_display() again after 50 milliseconds
self.update_display_id =self.root.after(50, self.update_display) self.update_display_id =self.root.after(1, self.update_display)
def EMG_Display(self): def EMG_Display(self):
data_collection_duration = 3 data_collection_duration = 3
if self.EMG_transmitting: if self.EMG_transmitting:
try: try:
#while ((self.arduino_EMG.inWaiting() > 0) and while self.arduino.inWaiting() > 0:
# (self.EMG_transmitting == True)): dataPacket = self.arduino.readline()
# data = self.arduino_EMG.readline() dataPacket = dataPacket.decode()
emg_data = self.collect_emg_data() cleandata = dataPacket.replace("\r\n", "")
if emg_data is not None: row = cleandata.strip().split(',')
print(f"EMG 1: {emg_data[0]} , EMG 2: {emg_data[1]}")
if len(row) == 10:
splitPacket = cleandata.split(',')
except Exception as e:
print(f"An error occurred: {e}")
# Call update_display() again after 50 milliseconds
self.EMG_display_id = self.root.after(1, self.EMG_Display)
def _decode(self, serial_data):
serial_string = serial_data.decode(errors="ignore")
adc_string_1 = ""
adc_string_2 = ""
self.adc_values = [0, 0]
if '\n' in serial_string:
# remove new line character
serial_string = serial_string.replace("\n", "")
if serial_string != '':
# Convert number to binary, placing 0s in empty spots
serial_string = format(int(serial_string, 10), "024b")
# Separate the input number from the data
for i0 in range(0, 12):
adc_string_1 += serial_string[i0]
for i0 in range(12, 24):
adc_string_2 += serial_string[i0]
self.adc_values[0] = int(adc_string_1, base=2)
self.adc_values[1] = int(adc_string_2, base=2)
return self.adc_values
def load_Function(self,filename='trained.txt'):
try:
with open(filename, 'r') as file:
lines = file.readlines()
if len(lines) < 2:
raise ValueError("File content is insufficient to read the vertical line parameters.")
a = float(lines[0].strip())
b = float(lines[1].strip())
print(f"a is {a}, b is {b}")
return a,b
except FileNotFoundError:
raise FileNotFoundError(f"The file {filename} does not exist.")
except ValueError as e:
raise ValueError(f"Error reading the file: {e}")
def predict(self, point,a,b):
"""判断点是否在垂直线的左侧或右侧"""
x, y = point
# 计算点的y值与垂直线的y值比较
line_y = a * x + b
if y < line_y:
return -1 # 点在垂直线的左侧
elif y > line_y:
return 1 # 点在垂直线的右侧
else:
return 0 # 点在垂直线上（可选）
class WelcomeWindow:
def __init__(self, root):
self.root = root
self.root.title("Welcome")
self.width = 1000
self.height = 600
screen_width = self.root.winfo_screenwidth()
screen_height = self.root.winfo_screenheight()
x = (screen_width // 2) - (self.width // 2)
y = (screen_height // 2) - (self.height // 2)
self.root.geometry(f"{self.width}x{self.height}+{x}+{y}")
# Configure the grid to be expandable
self.root.columnconfigure(0, weight=1)
self.root.columnconfigure(1, weight=1)
self.root.rowconfigure(0, weight=1)
self.root.rowconfigure(1, weight=1)
try:
self.bg_image = Image.open("backGrond.jpg")
print("Image loaded successfully")
self.bg_image = self.bg_image.resize((self.width, self.height), Image.Resampling.LANCZOS)
self.bg_photo = ImageTk.PhotoImage(self.bg_image)
self.bg_label = tk.Label(self.root, image=self.bg_photo)
self.bg_label.place(x=0, y=0, relwidth=1, relheight=1)
except Exception as e:
print(f"Error loading image: {e}")
#self.frame1 = tk.Frame(self.root, borderwidth=1, relief="solid", width=self.width, height=self.height)
#self.frame1.grid(row=0, column=0, columnspan=2, rowspan=2, sticky="nsew")
#self.button1 = tk.Button(self.frame1, text="Start", command=self.startButton)
#self.button1.place(relx=0.5, rely=0.8, anchor='center')
self.button1 = tk.Button(self.root, text="Start", command=self.startButton,width=18,
height=2, font=("Helvetica", 15))
self.button1.place(relx=0.8, rely=0.8, anchor='center') # Position the button relative to the root window
def startButton(self):
self.root.destroy() # Close the welcome window
new_root = tk.Tk()
app = trainingInterface(new_root)
new_root.mainloop()
class trainingInterface:
def __init__(self, root):
self.root = root
self.root.title("preparation Interface")
self.width = 1000
self.height = 600
self.width = 1000
self.height = 600
screen_width = self.root.winfo_screenwidth()
screen_height = self.root.winfo_screenheight()
x = (screen_width // 2) - (self.width // 2)
y = (screen_height // 2) - (self.height // 2)
self.root.geometry(f"{self.width}x{self.height}+{x}+{y}")
self.ports = [port.device for port in serial.tools.list_ports.comports()]
# Configure the grid to be expandable
self.root.columnconfigure(0, weight=1)
self.root.columnconfigure(1, weight=1)
self.root.rowconfigure(0, weight=1)
self.root.rowconfigure(1, weight=1)
# Create a frame
self.frame1 = tk.Frame(self.root, borderwidth=1, relief="solid", width=self.width, height=(self.height *2/ 3))
self.frame1.grid(row=0, column=0, padx=10, pady=10, sticky="nsew")
self.frame2 = tk.Frame(self.root, borderwidth=1, relief="solid", width=self.width, height=self.height *1/ 3)
self.frame2.grid(row=1, column=0, padx=10, pady=10, sticky="nsew")
self.initialEMGTraining()
if 'COM8' in self.ports:
self.emg_data_1 = [-1.0] * 41
self.emg_data_2 = [-1.0] * 41
self.savingData=[]
self.openHandButton=tk.Button(self.frame2,text="Hand Open",command=self.EMG_connect_HandOpen,width=15, height=2,font=("Helvetica", 12))
self.openHandButton.place(relx=0.3, rely=0.3, anchor='center')
self.handCloseButton=tk.Button(self.frame2,text="Hand Close",command=self.handCloseButton,width=15, height=2,font=("Helvetica", 12))
self.handCloseButton.place(relx=0.7, rely=0.3, anchor='center')
self.gameStartButton = tk.Button(self.frame2, text="Start", command=self.startButton, width=15,
height=2,font=("Helvetica", 12))
self.gameStartButton.place(relx=0.5, rely=0.5, anchor='center')
if 'COM8' not in self.ports:
self.label=tk.Label(self.frame2, text="No EMG device found, Please check the hardware connection",font=("Helvetica", 15))
self.label.place(relx=0.5, rely=0.3, anchor='center')
self.gameStartButton = tk.Button(self.frame2, text="Start", command=self.startButton, width=15,
height=2, font=("Helvetica", 12))
self.gameStartButton.place(relx=0.5, rely=0.5, anchor='center')
def startButton(self):
self.root.destroy() # Close the welcome window
new_root = tk.Tk()
app = Window(new_root)
new_root.mainloop()
def EMG_connect_HandOpen(self):
self.arduino_EMG = serial.Serial('COM8', 9600, timeout=1)
gesture = "handOpen"
self.start_countdown(11)
self.displayAndsaveDate()
def handCloseButton(self):
self.arduino_EMG = serial.Serial('COM8', 9600, timeout=1)
gesture = "handOpen"
self.start_countdown_close(11)
self.displayAndsaveDate()
def EMG_disconnect(self):
if self.arduino_EMG is not None:
self.arduino_EMG.close()
self.arduino_EMG = None
def start_countdown(self, count):
if count > 0:
self.startSave=True
if count<11:
self.openHandButton.config(text=str(count))
self.frame2.after(1000, self.start_countdown, count - 1)
else:
self.openHandButton.config(text="Hand Open")
self.startSave = False
self.savedDataOpen = []
for i in self.savingData:
self.savedDataOpen.append(i)
print(f"open: {self.savedDataOpen}")
self.savingData.clear()
self.EMG_disconnect()
def start_countdown_close(self, count):
if count > 0:
self.startSave=True
if count<11:
self.handCloseButton.config(text=str(count))
self.frame2.after(1000, self.start_countdown_close, count - 1)
else:
self.handCloseButton.config(text="Hand Close")
self.startSave = False
self.savedDataClose=[]
for i in self.savingData:
self.savedDataClose.append(i)
self.savingData.clear()
print(f"close:{self.savedDataClose}")
self.EMG_disconnect()
self.trainData()
def displayAndsaveDate(self):
if self.startSave:
try:
while ((self.arduino_EMG.inWaiting() > 0) ):
dataPacket = self.arduino_EMG.readline()
dataPacket = dataPacket.decode()
cleandata = dataPacket.replace("\r\n", "")
row = cleandata.strip().split(',')
if len(row) == 10:
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}")
self.emg_data_1.append(emg1)
self.emg_data_1.pop(0)
self.emg_data_2.append(emg2)
self.emg_data_2.pop(0)
if self.startSave==True:
self.savingData.append([emg1,emg2])
print(len(self.savingData))
# Append the new data to the lists
if self.EMG_transmitting:
self.outer_EMG_Number.config(text=f"{emg_data[0]}")
self.inner_EMG_Number.config(text=f"{emg_data[1]}")
self.emg_data_1.append(emg_data[0])
self.emg_data_1.pop(0)
self.emg_data_2.append(emg_data[1])
self.emg_data_2.pop(0)
# Update the line data to shift the line from right to left # Update the line data to shift the line from right to left
self.line1.set_data(range(len(self.emg_data_1)), self.emg_data_1) self.line1.set_data(range(len(self.emg_data_1)), self.emg_data_1)
self.line2.set_data(range(len(self.emg_data_2)), self.emg_data_2) self.line2.set_data(range(len(self.emg_data_2)), self.emg_data_2)
# Redraw the canvas # Redraw the canvas
self.canvas1.draw() # Redraw the canvas self.canvas1.draw() # Redraw the canvas
except Exception as e: except Exception as e:
print(f"An error occurred: {e}") print(f"An error occurred: {e}")
self.EMG_display_id = self.root.after(1, self.displayAndsaveDate)
# Call update_display() again after 50 milliseconds
self.EMG_display_id=self.root.after(50, self.EMG_Display)
def initialEMGTraining(self):
self.EMG_transmitting = False
fig = Figure(figsize=(self.frame1.winfo_width() / 100, self.frame1.winfo_height() / 100))
self.ax1 = fig.add_subplot(111)
self.ax1.set_title("Electromyography Envelope", fontsize=14, pad=0)
self.ax1.set_xlim(0, 5)
self.ax1.set_ylim(0, 5)
self.ax1.set_xlabel("Sample (20 samples per second)", fontsize=8, labelpad=-2)
self.ax1.set_ylabel("Magnitude", labelpad=0)
self.ax1.set_xticks(np.arange(0, 41, 8))
self.ax1.set_yticks(np.arange(0, 1001, 200))
for x_tick in self.ax1.get_xticks():
self.ax1.axvline(x_tick, color='gray', linestyle='--', linewidth=0.5)
for y_tick in self.ax1.get_yticks():
self.ax1.axhline(y_tick, color='gray', linestyle='--', linewidth=0.5)
self.line1, = self.ax1.plot([], [], color='red', label='Outer Wrist Muscle (Extensor Carpi Ulnaris)')
self.line2, = self.ax1.plot([], [], color='blue', label='Inner Wrist Muscle (Flexor Carpi Radialis)')
self.ax1.legend(fontsize=9, loc='upper right')
# Embed the plot in the tkinter frame
self.canvas1 = FigureCanvasTkAgg(fig, master=self.frame1)
self.canvas1.draw()
self.canvas1.get_tk_widget().pack(fill=tk.BOTH, expand=True)
# Bind the resizing event to the figure update
self.frame1.bind("<Configure>", self.on_frame_resize)
def on_frame_resize(self, event):
width = self.frame1.winfo_width()
height = self.frame1.winfo_height()
self.canvas1.get_tk_widget().config(width=width, height=height)
self.canvas1.draw()
'''
Train Data
'''
def trainData(self):
# 删除文件 'trained.txt'，如果存在
if os.path.exists('trained.txt'):
os.remove('trained.txt')
if (self.savedDataClose != []) and (self.savedDataOpen != []):
vertical_line = Algorithm(self.savedDataClose, self.savedDataOpen)
print(f"垂直线方程: y = {vertical_line.a}x + {vertical_line.b}")
# 创建新的 'trained.txt' 文件并写入内容
with open('trained.txt', 'w') as file:
file.write(f"{vertical_line.a}\n")
file.write(f"{vertical_line.b}\n")
test_points = [[2, 5], [3, 3], [4, 1]]
for point in test_points:
position = vertical_line.predict(point)
print(f"点 {point} 在垂直线的 {'左侧' if position == -1 else '右侧' if position == 1 else '上面/下面'}")
return vertical_line
def _decode(self, serial_data): def _decode(self, serial_data):
serial_string = serial_data.decode(errors="ignore") serial_string = serial_data.decode(errors="ignore")
...@@ -501,7 +822,312 @@ class Window: ...@@ -501,7 +822,312 @@ class Window:
return self.adc_values return self.adc_values
class Algorithm:
def __init__(self, list1, list2):
self.a, self.b = self.calculate_line_equation(list1, list2)
def calculate_average(self, lst):
"""计算列表中点的平均坐标"""
n = len(lst)
if n == 0:
return (0, 0)
sum_x = sum(point[0] for point in lst)
sum_y = sum(point[1] for point in lst)
return (sum_x / n, sum_y / n)
def calculate_line_equation(self, list1, list2):
"""计算垂直线方程 y = ax + b"""
avg1 = self.calculate_average(list1)
avg2 = self.calculate_average(list2)
x1, y1 = avg1
x2, y2 = avg2
# 计算斜率
if x1 == x2:
raise ValueError("垂直线的斜率是未定义的，因为两个点在同一垂直线上。")
slope = (y2 - y1) / (x2 - x1)
# 垂直线的斜率是原斜率的负倒数
perpendicular_slope = -1 / slope
# 使用点斜式方程 y - y1 = m(x - x1) 转换为 y = ax + b 的形式
a = perpendicular_slope
b = y1 - a * x1
return a, b
def predict(self, point):
"""判断点是否在垂直线的左侧或右侧"""
x, y = point
# 计算点的y值与垂直线的y值比较
line_y = self.a * x + self.b
if y < line_y:
return -1 # 点在垂直线的左侧
elif y > line_y:
return 1 # 点在垂直线的右侧
else:
return 0 # 点在垂直线上（可选）
class gameScreen:
def __init__(self, root):
self.root = root
self.root.title("game Interface")
self.width = 1000
self.height = 600
self.width = 1000
self.height = 600
screen_width = self.root.winfo_screenwidth()
screen_height = self.root.winfo_screenheight()
x = (screen_width // 2) - (self.width // 2)
y = (screen_height // 2) - (self.height // 2)
self.root.geometry(f"{self.width}x{self.height}+{x}+{y}")
self.ports = [port.device for port in serial.tools.list_ports.comports()]
# Configure the grid to be expandable
self.root.columnconfigure(0, weight=1)
self.root.columnconfigure(1, weight=1)
self.root.rowconfigure(0, weight=1)
self.root.rowconfigure(1, weight=1)
# Create a frame
self.frame1 = tk.Frame(self.root, borderwidth=1, relief="solid", width=self.width, height=(self.height * 1 / 2))
self.frame1.grid(row=0, column=0, padx=10, pady=10, sticky="nsew")
self.frame2 = tk.Frame(self.root, borderwidth=1, relief="solid", width=self.width, height=self.height * 1 / 2)
self.frame2.grid(row=1, column=0, padx=10, pady=10, sticky="nsew")
if 'COM5' in self.ports :
self.arduino_EMG = serial.Serial('COM5', 9600, timeout=1)
self.outer_EMG_label = tk.Label(self.frame2, text=f"EMG for Extensor Carpi Ulnaris is :")
self.outer_EMG_label.config(font=("Arial", 12))
self.outer_EMG_label.place(relx=0.1, rely=0.2, anchor='w')
self.outer_EMG_Number = tk.Label(self.frame2, text="", fg="red")
self.outer_EMG_Number.config(font=("Arial", 12))
self.outer_EMG_Number.place(relx=0.2, rely=0.3, anchor='w')
self.inner_EMG_label = tk.Label(self.frame2, text=f"EMG for Flexor Carpi Radialis is :")
self.inner_EMG_label.config(font=("Arial", 12))
self.inner_EMG_label.place(relx=0.1, rely=0.4, anchor='w')
self.inner_EMG_Number = tk.Label(self.frame2, text="", fg="blue")
self.inner_EMG_Number.config(font=("Arial", 12))
self.inner_EMG_Number.place(relx=0.2, rely=0.5, anchor='w')
self.gesture_label = tk.Label(self.frame2, text=f"Gesture is :")
self.gesture_label.config(font=("Arial", 12))
self.gesture_label.place(relx=0.1, rely=0.6, anchor='w')
self.gesture_predict = tk.Label(self.frame2, text="")
self.gesture_predict.config(font=("Arial", 12))
self.gesture_predict.place(relx=0.2, rely=0.7, anchor='w')
self.a, self.b = self.load_Function()
self.emg_thread = threading.Thread(target=self.EMG_Display)
self.emg_thread.start()
#self.EMG_Display()
if 'COM6' in self.ports:
self.column_limit = 9
self.last_averageRoll = 0
self.last_averageyaw = 0
self.last_averagePitch = 0
self.averageroll = 0
self.averageyaw = 0
self.averagepitch = 0
self.last_print_time = time()
self.arduino = serial.Serial('COM6', 115200)
self.roll_label = tk.Label(self.frame1, text="roll is : ")
self.roll_label.config(font=("Arial", 12))
self.roll_label.place(relx=0.2, rely=0.3, anchor='w')
self.pitch_label = tk.Label(self.frame1, text="pitch is : ")
self.pitch_label.config(font=("Arial", 12))
self.pitch_label.place(relx=0.2, rely=0.4, anchor='w')
self.yaw_label = tk.Label(self.frame1, text="yaw is : ")
self.yaw_label.config(font=("Arial", 12))
self.yaw_label.place(relx=0.2, rely=0.5, anchor='w')
self.imu_thread = threading.Thread(target=self.IMU_Display)
self.imu_thread.start()
#self.IMU_Display()
def _decode(self, serial_data):
serial_string = serial_data.decode(errors="ignore")
adc_string_1 = ""
adc_string_2 = ""
self.adc_values = [0, 0]
if '\n' in serial_string:
# remove new line character
serial_string = serial_string.replace("\n", "")
if serial_string != '':
# Convert number to binary, placing 0s in empty spots
serial_string = format(int(serial_string, 10), "024b")
# Separate the input number from the data
for i0 in range(0, 12):
adc_string_1 += serial_string[i0]
for i0 in range(12, 24):
adc_string_2 += serial_string[i0]
self.adc_values[0] = int(adc_string_1, base=2)
self.adc_values[1] = int(adc_string_2, base=2)
return self.adc_values
def EMG_Display(self):
try:
while (self.arduino_EMG.inWaiting() > 0):
data = self.arduino_EMG.readline()
emg_data = self._decode(data)
if emg_data is not None:
print(f"EMG 1: {emg_data[0]} , EMG 2: {emg_data[1]}")
self.outer_EMG_Number.config(text=f"{emg_data[0]}")
self.inner_EMG_Number.config(text=f"{emg_data[1]}")
data = [emg_data[0], emg_data[1]]
predictions = self.predict(data, self.a, self.b)
ges_predictions = None
if predictions is not None:
if predictions == -1:
ges_predictions = "Hand Open"
if predictions == 1:
ges_predictions = "Hand Close"
if predictions == 0:
ges_predictions = "Unknown"
self.gesture_predict.config(text=f"{ges_predictions}")
self.send_command_to_unity(f"Hand :{ges_predictions}")
except Exception as e:
print(f"An error occurred: {e}")
# Call update_display() again after 50 milliseconds
self.EMG_display_id = self.root.after(1, self.EMG_Display)
def IMU_Display(self):
while True:
try:
while self.arduino.inWaiting() == 0:
pass
dataPacket = self.arduino.readline().decode()
cleandata = dataPacket.replace("\r\n", "")
row = cleandata.strip().split(',')
if len(row) == self.column_limit:
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
# Callibration Statuses
aC = float(splitPacket[5]) # Accelerometer
gC = float(splitPacket[6]) # Gyroscope
mC = float(splitPacket[7]) # Magnetometer
sC = float(splitPacket[8]) # Whole System
# calculate angle
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))
self.roll_label.config(text="roll is : " + str(roll))
self.pitch_label.config(text="pitch is : " + str(pitch))
self.yaw_label.config(text="yaw is : " + str(yaw))
current_time = time()
if current_time - self.last_print_time >= 0.01:
print(f"roll is: {roll}")
print(f"last roll is: {self.last_averageRoll}")
differ_roll = self.last_averageRoll - roll
print(f"differ roll is: {differ_roll}")
CalculatedAngle = differ_roll * 3000 / 2.5
print(f"CalculatedAngle is: {CalculatedAngle}")
if (differ_roll) > 0:
self.send_command_to_unity(f"Command : down {CalculatedAngle}")
if (differ_roll) < 0:
self.send_command_to_unity(f"Command : up {-CalculatedAngle}")
if (yaw < 0):
yaw = -yaw
print(f"yaw is: {yaw}")
print(f"last yaw is: {self.last_averageyaw}")
differ_yaw = self.last_averageyaw - yaw
print(f"differ yaw is: {differ_yaw}")
yawAngle = differ_yaw * 90 / 2
print(f"yawAngle is: {yawAngle}")
if (differ_yaw) < 0:
self.send_command_to_unity(f"Command : back {-yawAngle}")
if (differ_yaw) > 0:
self.send_command_to_unity(f"Command : roll {yawAngle}")
self.last_print_time = current_time
self.last_averageRoll = roll
self.last_averageyaw = yaw
self.last_averagePitch = pitch
except Exception as e:
print("Error:", str(e))
def load_Function(self,filename='trained.txt'):
try:
with open(filename, 'r') as file:
lines = file.readlines()
if len(lines) < 2:
raise ValueError("File content is insufficient to read the vertical line parameters.")
a = float(lines[0].strip())
b = float(lines[1].strip())
print(f"a is {a}, b is {b}")
return a,b
except FileNotFoundError:
raise FileNotFoundError(f"The file {filename} does not exist.")
except ValueError as e:
raise ValueError(f"Error reading the file: {e}")
def predict(self, point, a, b):
"""判断点是否在垂直线的左侧或右侧"""
x, y = point
# 计算点的y值与垂直线的y值比较
line_y = a * x + b
if y < line_y:
return -1 # 点在垂直线的左侧
elif y > line_y:
return 1 # 点在垂直线的右侧
else:
return 0 # 点在垂直线上（可选）
def send_command_to_unity(self,command):
host = '127.0.0.1' # Unity服务器的IP地址
port = 65432 # Unity服务器监听的端口
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
s.connect((host, port))
s.sendall(command.encode())
response = s.recv(1024)
print('Received', repr(response))
if __name__ == "__main__":
root1 = tk.Tk()
appWelcome = WelcomeWindow(root1)
root1.mainloop()
'''
if __name__ == "__main__": if __name__ == "__main__":
root = tk.Tk() root = tk.Tk()
app = Window(root) app = Window(root)
root.mainloop() root.mainloop()
\ No newline at end of file
'''
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment