二维舵机颜色追踪，使用树莓派+opencv+usb摄像头+两个舵机实现颜色追踪，采用pid调控
效果演示
二维云台颜色追踪
使用树莓派+opencv+usb摄像头+两个舵机实现颜色追踪，采用pid调控
import cv2
import time
import numpy as np
from threading import Thread
from servo import Servo
from pid import PID# 初始化伺服电机
pan = Servo(pin=19)
tilt = Servo(pin=16)
panAngle = 0
tiltAngle = 0
pan.set_angle(panAngle)
tilt.set_angle(tiltAngle)# 定义视频流类
class VideoStream:def __init__(self, src=0):self.stream = cv2.VideoCapture(src)self.stream.set(cv2.CAP_PROP_FRAME_WIDTH, 320)self.stream.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)self.stream.set(cv2.CAP_PROP_FPS, 30)self.grabbed, self.frame = self.stream.read()self.stopped = Falsedef start(self):Thread(target=self.update, args=()).start()return selfdef update(self):while not self.stopped:self.grabbed, self.frame = self.stream.read()def read(self):return self.framedef stop(self):self.stopped = Trueself.stream.release()# 启动视频流
vs = VideoStream(src=0).start()# 设置 PID 控制器参数
pan_pid = PID(0.05, 0.01, 0.001)
tilt_pid = PID(0.05, 0.01, 0.001)
pan_pid.initialize()
tilt_pid.initialize()# 计算帧率
fps = 0
pos = (10, 20)
font = cv2.FONT_HERSHEY_SIMPLEX
height = 0.5
weight = 1
myColor = (0, 0, 255)def nothing(x):passcv2.namedWindow('PID Tuner')
cv2.createTrackbar('Pan Kp', 'PID Tuner', int(pan_pid.kP * 100), 100, nothing)
cv2.createTrackbar('Pan Ki', 'PID Tuner', int(pan_pid.kI * 100), 100, nothing)
cv2.createTrackbar('Pan Kd', 'PID Tuner', int(pan_pid.kD * 100), 100, nothing)
cv2.createTrackbar('Tilt Kp', 'PID Tuner', int(tilt_pid.kP * 100), 100, nothing)
cv2.createTrackbar('Tilt Ki', 'PID Tuner', int(tilt_pid.kI * 100), 100, nothing)
cv2.createTrackbar('Tilt Kd', 'PID Tuner', int(tilt_pid.kD * 100), 100, nothing)last_update = time.time()
update_interval = 0.1 # 控制更新频率try:while True:tStart = time.time()frame = vs.read()if frame is None:print("Failed to grab frame")breakframe = cv2.flip(frame, 1)frameHSV = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)cv2.putText(frame, str(int(fps)) + ' FPS', pos, font, height, myColor, weight)lowerBound = np.array([0, 147, 114])upperBound = np.array([88, 255, 255])myMask = cv2.inRange(frameHSV, lowerBound, upperBound)contours, _ = cv2.findContours(myMask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)if len(contours) > 0:contours = sorted(contours, key=lambda x: cv2.contourArea(x), reverse=True)contour = contours[0]x, y, w, h = cv2.boundingRect(contour)cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 0, 255), 3)# 计算误差errorX = (x + w / 2) - (320 / 2)errorY = (240 / 2) - (y + h / 2) # 反转误差方向if time.time() - last_update > update_interval:# 获取PID参数并更新pan_pid.kP = cv2.getTrackbarPos('Pan Kp', 'PID Tuner') / 100pan_pid.kI = cv2.getTrackbarPos('Pan Ki', 'PID Tuner') / 100pan_pid.kD = cv2.getTrackbarPos('Pan Kd', 'PID Tuner') / 100tilt_pid.kP = cv2.getTrackbarPos('Tilt Kp', 'PID Tuner') / 100tilt_pid.kI = cv2.getTrackbarPos('Tilt Ki', 'PID Tuner') / 100tilt_pid.kD = cv2.getTrackbarPos('Tilt Kd', 'PID Tuner') / 100panAdjustment = pan_pid.update(errorX, sleep=0)tiltAdjustment = tilt_pid.update(errorY, sleep=0)panAngle += panAdjustmenttiltAngle += tiltAdjustment# 限制角度范围panAngle = max(-90, min(panAngle, 120))tiltAngle = max(-90, min(tiltAngle, 90))# 设置伺服电机角度pan.set_angle(panAngle)tilt.set_angle(tiltAngle)last_update = time.time()# 仅在图形环境中显示图像窗口try:cv2.imshow('Camera', frame)cv2.imshow('Mask', myMask)except cv2.error as e:print(f"OpenCV error: {e}")if cv2.waitKey(1) == ord('q'):breaktEnd = time.time()loopTime = tEnd - tStartfps = .9 * fps + .1 * (1 / loopTime)finally:vs.stop()cv2.destroyAllWindows()在相同文件路径下创建一个名为pid.py的文件
# pid.py
# -*- coding: UTF-8 -*-
# 调用必需库
import timeclass PID:def __init__(self, kP=1, kI=0, kD=0):# 初始化参数self.kP = kPself.kI = kIself.kD = kDdef initialize(self):# 初始化当前时间和上一次计算的时间self.currTime = time.time()self.prevTime = self.currTime# 初始化上一次计算的误差self.prevError = 0# 初始化误差的比例值，积分值和微分值self.cP = 0self.cI = 0self.cD = 0def update(self, error, sleep=0.2):# 暂停time.sleep(sleep)# 获取当前时间并计算时间差self.currTime = time.time()deltaTime = self.currTime - self.prevTime# 计算误差的微分deltaError = error - self.prevError# 比例项self.cP = error# 积分项self.cI += error * deltaTime# 微分项self.cD = (deltaError / deltaTime) if deltaTime > 0 else 0# 保存时间和误差为下次更新做准备self.prevTime = self.currTimeself.prevError = error# 返回输出值return sum([self.kP * self.cP,self.kI * self.cI,self.kD * self.cD])def set_Kp(self, kP):self.kP = kPdef set_Ki(self, kI):self.kI = kIdef set_Kd(self, kD):self.kD = kD在相同文件路径下创建一个名为servo.py的文件
import pigpio
from time import sleep
import subprocess# Start the pigpiod daemon
result = None
status = 1
for x in range(3):p = subprocess.Popen('sudo pigpiod', shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)result = p.stdout.read().decode('utf-8')status = p.poll()if status == 0:breaksleep(0.2)
if status != 0:print(status, result)class Servo():MAX_PW = 1250 # 0.5/20*100MIN_PW = 250 # 2.5/20*100_freq = 50 # 50 Hz, 20msdef __init__(self, pin, min_angle=-90, max_angle=90):self.pi = pigpio.pi()self.pin = pinself.pi.set_PWM_frequency(self.pin, self._freq)self.pi.set_PWM_range(self.pin, 10000)self.angle = 0self.max_angle = max_angleself.min_angle = min_angleself.pi.set_PWM_dutycycle(self.pin, 0)def set_angle(self, angle):if angle > self.max_angle:angle = self.max_angleelif angle < self.min_angle:angle = self.min_angleself.angle = angleduty = self.map(angle, -90, 90, 250, 1250)self.pi.set_PWM_dutycycle(self.pin, duty)def get_angle(self):return self.angledef stop(self):self.pi.set_PWM_dutycycle(self.pin, 0)self.pi.stop()def map(self, x, in_min, in_max, out_min, out_max):return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_minif __name__ == '__main__':pan = Servo(pin=13, max_angle=90, min_angle=-90)tilt = Servo(pin=12, max_angle=30, min_angle=-90)panAngle = 0tiltAngle = 0pan.set_angle(panAngle)tilt.set_angle(tiltAngle)sleep(1)while True:for angle in range(0, 90, 1):pan.set_angle(angle)tilt.set_angle(angle)sleep(.01)sleep(.5)for angle in range(90, -90, -1):pan.set_angle(angle)tilt.set_angle(angle)sleep(.01)sleep(.5)for angle in range(-90, 0, 1):pan.set_angle(angle)tilt.set_angle(angle)sleep(.01)sleep(.5)