Untitled

mail@pastecode.io avatar
unknown
python
a month ago
3.2 kB
1
Indexable
Never
import cv2
import mediapipe as mp
import numpy as np
import time
import serial 
import pandas as pd
import matplotlib.pyplot as plt

pTime = 0
cTime = 0
cap = cv2.VideoCapture(0)
mpPose = mp.solutions.pose
pose = mpPose.Pose(min_detection_confidence=0.5, min_tracking_confidence=0.5)
mpDraw = mp.solutions.drawing_utils
ser = serial.Serial("COM3",9600)
status = False

fig,ax = plt.subplots(figsize = (15, 3))

angle = {
     "lelb": "150",
     "lesh": "61"
}

def calculate_angle(a,b,c):
        a = np.array(a) # First
        b = np.array(b) # Mid
        c = np.array(c) # End
   
        radians = np.arctan2(c[1]-b[1], c[0]-b[0]) - np.arctan2(a[1]-b[1], a[0]-b[0])
        angle = np.abs(radians*180.0/np.pi)
   
        if angle > 180.0:
            angle = 360-angle
       
        return angle

while cap.isOpened():
    ret, img = cap.read()
    if ret:
        imgRGB = cv2.cvtColor(img,cv2.COLOR_BGR2RGB)
        result = pose.process(imgRGB)

        imgHeight = img.shape[0]
        imgWidth = img.shape[1]

        mpDraw.draw_landmarks(img, result.pose_landmarks, mpPose.POSE_CONNECTIONS,
                                mpDraw.DrawingSpec(color=(2,2,190), thickness=2, circle_radius=2),
                                mpDraw.DrawingSpec(color=(140,230,72), thickness=2, circle_radius=2))
       
        cTime = time.time()
        fps = 1/(cTime-pTime)
        pTime = cTime
        cv2.putText(img, f"FPS:{int(fps)}", (30, 50), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255, 0, 0), 2)
        cv2.imshow('Frame Receiving', img)
   
        try:
            landmarks = result.pose_landmarks.landmark
            left_shoulder = [landmarks[mpPose.PoseLandmark.LEFT_SHOULDER.value].x,landmarks[mpPose.PoseLandmark.LEFT_SHOULDER.value].y]
            left_elbow = [landmarks[mpPose.PoseLandmark.LEFT_ELBOW.value].x,landmarks[mpPose.PoseLandmark.LEFT_ELBOW.value].y]
            left_wrist = [landmarks[mpPose.PoseLandmark.LEFT_WRIST.value].x,landmarks[mpPose.PoseLandmark.LEFT_WRIST.value].y]
            left_hip = [landmarks[mpPose.PoseLandmark.LEFT_HIP.value].x,landmarks[mpPose.PoseLandmark.LEFT_HIP.value].y]

            # Calculate angle
            angle["lelb"] = (calculate_angle(left_shoulder, left_elbow, left_wrist))
            angle["lesh"] = (calculate_angle(left_hip, left_shoulder, left_elbow))
            lelb = []
            lesh = []
            if cv2.waitKey(1) == ord('y'):
                data = {'angle' : [angle["lelb"],angle["lesh"]]}
                df = pd.DataFrame(data)
                df.to_json('output.json', orient='records', indent=4)

            lelb.append(angle["lelb"])
            lesh.append(angle["lesh"])

            tmp = f"a{angle['lelb']}\n"
            ser.write(str.encode(tmp))
            time.sleep(0.002)
            tmp = f"b{angle['lesh']}\n"
            ser.write(str.encode(tmp))
            
        except:
            pass
    
        
        if cv2.waitKey(1) == ord('f'):
             print(lelb,lesh)
             ax.plot(lelb,lesh)
             plt.show()

        elif cv2.waitKey(1) == ord('q'):
            ser.close()
            break
Leave a Comment