free

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

integration/IMUEMGCombined.py

+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()