import sensor#引入感光元件的模块

# 设置摄像头
sensor.reset()#初始化感光元件
sensor.set_pixformat(sensor.RGB565)#设置为彩色
sensor.set_framesize(sensor.QVGA)#设置图像的大小
sensor.skip_frames()#跳过n张照片，在更改设置后，跳过一些帧，等待感光元件变稳定。

# 一直拍照
while(True):
    img = sensor.snapshot()#拍摄一张照片，img为一个image对象

import sensor, image, time

sensor.reset ()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)  #分辨率160*120
#sensor.set_auto_gain (False)       #颜色跟踪必须关闭自动增益
#sensor.set_auto_whitebal (False)   #颜色跟踪必须关闭白平衡
sensor.skip_frames (time = 2000)

Range = (0,0,160,120)   #设定感兴趣区域

#【0】记忆色块阈值
for i in range (200):
    img = sensor.snapshot()
    img.draw_rectangle((75,55,10,10),color=(255,0,0))   #圈出搜寻区域
    Statistics = img.get_statistics (roi=(75,55,10,10)) #得到搜寻区域内的图像统计信息
    Threshold = [Statistics.l_min(),Statistics.l_max(), #从统计信息中得到色块阈值
                 Statistics.a_min(),Statistics.a_max(),
                 Statistics.b_min(),Statistics.b_max()]
    print(Threshold)

while(True):
    img = sensor.snapshot()
    #【1】找色块
    for blob in img.find_blobs([Threshold],roi=Range, pixels_threshold=100, area_threshold=100, merge=True, margin=10):
        #【2】输出
        img.draw_rectangle(blob.rect())      #画框
        img.draw_cross(blob.cx(), blob.cy()) #画十字
        print(blob.cx(), blob.cy())          #打印坐标

#串口输出数据包(发送的是16进制数)
data = bytearray([0xa3,0xb3, obj.classid()+1, pos[0], pos[1], 0xc3]) #帧头 + 帧头 + 数字 + x坐标 + y坐标 + 帧尾
usart3.write(data)

from pyb import UART

uart = UART(3, 9600)
uart.write('hello')
uart.read(5) # read up to 5 bytes

from machine import UART

uart = UART(1, 9600)
uart.write('hello')
uart.read(5) # read up to 5 bytes

# 巡线(红线) + 判断十字路口
import sensor, image, time, pyb
import display
from pyb import UART, LED
LED(1).on()
LED(2).on()
LED(3).on()
uart = pyb.UART(3, 9600)
uart.init(9600, bits=8, parity=None, stop=1)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA2)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
lcd = display.SPIDisplay()
sensor.skip_frames (time = 1000)
clock = time.clock()
def find_max(blobs):
	max_size=0
	for blob in blobs:
		if blob[2]*blob[3] > max_size:
			max_blob=blob
			max_size = blob[2]*blob[3]
	return max_blob
theta_err = 90
rotate_angle = 90
crossroad = 0
while(True):
	clock.tick()
	img = sensor.snapshot().binary([(4, 70, 18, 127, -125, 127)])
	blobs = img.find_blobs([(20, 255)], x_stride=1, y_stride=1, pixels_threshold=180, area_threshold=180, merge=True)
	if blobs:
		max_blob = find_max(blobs)
		rotate_angle =  180 * (max_blob.cx() / img.width())
		if max_blob.w()<img.width()*0.7:
			crossroad = 0
		else:
			crossroad = 1
		img.draw_rectangle(max_blob.rect())
		img.draw_cross(max_blob.cx(), max_blob.cy(),color=(255,0,0))
	line = img.get_regression([(20, 255)], pixels_threshold=120, area_threshold=120, robust = True)
	if line:
		if line.theta()>90:
			theta_err = line.theta()-90
		else:
			theta_err = line.theta()+90
		img.draw_line(line.line(), color = 127)
	chimera = theta_err*0.7 + rotate_angle*0.3
	img.draw_string(10, 10, str(int(chimera)), color=(0,185,255), scale=2, x_spacing=-5)
	data=bytearray([0xa3,0xb3,int(chimera),int(crossroad),0xc3])
	uart.write(data)
	print(" ")
	print("当前帧数:", clock.fps(), "fps")
	print("是否检测到十字路口:", crossroad)
	print("推荐转向", chimera)
	lcd.write(img)
    

