openmv

import sensor, image, time
from pyb import UART,LED,Timer

#Thresholds是你想识别的颜色的阙值，注意它是一个列表对象
black_threshold   = (0，50，-10，10，-10，10)   #字母A阈值

# 为了使色彩追踪效果真的很好，你应该在一个非常受控制的照明环境中。
# IR红外图像中明亮的白光的阈值
# 设置黑色的阈值，括号里面的数值分别是L A B 的最大值和最小值（minL, maxL, minA,
# maxA, minB, maxB），LAB的值在图像左侧三个坐标图中选取。如果是灰度图，则只需
# 设置（min, max）两个数字即可。

sensor.reset() # 初始化sensor，初始化摄像头
sensor.set_pixformat(sensor.RGB565) # use RGB565.设置采集到照片的格式：彩色RGB
#设置图像色彩格式，有RGB565色彩图和GRAYSCALE灰度图两种
sensor.set_framesize(sensor.QQVGA) # 使用QQVGA 120×160。
#设置图像像素大小
sensor.skip_frames(10) # 让新的设置生效，跳过10帧
sensor.set_auto_whitebal(False) # turn this off.
#关闭白平衡。白平衡是默认开启的，在颜色识别中，需要关闭白平衡。
clock = time.clock() # 跟踪FPS帧率

#指示灯
led1 = LED(1)   #红灯
led2 = LED(2)   #蓝灯
led3 = LED(3)   #红灯

#初始化串口3 波特率 115200
uart = UART(3,115200)

#定义帧率
fps_cnt = 0

#坐标数据打包
def pack_A_data(data_x,data_y):

    data_code = 1

    pack_data=bytearray([0xAA,0xAA,0xF2,0x00,data_x>>8,data_x,data_y>>8,data_y,data_code,0x00])

    lens = len(pack_data)#数据包大小
    pack_data[3] = lens-5;#有效数据个数

    i = 0
    sum = 0

    #和校验
    while i<(lens-1):
        sum = sum + pack_data[i]
        i = i+1
    pack_data[lens-1] = sum

    return pack_data

#串口接收函数
def Receive_Prepare(data):

    if R.state==0:

        if data == 0xAA:#帧头
            R.state = 1
            R.uart_buf.append(data)
        else:
            R.state = 0

    elif R.state==1:
        if data == 0xAF:#帧头
            R.state = 2
            R.uart_buf.append(data)
        else:
            R.state = 0

    elif R.state==2:
        if data<=0xFF:#功能字
            R.state = 3
            R.uart_buf.append(data)
        else:
            R.state = 0

    elif R.state==3:
        if data>0 and data<33:#数据个数
            R.state = 4
            R.uart_buf.append(data)
            R._data_len = data
            R._data_cnt = 0
        else:
            R.state = 0

    elif R.state==4:
        if R._data_len > 0:
            R. _data_len = R._data_len - 1
            R.uart_buf.append(data)
            if R._data_len == 0:
                R.state = 5
        else:
            R.state = 0

    elif R.state==5:
        R.state = 0
        R.uart_buf.append(data)
    else:
        R.state = 0

#读取串口缓存
def uart_read_buf():
    i = 0
    buf_size = uart.any()
    while i<buf_size:
        Receive_Prepare(uart.readchar())
        i = i + 1

#定时器中断
def time_irq(t):
    global fps_cnt
    print(fps_cnt)
    fps_cnt = 0

#识别圆的函数
def circle_direct():
    clock.tick()

    #lens_corr(1.8)畸变矫正
    img = sensor.snapshot().lens_corr(1.8)

    # Circle对象有四个值: x, y, r (半径), 和 magnitude。
    # magnitude是检测圆的强度。越高越好

    # roi 是一个用以复制的矩形的感兴趣区域(x, y, w, h)。如果未指定，
    # ROI 即图像矩形。操作范围仅限于roi区域内的像素。

    # x_stride 是霍夫变换时需要跳过的x像素的数量。若已知圆较大，可增加
    # x_stride 。

    # y_stride 是霍夫变换时需要跳过的y像素的数量。若已知直线较大，可增加
    # y_stride 。

    # threshold 控制从霍夫变换中监测到的圆。只返回大于或等于阈值的圆。
    # 应用程序的阈值正确值取决于图像。注意：一条圆的大小是组成圆所有
    # 索贝尔滤波像素大小的总和。

    # x_margin 控制所检测的圆的合并。 圆像素为 x_margin 、 y_margin 和
    # r_margin的部分合并。

    # y_margin 控制所检测的圆的合并。 圆像素为 x_margin 、 y_margin 和
    # r_margin 的部分合并。

    # r_margin 控制所检测的圆的合并。 圆像素为 x_margin 、 y_margin 和
    # r_margin 的部分合并。

    # r_min，r_max和r_step控制测试圆的半径。
    # 缩小测试圆半径的数量可以大大提升性能。
    # threshold = 3500比较合适。如果视野中检测到的圆过多，请增大阈值；
    # 相反，如果视野中检测到的圆过少，请减少阈值。
    for c in img.find_circles(threshold = 3500, x_margin = 10, y_margin = 10, r_margin = 10,r_min = 2, r_max = 100, r_step = 2):
        img.draw_circle(c.x(), c.y(), c.r(), color = (255, 0, 0))
        print(c)

    print("FPS %f" % clock.fps())


#识别颜色和圆一起
def black_circle():
    clock.tick()
    img = sensor.snapshot().lens_corr(1.8)
    for c in img.find_circles(threshold = 3500, x_margin = 10, y_margin = 10, r_margin = 10,r_min = 2, r_max = 100, r_step = 2):
        area = (c.x()-c.r(), c.y()-c.r(), 2*c.r(), 2*c.r())
        #area为识别到的圆的区域，即圆的外接矩形框
        statistics = img.get_statistics(roi=area)#像素颜色统计
        print(statistics)
        #(0,100,0,120,0,120)是红色的阈值，所以当区域内的众数（也就是最多的颜色），范围在这个阈值内，就说明是红色的圆。
        #l_mode()，a_mode()，b_mode()是L通道，A通道，B通道的众数。
        if 0<statistics.l_mode()<100 and 0<statistics.a_mode()<127 and 0<statistics.b_mode()<127:#if the circle is red
            img.draw_circle(c.x(), c.y(), c.r(), color = (255, 0, 0))#识别到的红色圆形用红色的圆框出来
        else:
            img.draw_rectangle(area, color = (255, 255, 255))
            #将非红色的圆用白色的矩形框出来
    print("FPS %f" % clock.fps())
def pilot_lamp_mode():
    led1.on()
    led2.on()
    led3.on()
    time.sleep_ms(30)              #红、蓝、红三灯全亮
    led1.off()
    led2.off()
    led3.off()

#用于判断是否成功检测到圆
def pilot_lamp_A():
    led1.on()
    led2.on()
    time.sleep_ms(30)               #红、蓝两灯亮
    led1.off()
    led2.off()

#用于判断是否成功检测到十字停止位
def pilot_lamp_cross():
    led2.on()                         #蓝灯亮
    time.sleep_ms(30)
    led2.off()

#用于判断是否成功检测到颜色
def pilot_lamp_angle():
    led1.on()                         #红灯亮
    time.sleep_ms(30)
    led1.off()

#定义一个类对象
class receive(object):
    uart_buf = []
    _data_len = 0
    _data_cnt = 0
    state = 0
R=receive()

#定义一个类对象
class buffer_flag(object):
    launch_flag = 0   #起飞标志位
    circle_flag = 0     #圆识别标志位
    color_flag = 0    #颜色检测标志位
f = buffer_flag()

#定时器频率1HZ中断
tim = Timer(2,freq=1,callback=time_irq) #初始化定时器

while(True):
    uart_read_buf()   #打开串口接收
    fps_cnt = fps_cnt + 1 #统计频率
    if len(R.uart_buf)>0:
        if R.uart_buf[7]==1:
            pilot_lamp_mode()
            f.color_flag = 1
            f.angle_flag = 1
            R.uart_buf = []
        if R.uart_buf[8]==2:
            f.circle_flag = 1
            f.color_flag = 0
            R.uart_buf = []