検出された角度にサーボを動かす

あとは標準出力からシリアルポートに出力先を変更するだけです。Bluetooth接続されたポート番号をプログラム内に記入してください。

import serial
pre_angle_s = 0
ser = serial.Serial(‘COM5’, 115200) #ポート番号と速度は環境に合わせて変更
…..
ser.write(str.encode(str(int(angle_s))+”\n”))
….
ser.close()

などを追加します。ある程度検出角度の変化があった場合に値を送信するようにしておくと、サーボが忙しく動きません。次の例では、1ステップ前の値と5より大きな差があればサーボに数値を送るようにしています。

import cv2
import math
import mediapipe as mp
import serial

mp_drawing = mp.solutions.drawing_utils
mp_drawing_styles = mp.solutions.drawing_styles
mp_pose = mp.solutions.pose
pre_angle_s = 0
ser = serial.Serial('COM5', 115200)

cap = cv2.VideoCapture(1)
with mp_pose.Pose(
    min_detection_confidence=0.5,
    min_tracking_confidence=0.5) as pose:
  while cap.isOpened():
    success, image = cap.read()
    if not success:
      print("Ignoring empty camera frame.")
      continue

    image.flags.writeable = False
    image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
    results = pose.process(image)

    image.flags.writeable = True
    image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
    mp_drawing.draw_landmarks(
        image,
        results.pose_landmarks,
        mp_pose.POSE_CONNECTIONS,
        landmark_drawing_spec=mp_drawing_styles.get_default_pose_landmarks_style())
    if results.pose_landmarks:
      x1=results.pose_landmarks.landmark[12].x #x1,y1 = shoulder
      x2=results.pose_landmarks.landmark[14].x #x2,y2 = elbow
      y1=results.pose_landmarks.landmark[12].y
      y2=results.pose_landmarks.landmark[14].y
      
      if x2<x1:
        angle_s = int(math.degrees(math.atan((y2-y1)/(x1-x2))))
      else:# x2>=x1
        angle_s = int(180+math.degrees(math.atan((y2-y1)/(x1-x2))))

      if angle_s < 0:
        angle_s = 0
      elif angle_s > 180:
        angle_s = 180

      #print("\r"+str(angle_s))

      #Bluetoothで送信
      if abs(angle_s-pre_angle_s)>5:
        ser.write(str.encode(str(angle_s)+"\n"))
        pre_angle_s = angle_s


    cv2.imshow('SIGACI', cv2.flip(image, 1))
    if cv2.waitKey(5) & 0xFF == 27: #ESCで終了
      break
cap.release()
ser.close()