diff --git a/IMU/code/connect.py b/IMU/code/connect.py
index 57e522fde980bf8261b2630d76b46a25610c1fc1..69c7d087617e7db084da10facd9e347de6222d7c 100644
--- a/IMU/code/connect.py
+++ b/IMU/code/connect.py
@@ -109,8 +109,8 @@ def main():
if current_time - last_print_time >= 0.01:
- print(f"roll is: {roll}")
- print(f"last roll is: {last_averageRoll}")
+ #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
@@ -124,8 +124,8 @@ def main():
if(yaw<0):
yaw=-yaw
- print(f"yaw is: {yaw}")
- print(f"last yaw is: {last_averageyaw}")
+ #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
diff --git a/integration/IMUEMGCombined.py b/integration/IMUEMGCombined.py
new file mode 100644
index 0000000000000000000000000000000000000000..b3611ddb7527437d713093f7ebdd23737cdef92b
--- /dev/null
+++ b/integration/IMUEMGCombined.py
@@ -0,0 +1,247 @@
+from vpython import *
+import numpy as np
+from time import *
+import math
+import serial
+import time
+
+# Constants
+column_limit = 6
+data_collection_duration = 3
+num_measurements_hand = 1
+
+# Initialize the arduino object
+arduino = serial.Serial('COM17', 115200)
+# braccio = serial.Serial('COM6', 115200)
+sleep(1)
+
+# Conversions
+toRad = 2 * np.pi / 360
+toDeg = 1 / toRad
+
+# VPython scene setup
+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)
+
+
+# Function to collect EMG data for a given duration
+def collect_emg_data(duration):
+ emg_data = []
+ start_time = time.time()
+ while time.time() - start_time < duration:
+ if arduino.inWaiting() > 0:
+ data = arduino.readline().decode().strip().split(',')
+ if len(data) == column_limit:
+ try:
+ emg1 = float(data[0])
+ emg2 = float(data[1])
+ #print("Emg1: ",emg1)
+ #print("Emg2: ",emg2)
+ emg_data.append((emg1, emg2))
+ except ValueError:
+ continue
+ if emg_data:
+ return np.mean(emg_data, axis=0)
+ else:
+ return np.array([0.0, 0.0]) # Default value if no data is collected
+
+
+# Function to calibrate thresholds
+def calibrate_thresholds():
+ clenched_fist_measurements = []
+ open_hand_measurements = []
+
+ print("Get ready to calibrate the open/closed hand measurements")
+ for i in range(int(data_collection_duration / 2)):
+ time.sleep(1)
+ print((i + 1), "...")
+
+ for i in range(num_measurements_hand):
+ print(f"Get ready for closed hand measurement {i + 1}:")
+ time.sleep(3)
+ print("Hold your hand in a clenched fist for ", data_collection_duration, " seconds...")
+ for j in range(data_collection_duration):
+ time.sleep(1)
+ print((j + 1), "...")
+ clenched_fist = collect_emg_data(data_collection_duration)
+ clenched_fist_measurements.append(clenched_fist)
+ print(f"Clenched fist measurement {i + 1} EMG average: {clenched_fist}")
+
+ print("/n")
+
+ for i in range(num_measurements_hand):
+ print(f"Get ready for open hand measurement {i + 1}:")
+ time.sleep(3)
+ print("Now hold your hand open for ", data_collection_duration, " seconds...")
+ for j in range(data_collection_duration):
+ time.sleep(1)
+ print((j + 1), "...")
+ open_hand = collect_emg_data(data_collection_duration)
+ open_hand_measurements.append(open_hand)
+ print(f"Open hand measurement {i + 1} EMG average: {open_hand}")
+
+ print("/n")
+ clenched_fist_avg = np.mean(clenched_fist_measurements, axis=0)
+ open_hand_avg = np.mean(open_hand_measurements, axis=0)
+
+ return clenched_fist_avg, open_hand_avg
+
+
+# Function to calculate the average of multiple measurements
+def average_measurements(measurements):
+ return np.mean(measurements, axis=0)
+
+
+# Main calibration loop
+def main():
+ 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
+
+ while True:
+ clenched_fist, open_hand = calibrate_thresholds()
+ print("/n")
+
+ print(f"Averaged clenched fist EMG: {clenched_fist}")
+ print(f"Averaged open hand EMG: {open_hand}")
+
+ # Determine thresholds (simple example)
+ threshold1 = (clenched_fist[0] + open_hand[0]) / 2 # Finds the midpoint of avg
+ threshold2 = (clenched_fist[1] + open_hand[1]) / 2
+ print(f"Thresholds: {threshold1}, {threshold2}")
+
+ # Commented out for automatic operation
+ # accuracy = input("Is the motion detection accurate? (yes/no): ")
+ # if accuracy.lower() == 'yes':
+ # break
+ break
+
+ count2 = 0
+ while count2 < 2000: # Replace with True to run forever
+ count2 += 1
+ 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]) # 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
+
+ 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
+
+ averageemg1 += emg1
+ averageemg2 += emg2
+
+ if count == iterations:
+ averageroll /= iterations
+ averageyaw /= iterations
+ averagepitch /= iterations
+
+ averageemg1 /= iterations
+ 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 and averageemg2 > threshold2: # First threshold
+ print("Clenched fist detected")
+ M6 = 75
+ else:
+ M6 = 10
+ print("Open hand detected")
+
+ data_to_send = [M1, M2, M3, M4, M5, M6]
+
+ averageyaw = 0
+ averageroll = 0
+ averageemg1 = 0
+ averageemg2 = 0
+ else:
+ count += 1
+ else:
+ print("Test = row != column")
+ except Exception as e:
+ print(f"Error: {e}")
+
+
+if __name__ == "__main__":
+ main()