2023电赛E题云台激光追踪视觉实现（K210）

这是2023年E题题目。我们采用stm32作为主控，云台舵机控制激光方向，基础部分写死即可，发挥部分按照坐标来运动，坐标由k210识别，与stm32通信。

一、 任务
设计制作一个运动目标控制与自动追踪系统。系统包括模拟目标运动的红色光斑位置控制系统和指示自动追踪的绿色光斑位置控制系统。系统结构示意及摆放位置见图 1（a）。图中两个激光笔固定在各自独立的二维电控云台上。

红色激光笔发射的光斑用来模拟运动目标，光斑落在正前方距离 1m 处的白色屏幕上，光斑直径≤1cm。红色光斑位置控制系统控制光斑能在屏幕范围内任意移动。
绿色激光笔发射的光斑由绿色光斑位置系统控制，用于自动追踪屏幕上的红色光斑，指示目标的自动追踪效果，光斑直径≤1cm。绿色激光笔放置线段如图 1（b）所示，该线段与屏幕平行，位于红色激光笔两侧，距红色激光笔距离大于 0.4m、小于 1m。绿色激光笔在两个放置线段上任意放置。
屏幕为白色，有效面积大于 0.6╳0.6m2。用铅笔在屏幕中心画出一个边长0.5m 的正方形，标识屏幕的边线；所画的正方形的中心为原点，用铅笔画出原点位置，所用铅笔痕迹宽≤1mm。
二、 要求
1. 基本要求
1）设置运动目标位置复位功能。执行此功能，红色光斑能从屏幕任意位置回到原点。光斑中心距原点误差≤2cm。
2）启动运动目标控制系统。红色光斑能在 30 秒内沿屏幕四周边线顺时针移动一周，移动时光斑中心距边线距离≤2cm。
3）用约 1.8cm 宽的黑色电工胶带沿 A4 纸四边贴一个长方形，构成 A4 靶纸。将此 A4 靶纸贴在屏幕自定的位置。启动运动目标控制系统，红色光斑能在30 秒内沿胶带顺时针移动一周。超时不得分，光斑完全脱离胶带一次扣 2 分，连续脱离胶带移动 5cm 以上记为 0 分。
4）将上述 A4 靶纸以任意旋转角度贴在屏幕任意位置。启动运动目标控制系统，要求同（3）。
2. 发挥部分
1）运动目标位置复位，一键启动自动追踪系统，控制绿色光斑能在 2 秒内追踪红色光斑，追踪成功发出连续声光提示。此时两个光斑中心距离应≤3cm。
2）运动目标重复基本要求（3）~（4）的动作。绿色激光笔发射端可以放置在其放置线段的任意位置，同时启动运动目标及自动追踪系统，绿色光斑能自动追踪红色光斑。启动系统 2 秒后，应追踪成功，发出连续声光提示。此后，追踪过程中两个光斑中心距离大于 3cm 时，定义为追踪失败，一次扣 2 分。连续追踪失败 3 秒以上记为 0 分。运动目标控制系统和自动追踪系统均需设置暂停键。同时按下暂停键，红色和绿色光斑应立即制动，以便测量两个光斑中心距离。
3）其他。
三、 说明
1）红色、绿色光斑位置控制系统必须相互独立，之间不得有任何方式通信；光斑直径小于 1cm；屏幕上无任何电子元件；控制系统不能采用台式计算机或笔记本电脑。不符合要求不进行测试。
2）基本要求（3）、（4）未得分不进行发挥部分（2）的测试

矩形识别定位代码，与stm32通信，传入坐标

import sensor
import image
import time
import lcd
from machine import UART
from board import board_info
from fpioa_manager import fm


fm.register(6,fm.fpioa.UART2_TX)
fm.register(8,fm.fpioa.UART2_RX)

# 初始化摄像头
lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.run(1)
sensor.skip_frames()
sensor.skip_frames(time = 2000)     #跳过不稳定画面
# 初始化串口
#uart = UART(UART.UART2, 115200, 8, 0, 0, timeout=1000, read_buf_len=4096)


clock = time.clock()

while(True):
    clock.tick()
    img = sensor.snapshot()
# 自定义函数：识别矩形
# 在图像中寻找矩形
    for r in img.find_rects(threshold = 10000):
        # 判断矩形边长是否符合要求
        if r.w() > 20 and r.h() > 20:
            # 在屏幕上框出矩形
            img.draw_rectangle(r.rect(), color = (255, 0, 0), scale = 4)
            # 获取矩形角点位置
            corner = r.corners()
            # 在屏幕上圈出矩形角点
            img.draw_circle(corner[0][0], corner[0][1], 5, color = (0, 0, 255), thickness = 2, fill = False)
            img.draw_circle(corner[1][0], corner[1][1], 5, color = (0, 0, 255), thickness = 2, fill = False)
            img.draw_circle(corner[2][0], corner[2][1], 5, color = (0, 0, 255), thickness = 2, fill = False)
            img.draw_circle(corner[3][0], corner[3][1], 5, color = (0, 0, 255), thickness = 2, fill = False)

        # 打印四个角点坐标, 角点1的数组是corner[0], 坐标就是(corner[0][0],corner[0][1])
        # 角点检测输出的角点排序每次不一定一致，矩形左上的角点有可能是corner0,1,2,3其中一个
            corner1_str = corner[0][0],corner[0][1]
            corner2_str = corner[1][0],corner[1][1]
            corner3_str = corner[2][0],corner[2][1]
            corner4_str = corner[3][0],corner[3][1]
            #lcd.draw_string(0, 0, corner1_str, lcd.WHITE, lcd.BLACK)
            #lcd.draw_string(0, 20, corner2_str, lcd.WHITE, lcd.BLACK)
            #lcd.draw_string(0, 40, corner3_str, lcd.WHITE, lcd.BLACK)
            #lcd.draw_string(0, 60, corner4_str, lcd.WHITE, lcd.BLACK)
            lcd.display(img)
            print(corner1_str, corner2_str, corner3_str, corner4_str)
            #print(corner2_str)
            #print(corner3_str)
            #print(corner4_str)
    # 显示到屏幕上，此部分会降低帧率
            try:
                while True:
                    # send data per 500ms
                    if time.ticks_ms() - last_time > 500:
                        last_time = time.ticks_ms()
                        uart.write(corner1_str,corner2_str,corner3_str,corner4_str)
                    # read and print data
                    if uart.any():
                        read_data = uart.read()
                        if read_data:
                         print("read_data = ", read_data)
            except:
                pass
    lcd.display(img)

    # 打印帧率

激光识别代码，与stm32通信，传入坐标

import sensor,image,lcd,time
from machine import UART
from board import board_info
from fpioa_manager import fm
from modules import ybserial

#fm.register(15, fm.fpioa.UART1_RX, force=True)
#fm.register(17, fm.fpioa.UART1_TX, force=True)

#fm.register(6,fm.fpioa.UART2_TX)
#fm.register(8,fm.fpioa.UART2_RX)


serial = ybserial()
uart_A = UART(UART.UART2, 115200, 8, 0, 1, timeout=1000, read_buf_len=4096)
#常用初始化
lcd.init()

sensor.reset()                      #复位摄像头

sensor.set_pixformat(sensor.RGB565) #设置像素格式 RGB565
sensor.set_framesize(sensor.QVGA)   #设置帧尺寸 QVGA (320x240)
sensor.skip_frames(time = 2000)     #跳过不稳定画面

#sensor.run(1)
#阈值
red_threshold   = (56, 100, 45, 127, -128, 127)

#寻色函数定义
def find_max(blobs):
    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:
    img=sensor.snapshot()
    blobs = img.find_blobs([red_threshold],merge=True)#把拍摄的一张图片里满足的色块纳入集合中

    if blobs:
            max_blob = find_max(blobs)#调用函数，返回最大色块
            img.draw_circle(80,60,5,color=200)
            img.draw_circle(max_blob.cx(),max_blob.cy(),10,color=200)
            img.draw_rectangle((max_blob.x(),max_blob.y(),max_blob.w(),max_blob.h()),color=(255,0,0))#用红色框出最大色块
            img.draw_string(0,0, "(x,y) =")
            img.draw_string(max_blob.x()+40,max_blob.y(), str(max_blob.cx()))
            img.draw_string(max_blob.x()+60,max_blob.y(), str(max_blob.cy()))#在框图显示色块的中心坐标
            img.draw_string(40,0, str(max_blob.cx()))
            img.draw_string(60,0, str(max_blob.cy()))#在框图左上角显示色块的中心坐标

            print(max_blob.cx(),end=',')
            print(max_blob.cx(),end='\n')
            coordinate_str = "({}, {})".format(str(max_blob.cx()), str(max_blob.cy()))
            print(coordinate_str)

            num = serial.send(coordinate_str)
    lcd.display(img)