import sensor, image, time, math, pyb,time
from pyb import UART, LED
import ustruct

LED_R = pyb.LED(1) # Red LED = 1, Green LED = 2, Blue LED = 3, IR LEDs = 4.
LED_G = pyb.LED(2)
LED_B = pyb.LED(3)

LED_R.on()
LED_G.on()
LED_B.on()

red_threshold_01 = ((100, 96, -7, 7, -22, 11)) # 红色阈值
black_color_threshold = (0, 14, -5, 48, -28, 17) # RGB565格式，对应黑色的阈值范围
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time=2000)
#sensor.set_auto_gain(False) # must be turned off for color tracking
#sensor.set_auto_whitebal(False) # must be turned off for color tracking
clock = time.clock()

#LED_G.off()

uart = UART(3, 9600)   # 定义串口3变量
uart.init(9600, bits=8, parity=None, stop=1) # init with given parameters
#data =f'* 90 135 90 45 45 45 45 135 90 135\r\n'
#uart.write(data) 


#def sending_data(cx, cy):
#   global uart
#   ax=90+30/320*(160-cx)
#   ay=90+30/240*cy
#   print(f'{ay:.2f},{ax:.2f}')
#   uart.write(f'* {ay:.2f} {ax:2f}\r\n')   
while True:
    max_area = 0
    max_rect = None
    clock.tick()
    img = sensor.snapshot()
    for r in img.find_rects(threshold=10000):
        # 只处理黑色的矩形
        blobs = img.find_blobs([black_color_threshold], pixels_threshold=200, area_threshold=200, merge=True)
        if blobs:
            area = r.w() * r.h()
            if area > max_area:
                max_area = area
                max_rect = r
    if max_rect:
        img.draw_rectangle(max_rect.rect(), color=(255, 0, 0))
        corners = max_rect.corners()
        # 数据处理
        datas=[]
        print("左下 右下 右上 左上", corners)
        for p in corners:
            img.draw_circle(p[0], p[1], 5, color=(0, 255, 0))
            ax=90+30/320*(160-p[0])
            ay=90+30/240*p[1]
            datas.append(ay)
            datas.append(ax)
        datas_str = ' '.join(str(data) for data in datas)
        print(f'datas:{datas_str}')
        uart.write(f'* {datas_str}\r\n')