#导入函数库
import sensor, image, time, math ,lcd
clock = time.clock()# 跟踪FPS帧率
kernel_size = 1 # 3x3==1, 5x5==2, 7x7==3, etc.
#初始卷积核
kerne1 = [-1, -1,  -1, \
          -1,  9,  -1, \
           -1,  -1,  -1]
#变量初始化
K=327#测距比例常数 s=k*pix
Red_TH      = (46, 79, 28, 65, -24, 63)#红色阈值
Blue_TH     = (24, 63, -12, 33, -69, -25)#蓝色阈值
#TFT-LCD初始化
lcd.init()                #lcd函数初始化
lcd.set_direction(1)      #设置LCD显示方向 0和2是竖屏 1和3是横屏
#函数定义
def Get_MaxIndex(blobs):
    maxb_index=0                          #最大色块索引初始化
    max_pixels=0                          #最大像素值初始化
    for i in range(len(blobs)):           #对N个色块进行N次遍历
        if blobs[i].pixels() > max_pixels:#当某个色块像素大于最大值
            max_pixels = blobs[i].pixels()#更新最大像素
            maxb_index = i                #更新最大索引
            return  maxb_index
#摄像头初始化
sensor.reset()                     #初始化相机传感器
sensor.set_pixformat(sensor.RGB565)#设置相机模块的像素模式 16 bits/像素 GRAY为8
sensor.set_framesize(sensor.QQVGA) #设置相机模块的帧大小 160x120
sensor.skip_frames(30)             #跳过30帧 让相机图像在改变相机设置后稳定下来
sensor.set_auto_gain(False)        #关闭自动增益
sensor.set_auto_whitebal(False)    #关闭默认的白平衡
#主函数
while (1):
    clock.tick()
	OBS_img = sensor.snapshot()#镜头畸变校正 去除镜头的鱼眼效果
	OBS_img.mean(1)#均值滤波
	OBS_img.morph(kernel_size, kerne1)#卷积核滤波
	#在LCD上打印帧率和OpenMV工作模式
	OBS_img.draw_string(0, 20,"OpenMv Mode:\r(2)Obstacle Mode")
	OBS_img.draw_string(0, 0, "FPS:%.2f"%(clock.fps()))
	#寻找色块 (颜色阈值,ROI区域,像素阈值,区域阈值)
	blobs=OBS_img.find_blobs(OBS_TH1,pixels_threshold=100, 				                                            area_threshold=100,merge=True)
    if blobs:#找到了黄色色块
                    maxb_index =Get_MaxIndex(blobs)#找到最大色块并返回索引值
                    #返回最大色块外框元组(x,y,w,h) 绘制线宽为2的矩形框 不填充矩形
                    OBS_img.draw_rectangle(blobs[maxb_index].rect(), thickness = 2, fill = False)
                    #最大色块的中心位置标记十字
                    OBS_img.draw_cross(blobs[maxb_index].cx(),blobs[maxb_index].cy())
                    maxb= blobs[maxb_index]#定义最大色块为maxb
                    CPix = (maxb[2]+maxb[3])/2#色块的23索引可以获得色块宽度和高度 近似代替实物像素
                    Lm =round((K/CPix),2) #实际距离和像素大小成反比 圆整到小数后两位
                    OBS_img.draw_string(0, 0, "FPS:%.2f"%(clock.fps()))#LCD上显示帧率
                    OBS_img.draw_string(80, 0,"Dis:%.2fcm"%Lm)     #LCD上显示距离
                    print("Dis:%.2fcm"%Lm)
                    if Lm > OBS_DisTH:#如果距离大于阈值
                                OBS_img.draw_string(0, 10,"Distance is Ok")#LCD显示距离正常
                    if Lm <= OBS_DisTH:#如果距离小于阈值
                                OBS_img.draw_string(0, 10,"Dis is Not Ok ")#LCD显示距离过小
                else :#如果没找到色块 则距离也是正常 相当于距离大于阈值
                        OBS_img.draw_string(0, 10,"Distance is Ok")#LCD显示距离正常
                color_num=0
                blobs1 = OBS_img.find_blobs([Red_TH],pixels_threshold=100)
                if blobs1:
                    color_num=1
                    print("物体颜色:红色")
                    OBS_img.draw_string(0, 40,"Object:\r Red")
                else :
                    blobs2 = OBS_img.find_blobs([Blue_TH],pixels_threshold=100)
                    if blobs2:
                        color_num=2
                        print("物体颜色:蓝色")
                        OBS_img.draw_string(0, 40,"Object:\r Blue")
                lcd.display(OBS_img)#LCD显示图像