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

IMU done

parent aaef0aeb
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 84 additions and 69 deletions
integration/Window.py
+ 84
− 69
View file @ 6eaaa9b5
...@@ -59,6 +59,9 @@ class Window: ...@@ -59,6 +59,9 @@ class Window:
self.frame4.grid_propagate(False) self.frame4.grid_propagate(False)
label4 = tk.Label(self.frame4, text="Section 4") label4 = tk.Label(self.frame4, text="Section 4")
label4.place(relx=0.5, rely=0.5, anchor='center') label4.place(relx=0.5, rely=0.5, anchor='center')
self.start_button = tk.Button(self.frame2, text="Game Start", command=self.game_Start, width=15, height=1,
font=("Helvetica", 12))
self.start_button.place(relx=0.7, rely=0.15, anchor='center')
self.imu_thread = threading.Thread(target=self.initial_IMU) self.imu_thread = threading.Thread(target=self.initial_IMU)
self.emg_thread = threading.Thread(target=self._initialise_EMG_graph) self.emg_thread = threading.Thread(target=self._initialise_EMG_graph)
...@@ -217,8 +220,7 @@ class Window: ...@@ -217,8 +220,7 @@ class Window:
self.start_button = tk.Button(self.frame3, text="Disconnect", command=self.EMG_disconnect) self.start_button = tk.Button(self.frame3, text="Disconnect", command=self.EMG_disconnect)
self.start_button.place(relx=0.7, rely=0.8, anchor='center') self.start_button.place(relx=0.7, rely=0.8, anchor='center')
self.start_button = tk.Button(self.frame2, text="Game Start", command=self.game_Start, width = 15, height = 1, font = ("Helvetica", 12))
self.start_button.place(relx=0.7, rely=0.15, anchor='center')
else: else:
...@@ -626,8 +628,11 @@ class trainingInterface: ...@@ -626,8 +628,11 @@ class trainingInterface:
height=2,font=("Helvetica", 12)) height=2,font=("Helvetica", 12))
self.gameStartButton.place(relx=0.5, rely=0.5, anchor='center') self.gameStartButton.place(relx=0.5, rely=0.5, anchor='center')
if 'COM5' not in self.ports: if 'COM5' not in self.ports:
self.label=tk.Label(self.frame2, text="No ports found, Please check the hardware connection",font=("Helvetica", 15)) 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.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): def startButton(self):
self.root.destroy() # Close the welcome window self.root.destroy() # Close the welcome window
...@@ -898,6 +903,15 @@ class gameScreen: ...@@ -898,6 +903,15 @@ class gameScreen:
self.a, self.b = self.load_Function() self.a, self.b = self.load_Function()
self.EMG_Display() self.EMG_Display()
if 'COM6' in self.ports: 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.arduino = serial.Serial('COM6', 115200)
self.roll_label = tk.Label(self.frame1, text="roll is : ") self.roll_label = tk.Label(self.frame1, text="roll is : ")
self.roll_label.config(font=("Arial", 12)) self.roll_label.config(font=("Arial", 12))
...@@ -908,6 +922,7 @@ class gameScreen: ...@@ -908,6 +922,7 @@ class gameScreen:
self.yaw_label = tk.Label(self.frame1, text="yaw is : ") self.yaw_label = tk.Label(self.frame1, text="yaw is : ")
self.yaw_label.config(font=("Arial", 12)) self.yaw_label.config(font=("Arial", 12))
self.yaw_label.place(relx=0.2, rely=0.5, anchor='w') self.yaw_label.place(relx=0.2, rely=0.5, anchor='w')
self.IMU_Display()
def _decode(self, serial_data): def _decode(self, serial_data):
...@@ -963,76 +978,76 @@ class gameScreen: ...@@ -963,76 +978,76 @@ class gameScreen:
self.EMG_display_id = self.root.after(1, self.EMG_Display) self.EMG_display_id = self.root.after(1, self.EMG_Display)
def IMU_Display(self): def IMU_Display(self):
last_averageRoll = 0 while True:
last_averageyaw = 0
last_averagePitch = 0
averageroll = 0
averageyaw = 0
averagepitch = 0
last_print_time = time()
try: try:
while ((self.arduino.inWaiting() > 0) and while self.arduino.inWaiting() == 0:
(self.transmitting == True)): pass
dataPacket = self.arduino.readline()
dataPacket = dataPacket.decode() dataPacket = self.arduino.readline().decode()
cleandata = dataPacket.replace("\r\n", "") cleandata = dataPacket.replace("\r\n", "")
row = cleandata.strip().split(',') row = cleandata.strip().split(',')
if len(row) == 9: if len(row) == self.column_limit:
splitPacket = cleandata.split(',') splitPacket = cleandata.split(',')
emg = float(splitPacket[0]) # EMG sensor data emg = float(splitPacket[0]) # emg sensor data
q0 = float(splitPacket[1]) # qw q0 = float(splitPacket[1]) # qw
q1 = float(splitPacket[2]) # qx q1 = float(splitPacket[2]) # qx
q2 = float(splitPacket[3]) # qy q2 = float(splitPacket[3]) # qy
q3 = float(splitPacket[4]) # qz q3 = float(splitPacket[4]) # qz
# Calculate Angles # Callibration Statuses
roll = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2)) aC = float(splitPacket[5]) # Accelerometer
pitch = -math.asin(2 * (q0 * q2 - q3 * q1)) gC = float(splitPacket[6]) # Gyroscope
yaw = -math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)) mC = float(splitPacket[7]) # Magnetometer
sC = float(splitPacket[8]) # Whole System
self.roll_label.config(text="roll is : " + str(roll))
self.pitch_label.config(text="pitch is : " + str(pitch)) # calculate angle
self.yaw_label.config(text="yaw is : " + str(yaw)) roll = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))
pitch = -math.asin(2 * (q0 * q2 - q3 * q1))
current_time = time() yaw = -math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3))
if current_time - last_print_time >= 0.01: self.roll_label.config(text="roll is : " + str(roll))
print(f"roll is: {roll}") self.pitch_label.config(text="pitch is : " + str(pitch))
print(f"last roll is: {last_averageRoll}") self.yaw_label.config(text="yaw is : " + str(yaw))
differ_roll = last_averageRoll - roll
print(f"differ roll is: {differ_roll}") current_time = time()
CalculatedAngle = differ_roll * 3000 / 2.5
print(f"CalculatedAngle is: {CalculatedAngle}") if current_time - self.last_print_time >= 0.01:
if (differ_roll) > 0:
self.send_command_to_unity(f"Command : down {CalculatedAngle}") print(f"roll is: {roll}")
if (differ_roll) < 0: print(f"last roll is: {self.last_averageRoll}")
self.send_command_to_unity(f"Command : up {-CalculatedAngle}") differ_roll = self.last_averageRoll - roll
print(f"differ roll is: {differ_roll}")
if (yaw < 0): CalculatedAngle = differ_roll * 3000 / 2.5
yaw = -yaw print(f"CalculatedAngle is: {CalculatedAngle}")
if (differ_roll) > 0:
print(f"yaw is: {yaw}") self.send_command_to_unity(f"Command : down {CalculatedAngle}")
print(f"last yaw is: {last_averageyaw}") if (differ_roll) < 0:
differ_yaw = last_averageyaw - yaw self.send_command_to_unity(f"Command : up {-CalculatedAngle}")
print(f"differ yaw is: {differ_yaw}")
yawAngle = differ_yaw * 90 / 2 if (yaw < 0):
print(f"yawAngle is: {yawAngle}") yaw = -yaw
if (differ_yaw) < 0:
self.send_command_to_unity(f"Command : roll {-yawAngle}") print(f"yaw is: {yaw}")
if (differ_yaw) > 0: print(f"last yaw is: {self.last_averageyaw}")
self.send_command_to_unity(f"Command : back {yawAngle}") differ_yaw = self.last_averageyaw - yaw
last_print_time = current_time print(f"differ yaw is: {differ_yaw}")
last_averageRoll = roll yawAngle = differ_yaw * 90 / 2
last_averageyaw = yaw print(f"yawAngle is: {yawAngle}")
last_averagePitch = pitch 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: except Exception as e:
print(f"An error occurred: {e}") print("Error:", str(e))
self.root.after(50, self.IMU_Display)
def load_Function(self,filename='trained.txt'): def load_Function(self,filename='trained.txt'):
try: try:
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment