【2023年电赛】运动目标控制与自动追踪系统（E 题）最简单实现

本方案的思路是最简单的不涉及复杂算法：识别矩形框，标记矩形框，输出坐标和中心点，计算长度，控制舵机移动固定长度！仅供完成基础功能参考，不喜勿喷！

# 实现运动目标控制与自动追踪系统

## 任务概述
本文将介绍如何使用OpenMV开发板和舵机构建一个运动目标控制与自动追踪系统。该系统包括模拟目标运动的红色光斑位置控制系统和指示自动追踪的绿色光斑位置控制系统。通过本文的实现，我们可以在图像中识别目标，控制舵机沿着目标移动，并输出目标矩形框的中心位置和长度。

## 硬件准备
1. OpenMV H7 Plus开发板
2. 红色和绿色激光笔
3. 两个舵机（连接到OpenMV开发板）

## 硬件连接
将两个舵机分别连接到OpenMV开发板的舵机引脚（根据实际引脚选择）。

## 相机设置
在代码中，我们将相机设置为QVGA分辨率和RGB565格式。

import sensor, image, math, pyb

# 初始化相机
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time=2000)

# 其他代码...

## 目标检测与跟踪（目标检测部分代码实测过！）
### 寻找矩形函数
为了在图像中识别目标矩形框，我们需要编写一个寻找矩形函数。该函数将返回第二大的矩形区域，以便我们可以找到目标的位置。

# 寻找矩形函数，返回第二大的矩形区域
def find_second_largest_rectangle(blobs):
    max_area = 0
    max_blob = None
    second_max_area = 0
    second_max_blob = None

    for blob in blobs:
        area = blob.area()
        if area > max_area:
            second_max_area = max_area
            second_max_blob = max_blob
            max_area = area
            max_blob = blob
        elif area > second_max_area:
            second_max_area = area
            second_max_blob = blob

    return second_max_blob

## 主循环
在主循环中，我们将不断获取图像并进行目标检测和跟踪。

while True:
    img = sensor.snapshot() # 获取图像
    blobs = img.find_blobs([black_threshold], pixels_threshold=200, area_threshold=200)

    if blobs:
        # 寻找第二大的矩形区域
        second_max_blob = find_second_largest_rectangle(blobs)
        if second_max_blob:
            img.draw_rectangle(second_max_blob.rect(), color=(255, 0, 0), thickness=4) # 绘制第二大的矩形框，颜色为红色

            # 其他代码...

## 舵机控制
为了实现目标跟踪，我们需要控制舵机使其沿着矩形框移动一圈，并回到中心点。这里我们使用了`pyb.Servo()`来控制舵机运动。

# 舵机参数
servo_pan_pin = 1 # 舵机1的引脚编号
servo_tilt_pin = 2 # 舵机2的引脚编号
servo_pan = pyb.Servo(servo_pan_pin) # 初始化舵机1
servo_tilt = pyb.Servo(servo_tilt_pin) # 初始化舵机2
servo_speed = 50 # 舵机转动速度（0-100，越大越快）
servo_pan_range = (0, 180) # 舵机1转动范围（角度）
servo_tilt_range = (0, 180) # 舵机2转动范围（角度）

# 控制舵机沿着矩形框移动一圈，并回到中心点
for angle in range(servo_pan_range[0], servo_pan_range[1], servo_speed):
    servo_pan.angle(angle) # 控制舵机1水平旋转
    servo_tilt.angle(angle) # 控制舵机2垂直旋转
    pyb.delay(100) # 延时一段时间，控制舵机转动速度

# 将舵机回到中心点
servo_pan.angle((servo_pan_range[0] + servo_pan_range[1]) // 2)
servo_tilt.angle((servo_tilt_range[0] + servo_tilt_range[1]) // 2)

## 结果输出
在检测到目标后，我们将输出目标矩形框的中心位置和长度。同时，我们会在图像中标记出矩形框的位置。（测试坐标和长度还是比较准备）

# 获取矩形框的中心点坐标
x, y = second_max_blob.cx(), second_max_blob.cy()

# 计算矩形框的长度和宽度（单位：厘米）
width_cm = 2 * distance_cm * math.tan(math.radians(H_FOV / 2)) * (second_max_blob.w() / img.width())
height_cm = 2 * distance_cm * math.tan(math.radians(V_FOV / 2)) * (second_max_blob.h() / img.height())

# 输出矩形框的中心点坐标和长度（单位：厘米）
print("Rectangle Center Coordinates (cm): x={}, y={}".format(x, y))
print("Rectangle Width (cm): {}, Height (cm): {}".format(width_cm, height_cm))

# 绘制黄色圆点标记矩形框中心位置
img.draw_circle(x, y, 5, color=(255, 255, 0), thickness=2)

## 运行效果
将硬件连接好后，上传代码到OpenMV开发板，并调整舵机参数和位置。运行代码后，你将看到舵机沿着目标矩形框移动，并在图像中标记出矩形框的位置和中心点。