查看: 2926|回复: 7

[M10项目] 行空板电机扩展板 apriltag小车跟踪项目

本帖最后由 daybreak21 于 2024-9-10 20:47 编辑

一、项目简介

前不久，我拿到了df的行空板电机拓展版，让我觉得行空板的功能可以进一步拓展。我原来用哈士奇（二哈识图）做过小车跟随项目，用下来也挺稳定的。但是，大家知道，哈士奇就是个近视眼，对于高像素远距离的目标，它就很累；此外，哈士奇让我们快速的上手各种机器视觉项目，却封装的十分彻底，我无法了解检测的原理，作为一名教师，总觉得想让学生知道个所以然，所以，我在暑假研究了这样一个项目。能力有限，问题较多，请大家包含。

二、Apriltag介绍
AprilTag是由密歇根大学的APRIL Robotics实验室提出的视觉基准系统。它在增强现实、机器人和相机校准等领域广泛应用。通过识别AprilTag标签，可以确定计算标记相对于相机的精确3D位置、方向和标识，相对于传统QR二维码，它降低了复杂度以满足实时性需求，并且这些标签可以用打印机打印。以下是一个标签的示例图像：

行空板电机扩展板 apriltag小车跟踪项目图1

Apriltag的编码原理

Apriltag标签的数据有效承载通常为4到12位编码。

以下是每个标签家族的宽度（位数）和可用标签数量，在选择最复杂的设计之前，还要考虑到对于给定的物理标签尺寸，例如一个直径 25 厘米的标签，设计越复杂，数据位的尺寸越小，因此标签必须更近才能有足够的像素进行正确检测。

Family Name	Total Width	Width of Square	Fill Factor	Number Of Tags
16H5	8	6	0.75	30
21H7 Circle	9	5	0.55	38
25H9	9	7	0.78	35
36H11	10	8	0.8	587
41H12 Standard	9	5	0.56	2115

关于编码长度：
  以AprilTag 16H5为例，“16”表示编码长度为 16 位，“H5”表示标签之间的最小汉明距离为 5。这意味着任意两个标签之间至少有 5 位是不同的。较大的汉明距离可以提高标签识别率，因为即使有一些位被错误识别，标签仍然可以被正确区分。每个标签都可以通过编号来识别。标签设计还包含一些错误检测和纠正功能，因此即使读取到一两个位的错误，也可以正确识别出标签。
  假设我们有一个标签系统，每个标签用 16 位二进制数表示（即编码长度为 16）。可能的标签有：

0000 0000 0000 0000
0000 0000 0000 0001
0000 0000 0000 0010
0000 0000 0000 0011
…

这样，我们可以生成 (2^{16}) 个不同的标签，每个标签都有一个唯一的 16 位二进制编码。

关于汉明距离：
汉明距离是指两个二进制数之间不同位的数量。举个例子：

标签 A：0000 0000 0000 0101
标签 B：0000 0000 0000 1100

我们比较这两个标签的每一位：

第 1-12 位：相同
第 13 位：0 和 1 不同
第 14 位：1 和 1 相同
第 15 位：0 和 1 不同
第 16 位：1 和 0 不同

所以，标签 A 和标签 B 之间的汉明距离是 3，因为它们有 3 位不同。

为了在识别率和帧率之间取得平衡，本项目，我选用apriltag 25H9进行检测，一块4*4cm的标签，使用320*240摄像头分辨率，在一米以外仍可以检测，帧率在15fps，并且不出错。

三、认识行空板与电机扩展板

1、关于行空板
行空板是一款专为Python学习和使用设计的新一代国产开源硬件，采用单板计算机架构，集成LCD彩屏、WiFi蓝牙、多种常用传感器和丰富的拓展接口。同时，其自带Linux操作系统和Python环境，还预装了常用的Python库，让广大师生只需两步就能进行Python教学。
行空板电机扩展板 apriltag小车跟踪项目图8

