介绍

在这篇文章，我会先介绍需要用到且需要注意的函数，之后再给出整体代码

在追小球的云台中，比较重要的部分就是云台（实质上就是舵机）的控制以及对识别的色块位置进行处理得到相应信息后控制云台进行运动

1、舵机模块和PID模块的导入

需要注意的是，PID还需要官方提供的PID文件（当然你要是把整体代码全部移植过来也是可以的）

from pyb import PID
from pyb import Servo

2、初始化舵机端口

这里初始化舵机端口，我的理解是就相当于一个宏定义

sp_servo=Servo(1)
cz_servo=Servo(2)

3、校准舵机，设置PWM信号的范围

sp_servo.calibration(500, 2500, 1500)
cz_servo.calibration(500, 2500, 1500)

这里的代码根据之前的初始化舵机端口名字来写，不同名不一样

这里用到的函数是

Servo.calibration([pulse_min,pulse_max,pulse_centre[,pulse_angle_90,pulse_speed_100]])

它后面的两个参数不需要用到，我们只对前三个参数进行说明

pulse_min：设置的最小脉冲宽度

pulse_max：设置的最大脉冲宽度

pulse_centre：中心（90度）\0度对应的位置

4、初始化PID控制器

PID控制器的初始化分为两种情况

情况1是在上机调试时的情况，情况2时在脱机运行时的情况

按道理来说，上机调试时由于数据的传输是需要时间的，也就是实时数据的获取存在延迟；而脱机运行时数据的传输更快，延迟更小

根据实时数据获取的延迟来看，在脱机运行时，由于数据传输的实时性很好，所以PID参数可以采用大一些的值；同理，在上机调试时，由于数据的实时性较差，所以采用更小的PID参数来进行调节

根据我上面的阐述，在设置PID参数时，脱机设置较大值，上机设置较小值；不知道是不是官方给出的代码出现了错误，它给出来的PID参数设置与我上述设置相反（我也不确定到底是谁错了）

 sp_pid = PID(p=0.1, i=0, imax=90)
 cz_pid = PID(p=0.1, i=0, imax=90)

5、判断最大色块

由于在进行小球的追踪时，肯定会有其他小球之外的色块存在，所以在这个时候，我们只对最大的色块进行处理（默认最大色块为小球）

def find_max(blobs):
    max_size = 0
    max_blob = None
    for blob in blobs:
        if blob[2] * blob[3] > max_size:
            max_blob = blob
            max_size = blob[2] * blob[3]
    return max_blob

对于判断最大色块，我们定义了一个函数find_max（blobs）来寻找最大色块

在这判断色块大小是根据色块查找函数find_blobs(thresholds)返回的blob对象列表的值来确定的

在Openmv官方例程和函数库中并没有给出blob对象的值，所以我们我们自己写一个色块寻找程序，并打印出blob对象的值

可以看到blob对象的值如下图所示 

所以blob[2]、[3]分别是blob对象的第3、4个参数，也就是色块的宽度和色块的高度，两个参数相乘即为色块的面积

6、误差的获取

通过用最大色块返回的中心坐标值减去图像中心坐标值，获得误差

sp_error = max_blob.cx() - img.width() / 2
cz_error = max_blob.cy() - img.height() / 2

7、误差的处理

 sp_output = sp_pid.get_pid(sp_error, 1) / 2
 cz_output = cz_pid.get_pid(cz_error, 1)
 sp_servo.angle(sp_servo.angle() + sp_output)
 cz_servo.angle(cz_servo.angle() - cz_output)

对误差的控制引入了PID的控制，在这里我就不对涉及pid.py文件的部分作说明，这两天会再写一篇文章对该文件及PID算法进行说明

8、main.py代码

import sensor,image,time
from pyb import PID
from pyb import Servo

#初始化用于平移和倾斜的舵机（我的理解是这里就是做了一个宏定义）
sp_servo=Servo(1)
cz_servo=Servo(2)

# 校准舵机，设置PWM信号占空比的范围
sp_servo.calibration(500, 2500, 1500)
cz_servo.calibration(500, 2500, 1500)

# 初始化平移和倾斜的PID控制器
# 脱机运行或者禁用图像传输时使用这些PID值
sp_pid = PID(p=0.07, i=0, imax=90)
cz_pid = PID(p=0.05, i=0, imax=90)

# 如果在线调试，使用这些PID值,之所以在线调试和脱机调试用不一样的PID参数，是因为在线调试数据有延迟
# pan_pid = PID(p=0.1, i=0, imax=90)
# tilt_pid = PID(p=0.1, i=0, imax=90)

sensor.reset()  # 初始化摄像头传感器
sensor.set_pixformat(sensor.RGB565)  # 设置像素格式为RGB565
sensor.set_framesize(sensor.QQVGA)  # 设置分辨率为QQVGA以提高速度
sensor.skip_frames(1000)  # 让新设置生效
sensor.set_auto_whitebal(False)  # 关闭自动白平衡
sensor.set_auto_gain(False)  #关闭自动增益
clock = time.clock()  # 跟踪每秒帧数（FPS）

#根据find_blobs()函数返回的blob列表的blob对象的第3、4个参数分别是像素的宽和高
def find_max(blobs):
    #每次调用该函数都会对参数作初始化
    max_size = 0
    max_blob = None
    for blob in blobs:
        if blob[2] * blob[3] > max_size:
            max_blob = blob
            max_size = blob[2] * blob[3]
    return max_blob

while(True):
    clock.tick()  # 跟踪每次快照之间经过的毫秒数
    img = sensor.snapshot()  # 拍照并返回图像

    blobs = img.find_blobs([red_threshold])

    #如果识别到了色块
    if blobs:
        max_blob = find_max(blobs)

        #计算水平方向和垂直方向和图像中心的距离作为误差值
        sp_error = max_blob.cx() - img.width() / 2
        cz_error = max_blob.cy() - img.height() / 2

        print("sp_error:", sp_error)
        print("cz_error:", cz_error)

        img.draw_rectangle(max_blob.rect())  # 画出矩形
        img.draw_cross(max_blob.cx(), max_blob.cy())  # 画出十字

        sp_output = sp_pid.get_pid(sp_error, 1) / 2
        cz_output = cz_pid.get_pid(cz_error, 1)
        print("sp_output:", sp_output)
        print("cz_output:", cz_output)
        sp_servo.angle(sp_servo.angle() + sp_output)
        cz_servo.angle(cz_servo.angle() - cz_output)

9、pid.py代码

from pyb import millis  # 导入 pyboard 的 millis 函数，用于获取当前时间（毫秒）
from math import pi, isnan  # 导入 pi 和 isnan 函数

class PID:
    # 定义 PID 控制器的参数和状态变量
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    _RC = 1/(2 * pi * 20)  # RC 低通滤波器的时间常数

    def __init__(self, p=0, i=0, d=0, imax=0):
        # 初始化 PID 控制器的参数
        self._kp = float(p)  # 比例系数
        self._ki = float(i)  # 积分系数
        self._kd = float(d)  # 微分系数
        self._imax = abs(imax)  # 积分限制，防止积分饱和
        self._last_derivative = float('nan')  # 最后的导数值初始化为 NaN

    def get_pid(self, error, scaler):
        tnow = millis()  # 获取当前时间
        dt = tnow - self._last_t  # 计算时间差
        output = 0  # 初始化输出值

        if self._last_t == 0 or dt > 1000:  # 如果是第一次运行或者时间差大于 1 秒
            dt = 0  # 重置时间差
            self.reset_I()  # 重置积分器

        self._last_t = tnow  # 更新最后时间戳
        delta_time = float(dt) / float(1000)  # 将时间差转换为秒

        output += error * self._kp  # 计算比例项

        if abs(self._kd) > 0 and dt > 0:  # 如果微分系数大于 0 且时间差大于 0
            if isnan(self._last_derivative):  # 如果最后的导数值为 NaN
                derivative = 0  # 设置导数为 0
                self._last_derivative = 0  # 重置最后的导数值
            else:
                derivative = (error - self._last_error) / delta_time  # 计算误差的导数
            # 使用低通滤波器平滑导数值
            derivative = self._last_derivative + ((delta_time / (self._RC + delta_time)) * (derivative - self._last_derivative))
            self._last_error = error  # 更新最后的误差值
            self._last_derivative = derivative  # 更新最后的导数值
            output += self._kd * derivative  # 计算微分项并加到输出中

        output *= scaler  # 按比例缩放输出值

        if abs(self._ki) > 0 and dt > 0:  # 如果积分系数大于 0 且时间差大于 0
            self._integrator += (error * self._ki) * scaler * delta_time  # 计算积分项并加到积分器中
            # 限制积分器的值在 -imax 和 imax 之间，防止积分饱和
            if self._integrator < -self._imax:
                self._integrator = -self._imax
            elif self._integrator > self._imax:
                self._integrator = self._imax
            output += self._integrator  # 将积分项加到输出中

        return output  # 返回计算的 PID 控制器输出值

    def reset_I(self):
        self._integrator = 0  # 重置积分器
        self._last_derivative = float('nan')  # 重置最后的导数值为 NaN

结语

如果大家发现有什么不对的地方，请斧正！