# 无人机降落(黑色矩形区域)
import sensor, image, time, pyb, display
from pyb import UART
import json
import ustruct

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)  # 关闭白平衡
lcd = display.SPIDisplay() # 初始化lcd屏幕。
clock = time.clock()
uart3 = UART(3, 500000, timeout_char=1000)
# 色块阈值
#blue_threshold = (31, 66, -37, -9, 5, -32)
#yellow_threshold = (50, 78, -54, 108, 20, 78)
blue_threshold = (0,30,-128,10,-128,127)
yellow_threshold = (58,100,-128,127,-128,127)
# 测距常数
k = 950
moshi=0
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob

while True:
    clock.tick()
    img = sensor.snapshot()
    blobs_blue = img.find_blobs([blue_threshold], roi = (0,0,img.width(),img.height()))
    if blobs_blue:
    # 在矩形内检测到蓝色，说明是己方宝藏
        b = find_max(blobs_blue)  # 将返回数据赋值给b
        if b.density()>0.7 and b.w()>img.width()*0.3:
            img.draw_cross(b[5], b[6]) # cx, cy
            img.draw_cross(int(img.width()*0.5), int(img.height()*0.5), size=5, color=(0,255,0))
            #img.draw_cross(0, 0, size=5, color=(0,255,0))
            img.draw_line((int(img.width()*0.5), int(img.height()*0.5),b[5], b[6]), color=(255,0,255))
            a_x=b.cx()-int(img.width()*0.5)
            a_y=b.cy()-int(img.height()*0.5)
            img.draw_rectangle(b.rect(), color = (0, 0, 255))
            FH = bytearray([0xAA,0xFF,0xCC,0x00,moshi,a_x,a_y,0x00,0x00])
            lens = len(FH)#数据包大小
            FH[3] = lens-6;#有效数据个数
            i = 0
            sum = 0
            sum1 = 0
                #和校验
            while i<(lens-2):
                sum = sum + FH[i]
                sum1 = sum1 + sum
                i = i+1
            FH[lens-2] = sum;
            FH[lens-1] = sum1;
            #传输数据给单片机
            #uart_buf = bytearray(row_data)
            uart3.write(FH)
            print(FH)
            print("x偏移:",a_x,"y偏移:",a_y,"\n")
            # 在检测到的真宝藏上画蓝框
            # 屏幕显示
            img.draw_string(10, 10, str(a_x) + "," + str(a_y), color=(255,0,70), scale=2, x_spacing=-5, mono_space=True)
        else:
           print("未检测")
    else:
       print("未检测")
    img_copy = img.copy(x_scale=128/img.width(),y_scale=160/img.height())
    lcd.write(img_copy) # 让LCD屏显示拍到的图像

# 识别矩形 + 返回矩形四角坐标
import sensor, image, time, pyb
import display
from pyb import UART
uart = pyb.UART(3, 9600)
uart.init(9600, bits=8, parity=None, stop=1)
lcd = display.SPIDisplay()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA2)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.skip_frames(time = 500)
clock = time.clock()
while(True):
	clock.tick()
	img = sensor.snapshot()
	for r in img.find_rects(threshold = 10000):
		if r.w() > 15 and r.h() > 15:
			img.draw_rectangle(r.rect(), color = (255, 0, 0), scale = 4)
			corner = r.corners()
			img.draw_circle(corner[0][0], corner[0][1], 3, color = (135, 255, 0), thickness = 2, fill = False)
			img.draw_circle(corner[1][0], corner[1][1], 3, color = (0, 70, 255), thickness = 1, fill = False)
			img.draw_circle(corner[2][0], corner[2][1], 3, color = (0, 70, 255), thickness = 1, fill = False)
			img.draw_circle(corner[3][0], corner[3][1], 3, color = (0, 70, 255), thickness = 1, fill = False)
			corner1_str = f"({corner[0][0]},{corner[0][1]})"
			corner2_str = f"({corner[1][0]},{corner[1][1]})"
			corner3_str = f"({corner[2][0]},{corner[2][1]})"
			corner4_str = f"({corner[3][0]},{corner[3][1]})"
			print("矩形关键点坐标:")
			print("corner1 = " + corner1_str + "\n"
				  + "corner2 ="+ corner2_str + "\n"
				  + "corner3 ="+ corner3_str + "\n"
				  + "corner4 ="+ corner4_str	   )
			dataxy=bytearray([0xa1,0xb1,corner[0][0],corner[1][0],corner[2][0],corner[3][0],corner[0][1],corner[1][1],corner[2][1],corner[3][1],0xc1])
			uart.write(dataxy)
			img.draw_string(10, 10, corner1_str, color=(255,0,70), scale=2, x_spacing=-5, mono_space=True)
			img.draw_string(10, 10, "\n" + corner3_str, color=(255,0,70), scale=2, x_spacing=-5, mono_space=True)
	lcd.write(img)

# OpenMV 延时拍照片并存储
# 注意:您需要一张TF卡来运行

import sensor
import time
import display # 引入lcd屏幕

sensor.reset()  # 初始化sensor
sensor.set_pixformat(sensor.RGB565)  # 设置图像色彩格式，有RGB565色彩图和GRAYSCALE灰度图两种
sensor.set_framesize(sensor.QVGA)  # 设置图像像素大小 QVGA (320x240)
lcd = display.SPIDisplay() # 初始化lcd屏幕。
sensor.skip_frames(time=2000)  # 让新的设置生效

img = sensor.snapshot()
img.save("example.jpg")  # or "example.bmp" (or others)
img_copy = img.copy(x_scale=128/img.width(),y_scale=160/img.height())

while(True):
    lcd.write(img_copy) # 让LCD屏显示拍到的图像

import sensor, image, time, pyb
import display # 引入lcd屏幕

# 启用串口 P4,P5 (TX,RX)
from pyb import UART
uart = pyb.UART(3, 9600)
uart.init(9600, bits=8, parity=None, stop=1)

# 初始化lcd屏幕
lcd = display.SPIDisplay()

# 初始化摄像头
sensor.reset()
#sensor.set_vflip(True)              # 打开垂直翻转模式
#sensor.set_hmirror(True)            # 打开水平镜像模式
sensor.set_pixformat(sensor.RGB565) # 设置图像色彩格式为RGB565格式
sensor.set_framesize(sensor.QVGA)  # 设置图像大小
#sensor.set_framesize(sensor.QQVGA2) # 返回128x160图像帧大小以适配OpenMV配套液晶屏。
sensor.set_auto_gain(False)         # 颜色跟踪必须关闭自动增益
sensor.set_auto_whitebal(False)     # 颜色跟踪必须关闭白平衡
#sensor.set_brightness(3000)         # 设置亮度为3000
sensor.skip_frames(time = 500)      # 跳过帧

clock = time.clock()

# 给x毫秒时间准备，计算ROI区域中阈值，作为目标阈值之一;
for i in range (200):
    img = sensor.snapshot()
    img.draw_rectangle((0,0,img.width(),img.height()),color=(255,0,0)) # 在视野中框出指定ROI区域，用于提取其中颜色阈值
    statistics = img.get_statistics (roi=(0,0,img.width(),img.height())) #得到搜寻区域内的图像统计信息#thresholds=(10, 80, -20, 50, -20, 50),

    color_L_min = statistics.l_min() #分别赋值LAB的众数
    color_L_max = statistics.l_max()
    color_A_min = statistics.a_min()
    color_A_max = statistics.a_max()
    color_B_min = statistics.b_min()
    color_B_max = statistics.b_max()

    #基于色块容错空间
    relative_darkest_threshold   = (color_L_min,   color_L_min+1, color_A_min,   color_A_min+1, color_B_min,   color_B_min+1)
    relative_brightest_threshold = (color_L_max-1, color_L_max,   color_A_max-1, color_A_max,   color_B_max-1, color_B_max)
    print(relative_darkest_threshold)
    print(relative_brightest_threshold)
    print("\n")

while True:
    clock.tick()
    img = sensor.snapshot()
    print(relative_darkest_threshold)
    print(relative_brightest_threshold)
    print("\n")

import sensor