2、行空板双路电机扩展板
行空板电机扩展板 apriltag小车跟踪项目图4

此产品是专为行空板（UNIHIKER）设计的适配扩展板，支持2路直流电机，简化控制流程。采用创新的倾斜金手指插槽设计，为屏幕提供最佳可视角度。集成了两路直流电机驱动，支持独立电源供电，并配备了RGB灯、红外发射与接收功能，以及10路3Pin口和4路I2C口的扩展能力，配合DFRobot强大的Gravity产品体系，让行空板的创意实现更加多样化。
  选择双路电机扩展板的原因，是因为其为行空板专门设计，并且在小车控制中，双路差速控制是最简单的控制方法了，便于初学者学习。

3、行空板有线无线连接
有线连接：请看官网 https://www.unihiker.com.cn/wiki/get-started 快速使用教程
无线连接：请看官网 https://mc.dfrobot.com.cn/thread-316050-1-1.html
         不需要使用typec，但需要wifi以及外部供电

四、环境搭建

环境搭建：请看官网 mind+ 库管理  https://mindplus.dfrobot.com.cn/Python-code
行空板电机扩展板 apriltag小车跟踪项目图5

完整环境搭建步骤

安装库文件需要行空板wifi联网

运行 pip install apriltag 安装apriltag

行空板电机扩展板 apriltag小车跟踪项目图6

五、小车准备
为了适应行空板控制，我设计了一个小车底盘，摄像头安放在小车侧方，双前轮作为车头方向，使用高减速比的n20电机（减速比关乎到小车行驶的稳定性，太快或者太慢都会影响行驶体验，需要做些测试），2节锂电池串联装在小车底部。整体电压在8V左右，其左侧控制端口为P6、P16~，右侧控制端口为P5、P8~。

行空板电机扩展板 apriltag小车跟踪项目图11

以下是我用的万向轮，0.5寸，我找了很久，大家可淘宝自寻
行空板电机扩展板 apriltag小车跟踪项目图20

以下是小车车架打印文件，大家可以下载

https://mc.dfrobot.com.cn/forum.php?mod=attachment&aid=MTc3MzAwfGFkZDY3OWIzZmJlNGFlMTE1MmFiNTg4YWM4YzQyM2Q0fDE3MzQ1MTU3Nzc%3D&request=yes&_f=.3mf

https://mc.dfrobot.com.cn/forum.php?mod=attachment&aid=MTc3Mjk5fGIxYThhOWU0YjNlNjIwNmRiNDFkNzJhN2IzN2VjOTZmfDE3MzQ1MTU3Nzc%3D&request=yes&_f=.3mf

行空板电机扩展板 apriltag小车跟踪项目图16

根据产品文档：https://wiki.dfrobot.com.cn/SKU_DFR1136_%E8%A1%8C%E7%A9%BA%E6%9D%BF%E5%8F%8C%E8%B7%AF%E7%94%B5%E6%9C%BA%E9%A9%B1%E5%8A%A8IO%E6%89%A9%E5%B1%95%E6%9D%BF_DC_Motor_Driver_Carrier#target_0
行空板电机扩展板 apriltag小车跟踪项目图12

每个电机使用2根信号线控制，一个信号口输出高低电平控制电机转向，另外一个信号口输出pwm控制电机转速。P7控制M1转向，P16控制M1速度，P6控制M2转向，P8控制M2转速，PWM值为0~1023，比Arduino精度高很多。

以下是文档中电机控制程序，可以先用来测试小车

行空板电机扩展板 apriltag小车跟踪项目图13

六、项目研究思路

如果要尽量接近实验结果，我们需要一步步解决问题，上一个没问题了，再进行下一个步骤。确认没问题的部分不要轻易改动。
1、使用python+opencv+摄像头组合，捕获图像，并调用apriltag检测画面中的标签。
2、如果发现一个或多个标签，则计算标签的最大外接矩形，挑选其中最大的。
3、在画面最大的标签中，进一步提取、计算标签的宽度和中心点坐标。
4、将关键信息与电机pwm值进行映射，获得左右电机的pwm当前值，并驱动小车行驶。

图像坐标与电机差速控制关系计算
行空板电机扩展板 apriltag小车跟踪项目图15

OpenCV可以回传标识物的空间信息，当分辨率是320*240像素时。经过实验检测，它看到标识物的最大的像素宽度尺寸Width是80，最小的像素尺寸是11（可能会因为标识物不同有所区别）。标识物中心位置X，最左边达到50，最右边是270；

行空板控制电机转速PWM脉冲的最大值是1023，为了便于计算与行驶，我先把PWM限定在255内。此刻，我假设在直角坐标系中，当opencv反馈目标物宽度为65时，目标中心位置为200时，左右电机转速值PWM各是多少？

第一步：坐标映射
为了进行计算，需要将OpenCV反馈的目标物宽度和中心位置映射到0~255的坐标系中：

对于宽度，给定范围是30到80像素，可以将其映射到[-25, 25]的坐标系。
对于中心位置，给定范围是50到270像素，映射到[-255, 255]的坐标系。

Y轴（宽度到速度的映射）
假设目标物的宽度是65像素，宽度范围是30到80像素，目标物宽度的变化量是：
Y_range = 80 - 30 = 50
我们可以将其映射到[-25, 25]范围，并计算出相应的值：
Y_mapped = ((65 - 30) / 50) * 25 = 17.5
接下来将其映射到0~255的范围：
Y = (17.5 / 25) * 255 = 102

X轴（位置到转向的映射）
假设目标物中心位置为200像素，中心位置的变化范围是从50到270像素，总范围为：
X_range = 270 - 50 = 220
映射到[-255, 255]范围：
X_mapped = ((200 - 50) / 220) * 510 - 255 = 113.64

第二步：确定左右电机的速度关系
在这个系统中，左右电机的转速受两个因素的影响：

Y轴值控制整体的前进速度（左右电机的基准速度相同）。
X轴值控制转向时两侧电机的差速。X > 0 表示右转，X < 0 表示左转。

因此，左右电机的速度计算可以表达为：

左电机转速：L = Y + (X / 255) * Y
右电机转速：R = Y - (X / 255) * Y

为了保证速度值在0到255的范围内，需要使用 constrain() 函数来限制电机的转速不超出该范围。

第三步：公式推导
左右电机速度的最终计算公式为：
L = constrain(Y + (X / 255) * Y, 0, 255)R = constrain(Y - (X / 255) * Y, 0, 255)
其中：

Y 是前进速度，范围是0~255。
X 是转向值，范围是-255~255。X > 0 时左电机加速，右电机减速；X < 0 时左电机减速，右电机加速。

让我们总结一下：
基本原则是：两个电机的基础速度由Y轴值确定。两个电机的速度差由X轴值确定。如果X轴为200，则右侧电机R要减去g的部分，左侧电机L要加上i的部分。i、g的值不仅仅由x决定，而是要计算x的变化幅度在y上会改变多少（x/255*Y）

行空板电机扩展板 apriltag小车跟踪项目图7

七、分步实施步骤：

本项目opencv部分，我用模块化编程做了个简易测试。然因为在模块化中编写复杂代码效率太低，并且一些opencv函数并为放在其中，所以，整体项目我还是使用代码编写。

opencv摄像头+apriltag+opencv文本显示+电机控制+小车调试
行空板电机扩展板 apriltag小车跟踪项目图16

如果使用代码编写，可在行空板中新建py文件，并在mind+代码窗口中编译。程序编写完成，可以保存代码，并直接运行。
我的小车因为使用行空板wifi连接，所以就不用数据线了，调试也更加方便。

八、调试改进

  即便你在理论或解题思路上没有问题，但是，在测试过程中，最主要的还是需要调试。如果不经过调试，小车根本无法稳定运行。
以下是一些心得与建议：

1、观察小车运行状态，调节各种参数
  起初运行时，小车无法跟随二维码，这会出现两种状况，一是小车转向不足，应该是左转或右转的车轮转速不够；二是转向过度，这应该是由于车轮行驶惯性较大，即便电机停机，其也会多滚动半圈，再加上如果用单线程的程序方法，响应不够及时，所以要多方处理。我的简单方法就是在pwm值低于200时（0~1023），我干脆就把PWM归零了。

2、多线程运行，用满cpu载荷。

  许多静态项目在使用行空板运行时，并未考虑时效问题，但在opencv以及小车行驶项目中，需要更快的检测速度，否则经常错过目标。我在代码编写完成测试后，opencv帧率一直维持在16帧，这一度让我感到成功无望。行空板是4核处理器，询问AIGC，提示可以使用多线程方式，通过def u_thread2_function():函数，我将目标检测与小车行驶、屏幕信息显示放在不同的进程中。经过对比，多线程可以将帧率提高到24帧，这样小车灵敏多了。
还有许多参数调节的地方，我无法一一回忆，只希望大家能亲自尝试。

九、小结
  除了apriltag，其实很多应用我还未能去深度实现，比如用opencv级联器的人脸识别功能跟踪，我尝试过用刘德华的脸，小车可以正常跟踪，只是并为进一步尝试提升帧率。还有就是如果想让小车高速行驶，那就要将PID控制方式嵌入其中，并且要增加摄像头的分辨率（640*480），这我将在后面继续尝试。其实，对于中小学的人工智能教学，这个项目的难度已经足够了，学生们自己完成挑战估计也要花费半个学期的时间。通过本项目，我验证了行空板控制小车的一些可能性。或许很多用户觉得行空板速度太慢了，比如运行深度学习的一些项目，我觉得在硬件型号确定的情况下，如何挖掘硬件的潜力，发挥其更大的价值时非常有意义的，希望和同行们一同交流分享经验。

以下为项目完整代码：

电机测试代码

#  -*- coding: UTF-8 -*-

# MindPlus
# Python
import time
from pinpong.board import Board,Pin
from pinpong.extension.unihiker import *


Board().begin()
p_p5_out=Pin(Pin.P5, Pin.OUT)
p_p8_pwm=Pin(Pin.P8, Pin.PWM)
p_p6_out=Pin(Pin.P6, Pin.OUT)
p_p16_pwm=Pin(Pin.P16, Pin.PWM)

while True:
    p_p5_out.write_digital(1)
    p_p8_pwm.write_analog(512)
    p_p6_out.write_digital(1)
    p_p16_pwm.write_analog(512)
    time.sleep(1)
    p_p8_pwm.write_analog(0)
    p_p16_pwm.write_analog(0)
    time.sleep(1)
    p_p5_out.write_digital(0)
    p_p8_pwm.write_analog(512)
    p_p6_out.write_digital(0)
    p_p16_pwm.write_analog(512)
    time.sleep(1)
    p_p8_pwm.write_analog(0)
    p_p16_pwm.write_analog(0)
    time.sleep(1)
复制代码

单线程整体代码

import cv2  # 导入OpenCV库，用于图像处理和摄像头捕获
import apriltag  # 导入AprilTag库，用于二维码检测
import time  # 导入time模块，用于时间相关功能
from pinpong.board import Board, Pin  # 从pinpong库导入Board和Pin类，用于MindPlus板控制

# 初始化摄像头
cap = cv2.VideoCapture(0)  # 创建摄像头对象，参数0表示使用第一个摄像头设备
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320)  # 设置摄像头捕获帧的宽度为320像素
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)  # 设置摄像头捕获帧的高度为240像素

# 初始化AprilTag检测器
options = apriltag.DetectorOptions(families="tag25h9")  # 定义AprilTag检测器的参数，选择tag25h9家族的二维码
detector = apriltag.Detector(options)  # 创建AprilTag检测器实例

# 创建全屏显示窗口
cv2.namedWindow('AprilTag Detection', cv2.WND_PROP_FULLSCREEN)  # 创建名为'AprilTag Detection'的窗口
cv2.setWindowProperty('AprilTag Detection', cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)  # 将窗口设置为全屏显示

# 初始化帧率计算变量
fps = 0  # 存储计算得到的帧率
frame_count = 0  # 帧计数器，用于计算帧率
start_time = time.time()  # 记录开始时间，用于计算帧率

# 初始化MindPlus板
Board().begin()  # 初始化MindPlus板对象
p_p5_out = Pin(Pin.P5, Pin.OUT)  # 创建P5引脚对象，并设置为输出模式
p_p6_out = Pin(Pin.P6, Pin.OUT)  # 创建P6引脚对象，并设置为输出模式
p_p8_pwm = Pin(Pin.P8, Pin.PWM)  # 创建P8引脚对象，并设置为PWM输出模式
p_p16_pwm = Pin(Pin.P16, Pin.PWM)  # 创建P16引脚对象，并设置为PWM输出模式

# 定义PWM范围
PWM_MIN = 20  # PWM最小值，对应电机控制的最小脉冲宽度
PWM_MAX = 150  # PWM最大值，对应电机控制的最大脉冲宽度

# 映射函数：将输入值映射到指定的输出范围
def numberMap(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

# 限制函数：将输入值限制在指定的范围内
def constrain(amt, low, high):
    return max(low, min(amt, high))

# 控制小车前进函数
def forward(PWM_L, PWM_R):
    
    if PWM_L < 20: PWM_L = 1023
    if PWM_R < 20: PWM_R = 1023
    # 将PWM值映射到允许的PWM信号范围内，并取整数部分
    pwm_l_int = int(constrain(PWM_L, PWM_MIN, PWM_MAX))
    pwm_r_int = int(constrain(PWM_R, PWM_MIN, PWM_MAX))
    # 激活电机驱动引脚，设置为高电平
    p_p5_out.write_digital(1)
    p_p6_out.write_digital(1)
    # 写入PWM值到电机控制引脚，控制电机转速
    p_p8_pwm.write_analog(pwm_r_int)
    p_p16_pwm.write_analog(pwm_l_int)

# 停止小车函数
def STOP():
    # 写入0到PWM引脚，停止电机
    p_p8_pwm.write_analog(0)
    p_p16_pwm.write_analog(0)

# 主循环
while True:
    # 读取摄像头的下一帧
    ret, frame = cap.read()
    # 如果读取失败，则退出循环
    if not ret:
        break

    # 将当前帧转换为灰度图像，便于二维码检测
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    # 使用AprilTag检测器检测灰度图像中的二维码
    tags = detector.detect(gray)

    # 如果没有检测到二维码，调用STOP函数停止小车
    if not tags:
        STOP()
    else:
        # 如果检测到二维码，找到面积最大的二维码
        max_tag = max(tags, key=lambda tag: tag.corners[2][0] - tag.corners[0][0])
        # 计算最大二维码的中心点横坐标
        x = int(max_tag.center[0])
        # 计算最大二维码的宽度
        w = int(max_tag.corners[2][0] - max_tag.corners[0][0])

        # 使用映射函数将二维码中心点横坐标映射到PWM值
        x_mapped = numberMap(x, 0, 320, -(PWM_MAX), (PWM_MAX ))
        # 使用映射函数将二维码宽度映射到PWM值
        w_mapped = numberMap(w, 40, 13, PWM_MIN, PWM_MAX)

        # 根据映射结果计算左右电机的PWM值
        PWM_L = int(constrain(w_mapped + ((x_mapped/PWM_MAX)*w_mapped), PWM_MIN, PWM_MAX))
        PWM_R = int(constrain(w_mapped - ((x_mapped/PWM_MAX)*w_mapped), PWM_MIN, PWM_MAX))

        # 调用forward函数，根据计算得到的PWM值驱动小车前进
        forward(PWM_L, PWM_R)

    # 遍历所有检测到的二维码，绘制边框并显示标签ID
    for tag in tags:
        for idx in range(len(tag.corners)):
            # 绘制二维码边框
            cv2.line(frame, tuple(tag.corners[idx - 1].astype(int)), tuple(tag.corners[idx].astype(int)), (0, 255, 0), 2)
        # 在二维码中心显示标签ID
        cv2.putText(frame, str(tag.tag_id), (int(tag.center[0]), int(tag.center[1])), cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 0, 255), 2)

    # 更新帧率计算
    frame_count += 1
    elapsed_time = time.time() - start_time
    # 每过一秒钟，计算一次帧率
    if elapsed_time >= 1:
        fps = frame_count / elapsed_time
        frame_count = 0
        start_time = time.time()

    # 构建要显示的信息文本，包括FPS、标签数量、最大标签的ID、宽度和中心X坐标
    info_text = f"FPS: {int(fps)}\n"
    if tags:
        info_text += f"Tag ID: {max_tag.tag_id}\nTag Width: {w}\nCenter X: {x}\nPWM L/R: {PWM_L}/{PWM_R}"
    # 在窗口左下角显示信息文本
    for i, line in enumerate(info_text.split('\n')):
        cv2.putText(frame, line, (10, frame.shape[0] - 25 - (i * 25)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 200, 64), 2)

    # 在创建的窗口中显示处理后的帧
    cv2.imshow('AprilTag Detection', frame)

    # 检测按键，如果按下'q'键，则退出循环
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

