【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)