diff --git a/integration/RP2040IMUEMG.py b/integration/RP2040IMUEMG.py
new file mode 100644
index 0000000000000000000000000000000000000000..6c9835ab22a54db0bace9c6ad44a81538f86fd19
--- /dev/null
+++ b/integration/RP2040IMUEMG.py
@@ -0,0 +1,154 @@
+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
diff --git a/integration/Window2.py b/integration/Window2.py
index 084839745f9fa5b4812136d3585d41257ebf1440..c32f166f8344db94a8131e6d0baa9c97391cd056 100644
--- a/integration/Window2.py
+++ b/integration/Window2.py
@@ -9,6 +9,9 @@ import serial.tools.list_ports
from time import sleep
import math
import time
+import socket
+import os
+from PIL import Image, ImageTk
class Window:
def __init__(self, root):
@@ -56,8 +59,8 @@ class Window:
- self.emg_data_1 = [-1] * 41
- self.emg_data_2 = [-1] * 41
+ self.emg_data_1 = [-1.0] * 41
+ self.emg_data_2 = [-1.0] * 41
#self.initial_IMU()
#self._initialise_EMG_graph()
@@ -71,9 +74,9 @@ class Window:
def initial_IMU(self):
# 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.label1 = tk.Label(self.frame2,
text="click the Connect button to see the animation",
@@ -185,9 +188,9 @@ class Window:
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.label1 = tk.Label(self.frame3,
text="click the Connect button to see the animation",
@@ -263,6 +266,10 @@ class Window:
self.gesture_label = tk.Label(self.frame3, 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.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:
def start_data_transmission(self):
# 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:
- self.arduino = serial.Serial('COM7', 115200)
+ self.arduino = serial.Serial('COM8', 115200)
self.transmitting = True
self.update_display()
def start_EMG_data_transmission(self):
# 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:
- self.arduino = serial.Serial('COM7', 115200)
+ self.arduino = serial.Serial('COM8', 115200)
self.EMG_transmitting = True
self.EMG_Display()
@@ -326,14 +333,41 @@ class Window:
row = cleandata.strip().split(',')
- if len(row) == 6:
+ if len(row) == 10:
splitPacket = cleandata.split(',')
- emg1 = float(splitPacket[0]) # emg sensor data
- emg2 = float(splitPacket[1])
q0 = float(splitPacket[2]) # qw
q1 = float(splitPacket[3]) # qx
q2 = float(splitPacket[4]) # qy
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
roll = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))
@@ -437,46 +471,333 @@ class Window:
print(f"An error occurred: {e}")
# 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):
data_collection_duration = 3
if self.EMG_transmitting:
try:
- #while ((self.arduino_EMG.inWaiting() > 0) and
- # (self.EMG_transmitting == True)):
- # data = self.arduino_EMG.readline()
- emg_data = self.collect_emg_data()
- if emg_data is not None:
- print(f"EMG 1: {emg_data[0]} , EMG 2: {emg_data[1]}")
+ while self.arduino.inWaiting() > 0:
+ dataPacket = self.arduino.readline()
+ dataPacket = dataPacket.decode()
+ cleandata = dataPacket.replace("\r\n", "")
+ row = cleandata.strip().split(',')
+
+ 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
- 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.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
+ self.canvas1.draw() # Redraw the canvas
- except Exception as e:
- print(f"An error occurred: {e}")
+ except Exception as 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):
serial_string = serial_data.decode(errors="ignore")
@@ -501,7 +822,312 @@ class Window:
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__":
root = tk.Tk()
app = Window(root)
- root.mainloop()
\ No newline at end of file
+ root.mainloop()
+
+'''