# 循环结束后，释放摄像头资源并关闭所有OpenCV窗口
cap.release()  # 释放摄像头资源
cv2.destroyAllWindows()  # 关闭所有OpenCV窗口
复制代码

多线程整体代码

<font size="3">import cv2
import apriltag
import time
import threading
from pinpong.board import Board, Pin
from unihiker import GUI

# 初始化摄像头
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)

# 初始化AprilTag检测器
options = apriltag.DetectorOptions(families="tag25h9")
detector = apriltag.Detector(options)

# 初始化MindPlus板
Board().begin()
p_p5_out = Pin(Pin.P5, Pin.OUT)
p_p6_out = Pin(Pin.P6, Pin.OUT)
p_p8_pwm = Pin(Pin.P8, Pin.PWM)
p_p16_pwm = Pin(Pin.P16, Pin.PWM)

# 定义PWM范围
PWM_MIN = 0
PWM_MAX = 700

# 全局变量
frame = None
tags = []
fps = 0
max_tag, w, x, PWM_L, PWM_R = None, None, None, None, None
lock = threading.Lock()

# 创建GUI对象
u_gui = GUI()

# 映射函数
def numberMap(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_max) + out_min

def numberMap(x, in_min, in_max, out_min, out_max):
    # 避免分母为零的情况
    if in_max == in_min:
        raise ValueError("Input range cannot have zero width")
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min


# 限制函数
def constrain(amt, low, high):
    return max(low, min(amt, high))

# 电机控制函数
def forward(PWM_L, PWM_R):
    #防止左右转走过头，所以在pwm低于某个数值时干脆停车
    if PWM_L < 200:
        PWM_L = 0
    if PWM_R < 200:
        PWM_R = 0
    pwm_l_int = int(constrain(PWM_L, PWM_MIN, PWM_MAX))
    pwm_r_int = int(constrain(PWM_R, PWM_MIN, PWM_MAX))
    p_p5_out.write_digital(1)
    p_p6_out.write_digital(1)
    p_p8_pwm.write_analog(pwm_r_int)
    p_p16_pwm.write_analog(pwm_l_int)

