二维舵机颜色追踪，使用树莓派+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 = False

    def start(self):
        Thread(target=self.update, args=()).start()
        return self

    def update(self):
        while not self.stopped:
            self.grabbed, self.frame = self.stream.read()

    def read(self):
        return self.frame

    def stop(self):
        self.stopped = True
        self.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):
    pass

cv2.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")
            break

        frame = 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') / 100
                pan_pid.kI = cv2.getTrackbarPos('Pan Ki', 'PID Tuner') / 100
                pan_pid.kD = cv2.getTrackbarPos('Pan Kd', 'PID Tuner') / 100
                tilt_pid.kP = cv2.getTrackbarPos('Tilt Kp', 'PID Tuner') / 100
                tilt_pid.kI = cv2.getTrackbarPos('Tilt Ki', 'PID Tuner') / 100
                tilt_pid.kD = cv2.getTrackbarPos('Tilt Kd', 'PID Tuner') / 100

                panAdjustment = pan_pid.update(errorX, sleep=0)
                tiltAdjustment = tilt_pid.update(errorY, sleep=0)

                panAngle += panAdjustment
                tiltAngle += 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'):
            break

        tEnd = time.time()
        loopTime = tEnd - tStart
        fps = .9 * fps + .1 * (1 / loopTime)

finally:
    vs.stop()
    cv2.destroyAllWindows()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140

在相同文件路径下创建一个名为pid.py的文件

# pid.py
# -*- coding: UTF-8 -*-
# 调用必需库
import time

class PID:
    def __init__(self, kP=1, kI=0, kD=0):
        # 初始化参数
        self.kP = kP
        self.kI = kI
        self.kD = kD

    def initialize(self):
        # 初始化当前时间和上一次计算的时间
        self.currTime = time.time()
        self.prevTime = self.currTime

        # 初始化上一次计算的误差
        self.prevError = 0

        # 初始化误差的比例值，积分值和微分值
        self.cP = 0
        self.cI = 0
        self.cD = 0

    def 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.currTime
        self.prevError = error

        # 返回输出值
        return sum([
            self.kP * self.cP,
            self.kI * self.cI,
            self.kD * self.cD])

    def set_Kp(self, kP):
        self.kP = kP

    def set_Ki(self, kI):
        self.kI = kI

    def set_Kd(self, kD):
        self.kD = kD

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64

在相同文件路径下创建一个名为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:
        break
    sleep(0.2)
if status != 0:
    print(status, result)

class Servo():
    MAX_PW = 1250  # 0.5/20*100
    MIN_PW = 250   # 2.5/20*100
    _freq = 50     # 50 Hz, 20ms

    def __init__(self, pin, min_angle=-90, max_angle=90):
        self.pi = pigpio.pi()
        self.pin = pin
        self.pi.set_PWM_frequency(self.pin, self._freq)
        self.pi.set_PWM_range(self.pin, 10000)
        self.angle = 0
        self.max_angle = max_angle
        self.min_angle = min_angle
        self.pi.set_PWM_dutycycle(self.pin, 0)

    def set_angle(self, angle):
        if angle > self.max_angle:
            angle = self.max_angle
        elif angle < self.min_angle:
            angle = self.min_angle
        self.angle = angle
        duty = self.map(angle, -90, 90, 250, 1250)
        self.pi.set_PWM_dutycycle(self.pin, duty)

    def get_angle(self):
        return self.angle

    def 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_min

if __name__ == '__main__':
    pan = Servo(pin=13, max_angle=90, min_angle=-90)
    tilt = Servo(pin=12, max_angle=30, min_angle=-90)
    panAngle = 0
    tiltAngle = 0
    pan.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)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77