sensor.reset() #重启感光元件
#sensor.set_vflip(True) #打开垂直翻转模式
#sensor.set_hmirror(True) #打开水平镜像模式
sensor.set_pixformat(sensor.RGB565) # or sensor.GRAYSCALE # 设置相机模块的像素模式:RGB565
#sensor.set_framesize(sensor.QQVGA)  #设置返回图像的帧大小，分辨率160*120
sensor.set_framesize(sensor.QQVGA2) # 返回128x160图像帧大小以适配OpenMV配套液晶屏。
sensor.set_auto_gain(False)       #颜色跟踪必须关闭自动增益
sensor.set_auto_whitebal(False)   #颜色跟踪必须关闭白平衡
sensor.skip_frames (time = 1000)    #跳过启动后的前x毫秒以求稳定

import image

import time

clock = time.clock()    # 创建一个计时器对象，用于计算每秒钟的帧数。

import display # 引入lcd屏幕

lcd = display.SPIDisplay() # 初始化lcd屏幕。
    lcd.write(img) # 让LCD屏显示拍到的图像(在while中)

import pyb
# 启用串口 P4,P5 (TX,RX)
# OpenMV4 H7 Plus, OpenMV4 H7, OpenMV3 M7, OpenMV2 M4 的UART(3)是P4-TX P5-RX
# OpenMV RT 只有串口UART(1)，对应P4-TX P5-RX; OpenMV4 H7 Plus, OpenMV4 H7, OpenMV3 M7 的UART(1)是P0-RX P1-TX
from pyb import UART # 从pyb库中启用uart串口通信

uart = UART(3, 9600, timeout_char=1000) #创建UART对象，使用第3个UART端口，设置波特率为9600
uart.init(9600, bits=8, parity=None, stop=1, timeout_char=1000) #初始化UART总线，时钟速率9600，每个字符占8位，不进行奇偶化校验，停止位数量为1

from pyb import LED 

LED(1).on() #红
LED(2).on() #绿
LED(3).on() #蓝

from pyb import Pin # pyb模块包含与插件相关的特定函数，引入引脚

# 初始化P0引脚，输出高电平
p0 = Pin('P0', Pin.OUT)
p0.value(1)

# 最大色块更新函数
def find_max(blobs):    # 定义函数 find_max , 接受参数 blobs , 意在从分析出的色块中找出面积最大的
    max_size=0  # 定义一个变量 max_size , 初始化时这个值为0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob

ROI = (x,x,x,x)

thresholds  = [(4, 70, 18, 127, -125, 127), # for red1
               (0, 35, 20, 75, -25, 45),    # for red2
               (13, 70, 3, 55, 4, 40),      # for red3
               (30, 100, -64, -8, -32, 32), # for green
               (0, 30, 0, 64, -128, 0)]     # for blue

while(True):
    clock.tick()    # 计算从上一次调用tick()到现在的时间，以确定每秒钟的帧数
    img = sensor.snapshot() # 拍照片，返回图像
    # img = sensor.snapshot().binary([(4, 70, 18, 127, -125, 127)]) # 拍二值化图片(阈值自定义)，返回图像
    x'x'x'x'x'x'x'x'x (其他任务、算法)
    lcd.write(img)  # 让LCD屏显示拍到的图像(一般放在进程末尾)

# 最大色块算法
    # 在图像中寻找符合指定条件的色块。找到的色块将被存储在变量blobs中，在调用find_max函数找出最大色块
    blobs = img.find_blobs([(20, 255)], x_stride=1, y_stride=1, pixels_threshold=180, area_threshold=180, merge=True)#, suit,[thresholds[threshold_index]]
    if blobs:
        max_blob = find_max(blobs)

#        #定义横纵坐标偏移量，意为探测计算出的色块的中心点距离视野中心点的偏移量，旨在引导小车左右转和加速减速
#        x_error = max_blob.cx()-img.width()/2   # 左右偏移量 = 色块中心点横坐标-视野中心点横坐标，范围[-64,64]
#        y_error = max_blob.cy()-img.height()/2  # 纵偏移量 = 色块中心点纵坐标-视野中心点纵坐标,范围[-80,80]

        rotate_angle =  180 * (max_blob.cx() / img.width())

        #定义变量crossroad,判断是否为十字路口
        if max_blob.w()<img.width()*0.7:
            crossroad = 0
        else:
            crossroad = 1

        img.draw_rectangle(max_blob.rect()) # 画矩形，根据计算出的最大色块大小
        img.draw_cross(max_blob.cx(), max_blob.cy(),color=(255,0,0)) # 画交叉十字，根据计算出的最大色块中心点位置

