diff --git a/EMG/EMG.py b/EMG/EMG.py
new file mode 100644
index 0000000000000000000000000000000000000000..bddf4642ebfd11e3f6d66bda70f79e04f781866c
--- /dev/null
+++ b/EMG/EMG.py
@@ -0,0 +1,72 @@
+import serial
+import time
+
+# 指定串口端口和波特率（根据传感器手册确认波特率）
+port = 'COM5' # 例如，Windows上的串口
+baudrate = 9600 # 示例波特率，需根据实际情况修改
+
+# 打开串口连接
+ser = serial.Serial(port, baudrate, timeout=1)
+
+def _decode(serial_data):
+ """
+ 解码串口数据，返回两个ADC值
+ """
+ serial_string = serial_data.decode(errors="ignore")
+ adc_string_1 = ""
+ adc_string_2 = ""
+ 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 i in range(0, 12):
+ adc_string_1 += serial_string[i]
+ for i in range(12, 24):
+ adc_string_2 += serial_string[i]
+
+ adc_values[0] = int(adc_string_1, base=2)
+ adc_values[1] = int(adc_string_2, base=2)
+
+ return adc_values
+ return None
+
+def read_emg_data():
+ """
+ 从EMG传感器读取数据并返回两个输出值
+ """
+ try:
+ # 读取一行数据
+ line = ser.readline()
+ emg_values = _decode(line)
+ if emg_values:
+ return emg_values
+ except Exception as e:
+ print(f"Error reading data: {e}")
+ return None
+
+def main():
+ print("Starting EMG data collection...")
+ try:
+ while True:
+ emg_values = read_emg_data()
+ if emg_values is not None:
+ print(f"EMG Value 1: {emg_values[0]}, EMG Value 2: {emg_values[1]}")
+ # 在这里添加数据分析代码
+ # 例如，计算平均值，滤波等
+ else:
+ print("Failed to read EMG data")
+ time.sleep(0.1) # 控制读取频率
+ except KeyboardInterrupt:
+ print("Stopping EMG data collection.")
+ finally:
+ # 关闭串口连接
+ ser.close()
+
+if __name__ == "__main__":
+ main()
+
diff --git a/IMU/code/.idea/.gitignore b/IMU/code/.idea/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..13566b81b018ad684f3a35fee301741b2734c8f4
--- /dev/null
+++ b/IMU/code/.idea/.gitignore
@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
diff --git a/IMU/code/.idea/code.iml b/IMU/code/.idea/code.iml
new file mode 100644
index 0000000000000000000000000000000000000000..d0876a78d06ac03b5d78c8dcdb95570281c6f1d6
--- /dev/null
+++ b/IMU/code/.idea/code.iml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+ <component name="NewModuleRootManager">
+ <content url="file://$MODULE_DIR$" />
+ <orderEntry type="inheritedJdk" />
+ <orderEntry type="sourceFolder" forTests="false" />
+ </component>
+</module>
\ No newline at end of file
diff --git a/IMU/code/.idea/inspectionProfiles/profiles_settings.xml b/IMU/code/.idea/inspectionProfiles/profiles_settings.xml
new file mode 100644
index 0000000000000000000000000000000000000000..105ce2da2d6447d11dfe32bfb846c3d5b199fc99
--- /dev/null
+++ b/IMU/code/.idea/inspectionProfiles/profiles_settings.xml
@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+ <settings>
+ <option name="USE_PROJECT_PROFILE" value="false" />
+ <version value="1.0" />
+ </settings>
+</component>
\ No newline at end of file
diff --git a/IMU/code/.idea/misc.xml b/IMU/code/.idea/misc.xml
new file mode 100644
index 0000000000000000000000000000000000000000..892c1a2a9792ef062c0f1b5613273539ec24c82a
--- /dev/null
+++ b/IMU/code/.idea/misc.xml
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+ <component name="Black">
+ <option name="sdkName" value="Python 3.10 (pythonCommandForUnity)" />
+ </component>
+ <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.10 (pythonCommandForUnity)" project-jdk-type="Python SDK" />
+</project>
\ No newline at end of file
diff --git a/IMU/code/.idea/modules.xml b/IMU/code/.idea/modules.xml
new file mode 100644
index 0000000000000000000000000000000000000000..23968dc678111b184a2091404b0a05ec67a7b905
--- /dev/null
+++ b/IMU/code/.idea/modules.xml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+ <component name="ProjectModuleManager">
+ <modules>
+ <module fileurl="file://$PROJECT_DIR$/.idea/code.iml" filepath="$PROJECT_DIR$/.idea/code.iml" />
+ </modules>
+ </component>
+</project>
\ No newline at end of file
diff --git a/IMU/code/connect.py b/IMU/code/connect.py
new file mode 100644
index 0000000000000000000000000000000000000000..57e522fde980bf8261b2630d76b46a25610c1fc1
--- /dev/null
+++ b/IMU/code/connect.py
@@ -0,0 +1,165 @@
+import socket
+import serial
+import numpy as np
+from time import sleep, time
+from math import atan2, asin, cos, sin
+from vpython import vector, arrow, color, box, compound, scene, cross
+# from sourceCode.IMU import * # 假设 IMU 是一个模块，如果没有可以移除这行
+
+def send_command_to_unity(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))
+
+def setup_scene():
+ 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])
+
+ return frontArrow, sideArrow, upArrow, myObj
+
+def main():
+ column_limit = 9
+ arduino = serial.Serial('COM6', 115200)
+ sleep(1)
+
+ toRad = 2 * np.pi / 360
+ toDeg = 1 / toRad
+
+ frontArrow, sideArrow, upArrow, myObj = setup_scene()
+
+ last_averageRoll=0
+ last_averageyaw=0
+ last_averagePitch=0
+
+ averageroll = 0
+ averageyaw = 0
+ averagepitch = 0
+ last_print_time = time()
+
+ while True:
+ try:
+ while arduino.inWaiting() == 0:
+ pass
+
+ dataPacket = arduino.readline().decode()
+ cleandata = dataPacket.replace("\r\n", "")
+ row = cleandata.strip().split(',')
+
+ if len(row) == 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 = 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)) # 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)
+
+ 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
+ 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:
+ send_command_to_unity(f"Command : down {CalculatedAngle}")
+ if(differ_roll)<0:
+ 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:
+ send_command_to_unity(f"Command : roll {-yawAngle}")
+ if(differ_yaw)>0:
+ send_command_to_unity(f"Command : back {yawAngle}")
+
+
+
+
+ ''' if roll < -0.1:
+ send_command_to_unity("Command : down")
+ if roll > 0:
+ send_command_to_unity("Command : up")
+
+ if yaw > -0.5:
+ send_command_to_unity("Command : roll")
+ if yaw < -1.5:
+ send_command_to_unity("Command : back")
+ '''
+
+
+ last_print_time = current_time
+ last_averageRoll = roll
+ last_averageyaw=yaw
+ last_averagePitch=pitch
+
+ except Exception as e:
+ print("Error:", str(e))
+
+if __name__ == "__main__":
+ main()
+
+
+
diff --git a/IMU/code/sourceCode/EMG.py b/IMU/code/sourceCode/EMG.py
new file mode 100644
index 0000000000000000000000000000000000000000..337bd5dc1ff1e0e81367591fcdcd5b2a8f8bdb51
--- /dev/null
+++ b/IMU/code/sourceCode/EMG.py
@@ -0,0 +1,207 @@
+# Make a command line interface that allows for callibration, i.e tells the user to hold their hand in a clenched fist for 5 seconds, then uses that to figure out the emg theshold to open or close the robot arm in future hand movements. also does the same with the open hand, tlels the user to keep their hand open for 5-10 seconds. then allows for repeats if the user decides if the motion is accurate, maybe asking it a question that is it very accurate, then allows a repeat measurement to be made, not as a re-try but to use the 2+ measurements to get an average theshold value.
+
+from vpython import *
+import numpy as np
+from time import *
+import math
+import serial
+
+column_limit = 9
+
+arduino = serial.Serial('COM4', 115200)
+#braccio = serial.Serial('COM6', 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 = 45 # EMG measurements to get average
+
+count2 = 0
+print("Test1")
+while (count2 < 800):
+ count2 = count2 + 1
+ # print("Test2")
+ # sleep(0.5)
+ try:
+ # print("Test3")
+ while (arduino.inWaiting() == 0):
+ # print("TestNotWaiting")
+ pass
+ # sleep(0.01)
+
+ dataPacket = arduino.readline() # In quat2 it is arduino.read()
+ # print("TestRead")
+ # dataPacket = str(dataPacket, 'utf-8')
+ dataPacket = dataPacket.decode()
+ cleandata = dataPacket.replace("\r\n", "")
+ row = cleandata.strip().split(',')
+ # print("TestClean")
+
+ # print("Row length", len(row))
+ # print("Col limit", column_limit)
+
+ # print("Arduino (row) data", row)
+
+ if len(row) == column_limit:
+ splitPacket = cleandata.split(',')
+
+ emg1 = float(splitPacket[0]) # EMG data from the first set of electrodes
+ emg2 = float(splitPacket[1]) # EMG data from the second set of electrodes
+ q0 = float(splitPacket[2]) # qw
+ q1 = float(splitPacket[3]) # qx
+ q2 = float(splitPacket[4]) # qy
+ q3 = float(splitPacket[5]) # qz
+
+ # Callibration 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))
+
+ if column_limit == 9:
+ print(aC, gC, mC, sC, "pitch=", pitch * toDeg, "roll=", roll * toDeg, "yaw=", yaw * toDeg, "emg=", emg)
+ if column_limit == 10:
+ print(aC, gC, mC, sC, "pitch=", pitch * toDeg, "roll=", roll * toDeg, "yaw=", yaw * toDeg, "emg1=",
+ emg1, "emg2=", emg2)
+
+ # rate(50) #50 frames a second
+
+ 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
+
+ averageroll += roll * toDeg
+ averageyaw += yaw * toDeg
+ averagepitch += pitch * toDeg
+
+ averageemg1 += emg1
+ averageemg2 += emg2
+
+ if (count == iterations):
+ count = 0
+ # Concern: should this be emg/iterations rather than averageemg? No: averageemg is used in a second way: it is the total emg value (added each count)
+ 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 > threshold1): # First threshold
+ # M6 = 75
+ #elif (averageemg2 > threshold2): # Second threshold
+ # M6 = 75
+ #else:
+ # M6 = 10
+
+
+ data_to_send = [M1, M2, M3, M4, M5, M6]
+ # print(data_to_send)
+
+ # Old comments
+ # byte_data = bytearray(struct.pack('!{}i'.format(len(data_to_send)), *data_to_send))
+ # print(byte_data)
+ # braccio.write(pack('6h',0,averageyaw,45,90,averageroll,10)) #the 6h is 6 elements, and h is an int type data
+ #braccio.write(str(data_to_send).encode(
+ # 'utf-8')) # this sends the data over serial to the arduino IDE controlling the braccio
+ '''
+ sleep(0.01)
+ '''
+ #while (braccio.inWaiting() == 0):
+ # pass
+ #interpreted = braccio.readline()
+ # print(interpreted)
+
+ averageyaw = 0
+ averageroll = 0
+ averageemg1 = 0
+ averageemg2 = 0
+ else:
+ count = count + 1
+
+ else: # Expected num of data to process doesn't match Arduino data output.
+ print("Test = row != column")
+ except:
+ pass
+
+''' This doesn't work so I don't recommend uncommenting it. Possibly a future additional function?
+ if scene.kb.keys: # Check if there are any keyboard events
+ key = scene.kb.getkey() # Get the key that was pressed
+ if key == 'esc': # Check if the pressed key is 'esc' (Escape)
+ break # Break out of the loop
+'''
diff --git a/IMU/code/sourceCode/IMU.py b/IMU/code/sourceCode/IMU.py
new file mode 100644
index 0000000000000000000000000000000000000000..f6869aeb7687c15c0773a8dbe49aecd4f5a1a4f4
--- /dev/null
+++ b/IMU/code/sourceCode/IMU.py
@@ -0,0 +1,191 @@
+from vpython import *
+import numpy as np
+from time import *
+import math
+import serial
+
+column_limit = 9
+
+arduino = serial.Serial('COM6', 115200)
+#braccio = serial.Serial('COM6', 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
+averageemg=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
+
+ #sleep(0.5)
+ try:
+ while (arduino.inWaiting()==0):
+ pass
+
+
+ #sleep(0.01)
+ dataPacket = arduino.readline()
+ #dataPacket = str(dataPacket, 'utf-8')
+ dataPacket = dataPacket.decode()
+ cleandata = dataPacket.replace("\r\n","")
+ row = cleandata.strip().split(',')
+
+ if (len(row) == column_limit):
+
+ splitPacket = cleandata.split(',')
+ #Concern: not enough packet size for a second emg data if we're using 2 sets of electrodes.
+
+ 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 = 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))
+
+ #rate(50) #50 frames a second
+
+
+ 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
+
+ averageroll+=roll*toDeg
+ averageyaw+=yaw*toDeg
+ averagepitch+=pitch*toDeg
+ averageemg+=emg
+
+
+
+ if(count==iterations):
+ #print ( aC, gC, mC, sC, "pitch=", pitch*toDeg, "roll=", roll*toDeg, "yaw=", yaw*toDeg, "emg=",emg )
+ count=0
+ # Concern: should this be emg/iterations rather than averageemg? No: averageemg is used in a second way: it is the total emg value (added each count)
+ averageroll=averageroll/iterations
+ print("iterations :",iterations)
+ print("averageroll is",averageroll)
+ averageyaw=averageyaw/iterations
+ print("averageyaw is", averageyaw)
+ averagepitch=averagepitch/iterations
+ print("averagepitch is", averagepitch)
+ averageemg=averageemg/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 (averageemg>7.1):
+ M6=75
+ else:
+ M6=10
+
+ print ("Average emg=",averageemg)
+
+ data_to_send=[M1,M2,M3,M4,M5,M6]
+ print(data_to_send)
+
+ #Old comments
+ #byte_data = bytearray(struct.pack('!{}i'.format(len(data_to_send)), *data_to_send))
+ #print(byte_data)
+ #braccio.write(pack('6h',0,averageyaw,45,90,averageroll,10)) #the 6h is 6 elements, and h is an int type data
+ #braccio.write(str(data_to_send).encode('utf-8')) #this sends the data over serial to the arduino IDE controlling the braccio
+ '''
+ sleep(0.01)
+ '''
+ # while (braccio.inWaiting()==0):
+ # pass
+ #interpreted = braccio.readline()
+ #print(interpreted)
+
+ averageyaw=0
+ averageroll=0
+ emg=0
+ else:
+ count=count+1
+ except:
+ pass
+
+
+''' This doesn't work so I don't recommend uncommenting it. Possibly a future additional function?
+ if scene.kb.keys: # Check if there are any keyboard events
+ key = scene.kb.getkey() # Get the key that was pressed
+ if key == 'esc': # Check if the pressed key is 'esc' (Escape)
+ break # Break out of the loop
+'''
diff --git a/IMU/code/sourceCode/__pycache__/IMU.cpython-310.pyc b/IMU/code/sourceCode/__pycache__/IMU.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..b6b92e03c8cd2ea9566f5e63c01a90a175a995da
Binary files /dev/null and b/IMU/code/sourceCode/__pycache__/IMU.cpython-310.pyc differ