def STOP():
    p_p8_pwm.write_analog(0)
    p_p16_pwm.write_analog(0)

# 检测线程函数
def u_thread1_function():
    global frame, tags, max_tag, w, x, PWM_L, PWM_R
    while True:
        ret, new_frame = cap.read()
        if not ret:
            break

        gray = cv2.cvtColor(new_frame, cv2.COLOR_BGR2GRAY)
        detected_tags = detector.detect(gray)

        with lock:
            frame = new_frame
            tags = detected_tags

        if not tags:
            STOP()
            max_tag, w, x, PWM_L, PWM_R = None, None, None, None, None
        else:
            max_tag = max(tags, key=lambda tag: tag.corners[2][0] - tag.corners[0][0])
            x = int(max_tag.center[0])
            w = int(max_tag.corners[2][0] - max_tag.corners[0][0])

            # Ensure valid range
            if 0 < x < 320:
                x_mapped = numberMap(x, 0, 320, -PWM_MAX, PWM_MAX)
            else:
                x_mapped = 0

            if 8 < w < 50:
                w_mapped = numberMap(w, 50, 8, PWM_MIN, PWM_MAX)
            else:
                w_mapped = PWM_MIN

            PWM_L = int(constrain(w_mapped + ((x_mapped / PWM_MAX) * w_mapped), PWM_MIN, PWM_MAX))
            PWM_R = int(constrain(w_mapped - ((x_mapped / PWM_MAX) * w_mapped), PWM_MIN, PWM_MAX))


# 电机控制线程函数
def u_thread2_function():
    global PWM_L, PWM_R
    while True:
        with lock:
            if PWM_L is not None and PWM_R is not None:
                forward(PWM_L, PWM_R)
            else:
                STOP()

# 显示线程函数
def u_thread3_function():
    global frame, fps, tags, max_tag, w, x, PWM_L, PWM_R
    frame_count = 0
    start_time = time.time()

    # 设置显示窗口为全屏模式
    cv2.namedWindow('AprilTag Detection', cv2.WND_PROP_FULLSCREEN)
    cv2.setWindowProperty('AprilTag Detection', cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)

    while True:
        frame_count += 1
        elapsed_time = time.time() - start_time
        if elapsed_time >= 1:
            fps = frame_count / elapsed_time
            frame_count = 0
            start_time = time.time()

        with lock:
            if frame is not None:
                display_info(frame, fps, tags, max_tag, w, x, PWM_L, PWM_R)
                cv2.imshow('AprilTag Detection', frame)

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

# 显示信息函数
def display_info(frame, fps, tags, max_tag=None, w=None, x=None, PWM_L=None, PWM_R=None):
    info_text = f"FPS: {int(fps)}\n"
    if tags and max_tag:
        info_text += f"Tag ID: {max_tag.tag_id}\nTag Width: {w}\nCenter X: {x}\nPWM L/R: {PWM_L}/{PWM_R}"
    for i, line in enumerate(info_text.split('\n')):
        cv2.putText(frame, line, (10, frame.shape[0] - 25 - (i * 25)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 200, 64), 2)
    for tag in tags:
        for idx in range(len(tag.corners)):
            cv2.line(frame, tuple(tag.corners[idx - 1].astype(int)), tuple(tag.corners[idx].astype(int)), (0, 255, 0), 2)
        cv2.putText(frame, str(tag.tag_id), (int(tag.center[0]), int(tag.center[1])), cv2.FONT_HERSHEY_SIMPLEX, 2, (0, 0, 255), 2)

# 启动线程
thread1 = u_gui.start_thread(u_thread1_function)
thread2 = u_gui.start_thread(u_thread2_function)
thread3 = u_gui.start_thread(u_thread3_function)

# 主循环，保持程序运行
while True:
    time.sleep(1)</font>
复制代码