人脸识别加舵机

import cv2
import numpy as np
import time
from adafruit_servokit import ServoKit

# 初始化PCA9685和舵机
kit = ServoKit(channels=16)
pan_servo = 0  # 水平舵机通道
tilt_servo = 1  # 垂直舵机通道

# 舵机初始位置
pan_angle = 90  # 水平角度 (0-180)
tilt_angle = 90  # 垂直角度 (0-180)

# 设置舵机角度范围
kit.servo[pan_servo].angle = pan_angle
kit.servo[tilt_servo].angle = tilt_angle

# 加载人脸检测模型
face_cascade = cv2.CascadeClassifier(cv2.data.haarcascades + 'haarcascade_frontalface_default.xml')

# 初始化摄像头
cap = cv2.VideoCapture(0)
cap.set(3, 640)  # 设置宽度
cap.set(4, 480)  # 设置高度

# PID控制器参数 (可根据需要调整)
pan_kp = 0.1
pan_ki = 0.01
pan_kd = 0.05

tilt_kp = 0.1
tilt_ki = 0.01
tilt_kd = 0.05

pan_integral = 0
pan_prev_error = 0

tilt_integral = 0
tilt_prev_error = 0


def pid_controller(error, kp, ki, kd, integral, prev_error):
    integral += error
    derivative = error - prev_error
    output = kp * error + ki * integral + kd * derivative
    prev_error = error
    return output, integral, prev_error


def map_value(value, in_min, in_max, out_min, out_max):
    return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min


try:
    while True:
        ret, frame = cap.read()
        if not ret:
            break

        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        faces = face_cascade.detectMultiScale(gray, 1.1, 4)

        if len(faces) > 0:
            # 获取最大的人脸
            (x, y, w, h) = max(faces, key=lambda f: f[2] * f[3])

            # 在图像上绘制矩形框
            cv2.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 0), 2)

            # 计算人脸中心点
            face_center_x = x + w // 2
            face_center_y = y + h // 2

            # 计算与图像中心的偏差
            frame_center_x = frame.shape[1] // 2
            frame_center_y = frame.shape[0] // 2

            pan_error = frame_center_x - face_center_x
            tilt_error = frame_center_y - face_center_y

            # 使用PID控制器计算舵机调整量
            pan_output, pan_integral, pan_prev_error = pid_controller(
                pan_error, pan_kp, pan_ki, pan_kd, pan_integral, pan_prev_error)

            tilt_output, tilt_integral, tilt_prev_error = pid_controller(
                tilt_error, tilt_kp, tilt_ki, tilt_kd, tilt_integral, tilt_prev_error)

            # 更新舵机角度
            pan_angle += pan_output
            tilt_angle += tilt_output

            # 限制舵机角度范围
            pan_angle = max(0, min(180, pan_angle))
            tilt_angle = max(0, min(180, tilt_angle))

            # 移动舵机
            kit.servo[pan_servo].angle = pan_angle
            kit.servo[tilt_servo].angle = tilt_angle

            # 显示调试信息
            cv2.putText(frame, f"Pan: {pan_angle:.1f}", (10, 30),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
            cv2.putText(frame, f"Tilt: {tilt_angle:.1f}", (10, 60),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)

        # 显示图像
        cv2.imshow('Face Tracking', frame)

        # 按'q'退出
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

        # 添加短暂延迟以减少CPU使用率
        time.sleep(0.05)

except KeyboardInterrupt:
    print("程序被用户中断")

finally:
    # 清理资源
    cap.release()
    cv2.destroyAllWindows()
    # 将舵机归位
    kit.servo[pan_servo].angle = 90
    kit.servo[tilt_servo].angle = 90
    time.sleep(1)
    print("程序结束")