# 直线算法
    line = img.get_regression([(20, 255)], pixels_threshold=120, area_threshold=120, robust = True)
    if line:
#        rho_err = abs(line.rho())-img.width()/2
        if line.theta()>90:
            theta_err = line.theta()-90
        else:
            theta_err = line.theta()+90
        img.draw_line(line.line(), color = 127)

    # 色块算法
    # 在图像中寻找符合指定条件的色块。找到的色块将被存储在变量blobs中，在调用find_max函数找出最大色块
    blobs = img.find_blobs([(20, 255)], x_stride=1, y_stride=1, pixels_threshold=180, area_threshold=180, merge=True)#, suit,[thresholds[threshold_index]]
    if blobs:
        max_blob = find_max(blobs)

#        #定义横纵坐标偏移量，意为探测计算出的色块的中心点距离视野中心点的偏移量，旨在引导小车左右转和加速减速
#        x_error = max_blob.cx()-img.width()/2   # 左右偏移量 = 色块中心点横坐标-视野中心点横坐标，范围[-64,64]
#        y_error = max_blob.cy()-img.height()/2  # 纵偏移量 = 色块中心点纵坐标-视野中心点纵坐标,范围[-80,80]

        rotate_angle =  180 * (max_blob.cx() / img.width())

        #定义变量crossroad,判断是否为十字路口
        if max_blob.w()<img.width()*0.7:
            crossroad = 0
        else:
            crossroad = 1

        img.draw_rectangle(max_blob.rect()) # 画矩形，根据计算出的最大色块大小
        img.draw_cross(max_blob.cx(), max_blob.cy(),color=(255,0,0)) # 画交叉十字，根据计算出的最大色块中心点位置

    # 直线算法
    line = img.get_regression([(20, 255)], pixels_threshold=120, area_threshold=120, robust = True)
    if line:
#        rho_err = abs(line.rho())-img.width()/2
        if line.theta()>90:
            theta_err = line.theta()-90
        else:
            theta_err = line.theta()+90
        img.draw_line(line.line(), color = 127)

    chimera = theta_err*0.7 + rotate_angle*0.3
    # 屏幕显示参数区(如需节约性能，请写固定数值)
#    img.draw_string(int(img.width()*0.1), int(img.width()*0.1), str(int(chimera)), color=(0,185,255), scale=int(img.width()*0.016), x_spacing=int(img.width()*0.016)*-2)
    img.draw_string(10, 10, str(int(chimera)), color=(0,185,255), scale=2, x_spacing=-5)

    # 串口数据定义并发送区
    # 要发送的数据:[帧头 a3 b3,中间*个十六位数据，帧尾 c3]
    data=bytearray([0xa3,0xb3,int(chimera),int(crossroad),0xc3])
    # 发送数据
    uart.write(data)

    # 串行终端发送区
    print(" ")
    print("当前帧数:", clock.fps(), "fps")
##       print("目标横坐标(0-128):", max_blob.cx())
##       print("目标纵坐标(0-160):", max_blob.cy())
##       print("横坐标偏离值[-64,64]:", x_error)
##       print("纵坐标偏离值[-80,80]:", y_error)
##       print("色块转向角度:", max_blob.rotation())
    print("是否检测到十字路口:", crossroad)
#    print("色块偏离值:", rotate_angle) #[0,180]
#    print("线角度:",theta_err) #[0,180]
    print("推荐转向", chimera)

