openmv配机器人的色块追踪

#自动追踪红色小块，并根据机器人移动，找到红色小块
import sensor, image, time
from pyb import UART
from pyb import Pin


print("Tracking some color blocks！")


#给stm32发送启动信息
uart = UART(1, 9600)
uart.write("Waitting for beginning \r")


#设置p_in为输入引脚，并开启上拉电阻
p_in = Pin('P9', Pin.IN, Pin.PULL_UP)
value = p_in.value()


#等待32反馈
while(not value):
    print("waitting for stm32/ardunio ")
    time.sleep(200)
time.sleep(1000)


#自动追踪开始工作
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()


#QVGA的色域中心
r = [(320//2)-(50//2), (240//2)-(50//2), 50, 50]




for i in range(60):
    img = sensor.snapshot()
    img.draw_rectangle(r)


print("Learning thresholds...")
threshold = [50, 50, 0, 0, 0, 0] # Middle L, A, B values.
for i in range(60):
    img = sensor.snapshot()
    hist = img.get_histogram(roi=r)
    lo = hist.get_percentile(0.01) # Get the CDF of the histogram at the 1% range (ADJUST AS NECESSARY)!
    hi = hist.get_percentile(0.99) # Get the CDF of the histogram at the 99% range (ADJUST AS NECESSARY)!
    # Average in percentile values.
    threshold[0] = (threshold[0] + lo.l_value()) // 2
    threshold[1] = (threshold[1] + hi.l_value()) // 2
    threshold[2] = (threshold[2] + lo.a_value()) // 2
    threshold[3] = (threshold[3] + hi.a_value()) // 2
    threshold[4] = (threshold[4] + lo.b_value()) // 2
    threshold[5] = (threshold[5] + hi.b_value()) // 2
    for blob in img.find_blobs([threshold], pixels_threshold=150, \
                               area_threshold=150, merge=False, margin=10):
        img.draw_rectangle(blob.rect())
        img.draw_cross(blob.cx(), blob.cy())
        img.draw_rectangle(r)


#显示计数器
fps_counter=0
move_left_count=0
move_right_count=0


#事件计数器
miss_count=0 #追踪丢失计数
miss_check_count=0 #丢失检查计数
blob_x=[] #记录blob.x
blob_y=[] #记录blob.y
blob_count=0


#事件标志
missing_flag=0


#控制频率
freq_move=32


#开始进行图像处理
while(True):
    clock.tick()
    img = sensor.snapshot()
    for blob in img.find_blobs([threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10):
        img.draw_rectangle(blob.rect())
        img.draw_cross(blob.cx(), blob.cy(),size=25,color=(0,0,0))


#记录blob的左上角点坐标
    blob_count=blob_count+1
    if blob_count<17 :
        blob_x.append(blob.x())
        blob_y.append(blob.y())
    else :
        blob_count=0
        del blob_x[0]
        del blob_y[0]
        blob_x.append(blob.x())
        blob_y.append(blob.y())


#判断是否追踪丢失


    miss_check_count=miss_check_count+1
    if miss_check_count==32:
        miss_check_count=0
        if blob_x[-1]==blob_x[-2] and \
           blob_x[-2]==blob_x[-3] and \
           blob_x[-3]==blob_x[-4] and \
           blob_x[-4]==blob_x[-5]:


            #连续五个坐标相同，可断定为目标丢失
            #并通知取消转向判断
            missing_flag=1
            print("Missing ! Missing ! ")
        else:
            missing_flag=0




#打印当前刷新率


    fps_counter=fps_counter+1
    if fps_counter==16 :
        print(clock.fps())
        #test
        '''
        print(blob.x())
        print(blob.y())
        '''
        #test
        fps_counter=0


#判断距离
    if blob.area() < 15000 and (not missing_flag):
        print("go head")








#判断转向
    if blob.cx()>180 and blob.cx()<320 and (not missing_flag):
        move_right_count=move_right_count+1
        if move_right_count ==freq_move :
            print("move right !")
            move_right_count=0


            #通知
            #uart.write("r")






    elif blob.cx()>0 and blob.cx()<140 and (not missing_flag):
        move_left_count=move_left_count+1
        if move_left_count ==freq_move :
            print("move left !")
            move_left_count=0


            #通知
            #uart.write("l")