#THRESHOLD =(0, 23, -12, 5, -5, 13)
THRESHOLD_day=(0, 30, -47, 20, -28, 19)
import sensor, image, time,math,pyb
from pyb import UART,Pin,Timer
from pyb import LED
from pid import PID
output_str=''
uart = UART(3,9600)
sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_windowing((0,60,120,60))
sensor.skip_frames(time = 2000)
clock = time.clock()
LED(1).on()
LED(2).on()
LED(3).on()
roi_X=[30,0,30,30]
roi_stra=[50,0,30,30]
rho_pid = PID(p=0.7, i=0)
theta_pid = PID(p=0.3, i=0.5,d=0.1)
while(True):
     img = sensor.snapshot()
     img.draw_rectangle(roi_X, color = (255, 0, 0), thickness = 2, fill = False)
     img.draw_rectangle(roi_stra, color = (255, 0, 0), thickness = 2, fill = False)
     blobs = img.find_blobs([THRESHOLD_day], roi=roi_X,x_stride=1, y_stride=1,pixels_threshold=10)
     for blob in blobs:
            img.draw_rectangle(blob.rect())
            img.draw_cross(blob.cx(), blob.cy())
            if blob:
                blobsss = img.find_blobs([THRESHOLD_day],roi=roi_stra,x_stride=1, y_stride=1,pixels_threshold=10)
                for blob in blobs:
                        img.draw_rectangle(blob.rect())
                        img.draw_cross(blob.cx(), blob.cy())
                        if blob:
                            print('go!')
                            output_str =bytearray([0x2C,0x12,128,128,0x5B])
                            #output_str='&0%d%d*' % (300, 300)
                            uart.write(output_str)
                            print(output_str)
                            pyb.delay(200)
     img = sensor.snapshot().binary([THRESHOLD_day])
     line1 = img.get_regression([(100,100)], robust = True)
     if (line1):
         rho_err = abs(line1.rho())-img.width()/2
         if line1.theta()>90:
             theta_err = line1.theta()-180
         else:
             theta_err = line1.theta()
         img.draw_line(line1.line(), color = 127)
         if line1.magnitude()>8:
            rho_output = rho_pid.get_pid(rho_err,1)
            theta_output = theta_pid.get_pid(theta_err,1)
            output = rho_output+theta_output
            output1=128-output
            output2=128+output
            int(output1)
            int(output2)
            output_str =bytearray([0x2C,0x12,output1,output2,0x5B])
            #output_str='&0%d%d*' % (300-output, 300+output)
            uart.write(output_str)
            print(output_str)
         else:
            output_str =bytearray([0x2C,0x12,0,128,0x5B])
            #output_str='&0000300*'
            uart.write(output_str)  
            print(output_str)
            pass

# 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,pyb
import ustruct
from pyb import UART
from pyb import Pin
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()
led3 = pyb.LED(3) # Red LED = 1, Green LED = 2, Blue LED = 3, IR LEDs = 4.
led2 = pyb.LED(2)
led1 = pyb.LED(1)
p0_out = Pin('P0', Pin.OUT_PP)#设置p0为输出引脚
# 注意！与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中心位置
uart = UART(3, 115200)#初始化串口号及其波特率
uart.init(115200,bits=8,parity=None,stop=1)

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 sending_data(cx):
    global uart;
    #frame=[0x2C,18,cx%0xff,int(cx/0xff),cy%0xff,int(cy/0xff),0x5B];
    #data = bytearray(frame)
    data = ustruct.pack("<bbhb",      #格式为俩个字符俩个短整型(2字节)
                   0x2C,                      #帧头1
                   0x12,                      #帧头2
                   int(cx), # up sample by 4   #数据1
                    # up sample by 4    #数据2
                   0x5B)
    uart.write(data);   #必须要传入一字节的数组，这个函数似乎不能发送单个字节，必须得一次发送多个字节
def degrees(radians):
    return (180 * radians) / math.pi

while(True):
    clock.tick() 
    led1.on()
    led3.on()#LED指示openmv正在工作
    led2.on()
    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.z_translation())
        output_str="[%d]" % (tag.z_translation())
        print('you send:',output_str)
        sending_data(tag.z_translation())
        led1.off()
        led2.off()
        led3.off()
        
        pyb.delay(10)
        if print_args=="-8":
           p0_out.high()#设置p0_out引脚为高 
           
        else:
           p0_out.low()   
        uart.write(FH)
    else:
        print('not found!')
        #如果没有找到符合条件的色块，那么发送一个不可能出现的坐标
        #FH = bytearray([0x2C,0x12,0x77,0x55,0x5B])
        pyb.delay(10)
        #uart.write(FH)
        
        # 位置的单位是未知的，旋转的单位是角度