# -----矩形框部分-----
    # 在图像中寻找矩形
    for r in img.find_rects(threshold = 10000):
        # 判断矩形边长是否符合要求
        if r.w() > 15 and r.h() > 15:
            # 在屏幕上框出矩形
            img.draw_rectangle(r.rect(), color = (255, 0, 0), scale = 4)
            # 获取矩形角点位置
            corner = r.corners()
            # 在屏幕上圈出矩形角点(第一个点通常为左下角那个)
            img.draw_circle(corner[0][0], corner[0][1], 3, color = (135, 255, 0), thickness = 2, fill = False)
            img.draw_circle(corner[1][0], corner[1][1], 3, color = (0, 70, 255), thickness = 1, fill = False)
            img.draw_circle(corner[2][0], corner[2][1], 3, color = (0, 70, 255), thickness = 1, fill = False)
            img.draw_circle(corner[3][0], corner[3][1], 3, color = (0, 70, 255), thickness = 1, fill = False)

        # 打印四个角点坐标, 角点1的数组是corner[0], 坐标就是(corner[0][0],corner[0][1])
        # 角点检测输出的角点排序每次不一定一致，矩形左上的角点有可能是corner0,1,2,3其中一个
            corner1_str = f"({corner[0][0]},{corner[0][1]})"
            corner2_str = f"({corner[1][0]},{corner[1][1]})"
            corner3_str = f"({corner[2][0]},{corner[2][1]})"
            corner4_str = f"({corner[3][0]},{corner[3][1]})"
            print("矩形关键点坐标:")
            print("corner1 = " + corner1_str + "\n"
                  + "corner2 ="+ corner2_str + "\n"
                  + "corner3 ="+ corner3_str + "\n"
                  + "corner4 ="+ corner4_str       )

#            data1=bytearray([0xa1,0xb1,corner[0][0],corner[0][1],0xc1])          # 定义点坐标1(通常为左下角那个)
#            uart.write(data1)

#            data2=bytearray([0xa2,0xb2,corner[1][0],corner[1][1],0xc2])          # 定义点坐标2
#            uart.write(data2)

#            data3=bytearray([0xa3,0xb3,corner[2][0],corner[2][1],0xc3])          # 定义点坐标3
#            uart.write(data3)

#            data4=bytearray([0xa4,0xb4,corner[3][0],corner[3][1],0xc4])          # 定义点坐标4
#            uart.write(data4)

            dataxy=bytearray([0xa1,0xb1,corner[0][0],corner[1][0],corner[2][0],corner[3][0],corner[0][1],corner[1][1],corner[2][1],corner[3][1],0xc1])   # 四个点的x坐标和y坐标(通常左下角那个是第一个)
            uart.write(dataxy)
#            print(dataxy)

            img.draw_string(10, 10, corner1_str, color=(255,0,70), scale=2, x_spacing=-5, mono_space=True) # 屏幕显示
            img.draw_string(10, 10, "\n" + corner3_str, color=(255,0,70), scale=2, x_spacing=-5, mono_space=True) # 屏幕显示

blobs_blue = img.find_blobs([blue_threshold], roi = (0,0,img.width(),img.height()))
if blobs_blue:
# 在矩形内检测到蓝色，说明是己方宝藏
    b = find_max(blobs_blue)  # 将返回数据赋值给b
    if b.density()>0.7 and b.w()>img.width()*0.3:
        img.draw_cross(b[5], b[6]) # cx, cy
        img.draw_cross(int(img.width()*0.5), int(img.height()*0.5), size=5, color=(0,255,0))
        #img.draw_cross(0, 0, size=5, color=(0,255,0))
        img.draw_line((int(img.width()*0.5), int(img.height()*0.5),b[5], b[6]), color=(255,0,255))
        a_x=b.cx()-int(img.width()*0.5)
        a_y=b.cy()-int(img.height()*0.5)
        img.draw_rectangle(b.rect(), color = (0, 0, 255))
        FH = bytearray([0xAA,0xFF,0xCC,0x00,moshi,a_x,a_y,0x00,0x00])
        lens = len(FH)#数据包大小
        FH[3] = lens-6;#有效数据个数
        i = 0
        sum = 0
        sum1 = 0
            #和校验
        while i<(lens-2):
            sum = sum + FH[i]
            sum1 = sum1 + sum
            i = i+1
        FH[lens-2] = sum;
        FH[lens-1] = sum1;
        #传输数据给单片机
        #uart_buf = bytearray(row_data)
        uart3.write(FH)
        print(FH)
        print("x偏移:",a_x,"y偏移:",a_y,"\n")
        # 在检测到的真宝藏上画蓝框
        # 屏幕显示
        img.draw_string(10, 10, str(a_x) + "," + str(a_y), color=(255,0,70), scale=2, x_spacing=-5, mono_space=True)
    else:
       print("未检测")
else:
   print("未检测")

