免费分享openmv云台追踪红色小球项目main.py和pid.py详细注释以及演示视频。

一、演示视频（b站工房有相关材料清单，有需要的可以看看）

openmv云台追踪红色小球演示视频https://www.bilibili.com/video/BV1U8AheeEn9?vd_source=9f3c5dbe416321a5847f52123258d443

二、代码详细注释

以下为main.py代码详细注释

# 导入必要的库
import sensor, image, time

# 从pid模块导入PID类，用于实现PID控制算法
from pid import PID
# 从pyb模块导入Servo类，用于控制舵机
from pyb import Servo

# 初始化水平方向（平移）的舵机，使用引脚1
pan_servo = Servo(1)
# 初始化垂直方向（倾斜）的舵机，使用引脚2
tilt_servo = Servo(2)

# 对水平方向舵机进行校准，设置脉宽范围为500到2500，中间值为500
pan_servo.calibration(500, 2500, 500)
# 对垂直方向舵机进行校准，设置脉宽范围为500到2500，中间值为500
tilt_servo.calibration(500, 2500, 500)

# 定义红色颜色的阈值范围，用于颜色识别
red_threshold = (13, 49, 18, 61, 6, 47)

# 初始化水平方向的PID控制器，设置比例系数p为0.07，积分系数i为0，积分最大值imax为90
# 适用于脱机运行或者禁用图像传输的情况
pan_pid = PID(p=0.07, i=0, imax=90)
# 初始化垂直方向的PID控制器，设置比例系数p为0.05，积分系数i为0，积分最大值imax为90
# 适用于脱机运行或者禁用图像传输的情况
tilt_pid = PID(p=0.05, i=0, imax=90)
# 以下两行是在线调试时使用的PID参数设置，这里被注释掉了
# pan_pid = PID(p=0.1, i=0, imax=90)
# tilt_pid = PID(p=0.1, i=0, imax=90)

# 重置摄像头传感器，进行初始化操作
sensor.reset()
# 设置图像的像素格式为RGB565
sensor.set_pixformat(sensor.RGB565)
# 设置图像的帧大小为QQVGA，以提高处理速度
sensor.set_framesize(sensor.QQVGA)
# 跳过前10帧，让新的设置生效
sensor.skip_frames(10)
# 关闭自动白平衡功能，以保证颜色识别的准确性
sensor.set_auto_whitebal(False)
# 创建一个时钟对象，用于跟踪帧率
clock = time.clock()
# 设置摄像头垂直翻转，根据实际安装情况调整图像方向
sensor.set_vflip(True)

# 定义一个函数，用于从多个色块中找到面积最大的色块
def find_max(blobs):
    # 初始化最大面积为0
    max_size = 0
    # 遍历所有的色块
    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变量中
    img = sensor.snapshot()

    # 在图像中查找符合红色阈值的色块
    blobs = img.find_blobs([red_threshold])
    # 如果找到了符合条件的色块
    if blobs:
        # 调用find_max函数，找到面积最大的色块
        max_blob = find_max(blobs)
        # 计算水平方向的误差，即最大色块的中心点x坐标与图像宽度一半的差值
        pan_error = max_blob.cx() - img.width() / 2
        # 计算垂直方向的误差，即最大色块的中心点y坐标与图像高度一半的差值
        tilt_error = max_blob.cy() - img.height() / 2

        # 打印水平方向的误差值
        print("pan_error: ", pan_error)

        # 在图像上绘制最大色块的矩形框
        img.draw_rectangle(max_blob.rect())
        # 在图像上绘制最大色块的中心点十字标记
        img.draw_cross(max_blob.cx(), max_blob.cy())

        # 通过水平方向的PID控制器计算输出值，并将结果除以2
        pan_output = pan_pid.get_pid(pan_error, 1) / 2
        # 通过垂直方向的PID控制器计算输出值
        tilt_output = tilt_pid.get_pid(tilt_error, 1)
        # 打印水平方向的PID输出值
        print("pan_output", pan_output)
        # 根据水平方向的PID输出值调整水平舵机的角度
        pan_servo.angle(pan_servo.angle() + pan_output)
        # 根据垂直方向的PID输出值调整垂直舵机的角度
        tilt_servo.angle(tilt_servo.angle() - tilt_output)

以下为pid.py代码详细注释

# 从 pyb 模块导入 millis 函数，用于获取当前的毫秒级时间
from pyb import millis
# 从 math 模块导入 pi（圆周率）和 isnan 函数（用于判断一个值是否为 NaN）
from math import pi, isnan

# 定义一个 PID 类，用于实现 PID（比例 - 积分 - 微分）控制器
class PID:
    # 初始化类的静态属性，用于存储 PID 控制器的参数和状态
    _kp = _ki = _kd = _integrator = _imax = 0
    _last_error = _last_derivative = _last_t = 0
    # 计算低通滤波器的时间常数，这里截止频率为 20Hz
    _RC = 1 / (2 * pi * 20)

    # 类的构造函数，用于初始化 PID 控制器的参数
    def __init__(self, p=0, i=0, d=0, imax=0):
        # 将传入的比例系数 p 转换为浮点数并赋值给类的属性 _kp
        self._kp = float(p)
        # 将传入的积分系数 i 转换为浮点数并赋值给类的属性 _ki
        self._ki = float(i)
        # 将传入的微分系数 d 转换为浮点数并赋值给类的属性 _kd
        self._kd = float(d)
        # 取传入的积分上限 imax 的绝对值并赋值给类的属性 _imax
        self._imax = abs(imax)
        # 初始化上一次的微分值为 NaN
        self._last_derivative = float('nan')

    # 该方法用于根据当前误差计算并返回 PID 控制器的输出值
    def get_pid(self, error, scaler):
        # 获取当前的毫秒级时间
        tnow = millis()
        # 计算当前时间与上一次计算时间的差值（时间间隔）
        dt = tnow - self._last_t
        # 初始化 PID 控制器的输出值为 0
        output = 0

        # 如果是第一次计算（_last_t 为 0）或者时间间隔超过 1000 毫秒
        if self._last_t == 0 or dt > 1000:
            # 将时间间隔设为 0
            dt = 0
            # 重置积分项
            self.reset_I()

        # 更新上一次计算的时间为当前时间
        self._last_t = tnow
        # 将时间间隔从毫秒转换为秒
        delta_time = float(dt) / float(1000)

        # 计算比例项并累加到输出值中
        output += error * self._kp

        # 如果微分系数的绝对值大于 0 且时间间隔大于 0
        if abs(self._kd) > 0 and dt > 0:
            # 如果上一次的微分值为 NaN
            if isnan(self._last_derivative):
                # 初始化微分值为 0
                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

        # 如果积分系数的绝对值大于 0 且时间间隔大于 0
        if abs(self._ki) > 0 and dt > 0:
            # 计算积分项并累加到积分器中
            self._integrator += (error * self._ki) * scaler * delta_time
            # 对积分器的值进行限幅处理，确保不超过积分上限
            if self._integrator < -self._imax:
                self._integrator = -self._imax
            elif self._integrator > self._imax:
                self._integrator = self._imax
            # 将积分项累加到输出值中
            output += self._integrator

        # 返回最终的 PID 控制器输出值
        return output

    # 该方法用于重置积分器和上一次的微分值
    def reset_I(self):
        # 将积分器的值重置为 0
        self._integrator = 0
        # 将上一次的微分值重置为 NaN
        self._last_derivative = float('nan')