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:
self.frame4.grid_propagate(False)
label4 = tk.Label(self.frame4, text="Section 4")
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.emg_thread = threading.Thread(target=self._initialise_EMG_graph)
......@@ -217,8 +220,7 @@ class Window:
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 = 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:
......@@ -626,8 +628,11 @@ class trainingInterface:
height=2,font=("Helvetica", 12))
self.gameStartButton.place(relx=0.5, rely=0.5, anchor='center')
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.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
......@@ -898,6 +903,15 @@ class gameScreen:
self.a, self.b = self.load_Function()
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))
......@@ -908,6 +922,7 @@ class gameScreen:
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_Display()
def _decode(self, serial_data):
......@@ -963,76 +978,76 @@ class gameScreen:
self.EMG_display_id = self.root.after(1, self.EMG_Display)
def IMU_Display(self):
last_averageRoll = 0
last_averageyaw = 0
last_averagePitch = 0
averageroll = 0
averageyaw = 0
averagepitch = 0
last_print_time = time()
while True:
try:
while ((self.arduino.inWaiting() > 0) and
(self.transmitting == True)):
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 = 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 - last_print_time >= 0.01:
print(f"roll is: {roll}")
print(f"last roll is: {last_averageRoll}")
differ_roll = 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: {last_averageyaw}")
differ_yaw = 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 : roll {-yawAngle}")
if (differ_yaw) > 0:
self.send_command_to_unity(f"Command : back {yawAngle}")
last_print_time = current_time
last_averageRoll = roll
last_averageyaw = yaw
last_averagePitch = pitch
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(f"An error occurred: {e}")
print("Error:", str(e))
self.root.after(50, self.IMU_Display)
def load_Function(self,filename='trained.txt'):
try:
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment