【openMV】色块追踪

 实现色块扫描、返回最大色块的中心位置，相对距离，颜色数据。

从图像返回的参考座标系变换到二维笛卡尔座标系：

Blob_X = (MVData[3]<<8)+MVData[4];
Blob_Y = (MVData[5]<<8)+MVData[6];
Blob_X = Blob_X-160;//变换到二维平面座标系
Blob_Y = 120-Blob_Y;

'''
颜色追踪信令包
0&1、包头0xAA、0xAF
2、颜色标志位-->0:无、1:红色、2黄色
3-6-->色块位置数据
3、x轴高位
4、x轴低位
5、y轴高位
6、y轴低位
7、相对于二维平面原点的距离
'''
import sensor, image, time, math
from pyb import UART
uart = UART(3,9600)
thresholds_yellow = (50, 90, -32, 23, 20, 64)#黄色色块LAB阈值
#thresholds_blue = (46, 95, -34, 19, -45, -11)
thresholds_red  = (41, 73, 46, 94, 1, 77)

class clo_blob():
    flag = 0    #0无1红2蓝
    #最大色块中心座标
    position_x_H = 0            #x高八位
    position_x_L = 0            #x低八位
    position_y_H = 0            #y高八位
    position_y_L = 0            #y低八位
    Distance = 0                #相对于(160,120)的距离


def Print_Blob(obj,flag):
    tem.flag = flag
    #色块中心座标分解传输
    tem.position_x_H = obj.cx()>>8
    tem.position_x_L = obj.cx()
    tem.position_y_H = obj.cy()>>8
    tem.position_y_L = obj.cy()
    tem.Distance = int(math.sqrt(math.pow(obj.cx()-160,2) + math.pow(obj.cy()-120,2)))
    #print(tem.Distance)
    #print("X:{}  Y:{}".format(obj.cx()-160,120-obj.cy()))
    Data_Print = bytearray([0xAA,0xAF,
    tem.flag,
    tem.position_x_H,tem.position_x_L,
    tem.position_y_H,tem.position_y_L,
    tem.Distance])

    return Data_Print

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
clock = time.clock()
_roi = [0,0,320,240]#完整屏幕

tem = clo_blob()
Zero_return = bytearray([0xAA,0xAF,
    0x00,0x00,0x00,0x00,0x00,0x00,0x00])#未发现色块返回此数组
count = 0
flag_send = 0
while(True):
    clock.tick()
    img = sensor.snapshot()
    count = 0
    flag_send = 0

    for blob in img.find_blobs([thresholds_yellow], pixels_threshold=200, area_threshold=200, merge=True,roi=_roi):
    #筛选出最大黄色色块
        if count == 0:
            Max_blob = blob
            count += 1
        elif blob.pixels() > Max_blob.pixels():
            Max_blob = blob
    #标定输出黄色色块信息
    if count != 0:
        img.draw_edges(Max_blob.min_corners(), color=(0,0,0))
        img.draw_line(Max_blob.major_axis_line(), color=(0,0,0))
        img.draw_line(Max_blob.minor_axis_line(), color=(0,0,0))
        uart.write(Print_Blob(Max_blob,2))
        #print("yellow:",Max_blob)
        count = 0
        flag_send = 1
    for blob in img.find_blobs([thresholds_red], pixels_threshold=200, area_threshold=200, merge=True,roi=_roi):
    #筛选出最大红色色块
        if count == 0:
            Max_blob = blob
            count += 1
        elif blob.pixels() > Max_blob.pixels():
            Max_blob = blob
    #标定输出红色色块信息
    if count != 0:
        img.draw_edges(Max_blob.min_corners(), color=(0,0,0))
        img.draw_line(Max_blob.major_axis_line(), color=(0,0,0))
        img.draw_line(Max_blob.minor_axis_line(), color=(0,0,0))
        uart.write(Print_Blob(Max_blob,1))
        #print("red:",Max_blob)
        count = 0
        flag_send = 1
    if flag_send == 0:
        uart.write(Zero_return)