# 给x毫秒时间准备，计算ROI区域中阈值，作为目标阈值之一;
for i in range (200):
    img = sensor.snapshot()
    img.draw_rectangle((0,0,img.width(),img.height()),color=(255,0,0)) # 在视野中框出指定ROI区域，用于提取其中颜色阈值
    statistics = img.get_statistics (roi=(0,0,img.width(),img.height())) #得到搜寻区域内的图像统计信息#thresholds=(10, 80, -20, 50, -20, 50),

    color_L_min = statistics.l_min() #分别赋值LAB的众数
    color_L_max = statistics.l_max()
    color_A_min = statistics.a_min()
    color_A_max = statistics.a_max()
    color_B_min = statistics.b_min()
    color_B_max = statistics.b_max()

    #基于色块容错空间
    relative_darkest_threshold   = (color_L_min,   color_L_min+1, color_A_min,   color_A_min+1, color_B_min,   color_B_min+1)
    relative_brightest_threshold = (color_L_max-1, color_L_max,   color_A_max-1, color_A_max,   color_B_max-1, color_B_max)

img_copy = img.copy(x_scale=128/img.width(),y_scale=160/img.height())

    # 屏幕显示参数区(如需节约性能，请写固定数值)
#    img.draw_string(int(img.width()*0.1), int(img.width()*0.1), str(int(chimera)), color=(0,185,255), scale=int(img.width()*0.016), x_spacing=int(img.width()*0.016)*-2)
    img.draw_string(10, 10, str(int(chimera)), color=(0,185,255), scale=2, x_spacing=-5)
    img.draw_string(10, 10, corner1_str, color=(255,0,70), scale=2, x_spacing=-5, mono_space=True) # 屏幕显示
    mg.draw_string(10, 10, "\n" + corner3_str, color=(255,0,70), scale=2, x_spacing=-5, mono_space=True) # 屏幕显示

# 串口数据定义并发送区
# 要发送的数据:[帧头 a3 b3,中间*个十六位数据，帧尾 c3]
data=bytearray([0xa3,0xb3,int(chimera),int(crossroad),0xc3])
# 发送数据
uart.write(data)

# 串行终端发送区
    print(" ")
    print("当前帧数:", clock.fps(), "fps")
##       print("目标横坐标(0-128):", max_blob.cx())
##       print("目标纵坐标(0-160):", max_blob.cy())
##       print("横坐标偏离值[-64,64]:", x_error)
##       print("纵坐标偏离值[-80,80]:", y_error)
##       print("色块转向角度:", max_blob.rotation())
    print("是否检测到十字路口:", crossroad)
#    print("色块偏离值:", rotate_angle) #[0,180]
#    print("线角度:",theta_err) #[0,180]
    print("推荐转向", chimera)

# AprilTags 3D定位
# AprilTags Example
#
# This example shows the power of the OpenMV Cam to detect April Tags
# on the OpenMV Cam M7. The M4 versions cannot detect April Tags.

import sensor, image, time, math

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
sensor.skip_frames(30)
sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
clock = time.clock()

# 注意！与find_qrcodes不同，find_apriltags 不需要软件矫正畸变就可以工作。

# 注意，输出的姿态的单位是弧度，可以转换成角度，但是位置的单位是和你的大小有关，需要等比例换算

# f_x 是x的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/3.984*656，这个值是用毫米为单位的焦距除以x方向的感光元件的长度，乘以x方向的感光元件的像素（OV7725）
# f_y 是y的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/2.952*488，这个值是用毫米为单位的焦距除以y方向的感光元件的长度，乘以y方向的感光元件的像素（OV7725）

# c_x 是图像的x中心位置
# c_y 是图像的y中心位置

f_x = (2.8 / 3.984) * 160 # 默认值
f_y = (2.8 / 2.952) * 120 # 默认值
c_x = 160 * 0.5 # 默认值(image.w * 0.5)
c_y = 120 * 0.5 # 默认值(image.h * 0.5)

def degrees(radians):
    return (180 * radians) / math.pi

while(True):
    clock.tick()
    img = sensor.snapshot()
    for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # 默认为TAG36H11
        img.draw_rectangle(tag.rect(), color = (255, 0, 0))
        img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
        print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation(), \
            degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
        # 位置的单位是未知的，旋转的单位是角度
        print("Tx: %f, Ty %f, Tz %f, Rx %f, Ry %f, Rz %f" % print_args)
    print(clock.fps())