import sensor, image, time,math,pyb
from pyb import UART
import json

red=pyb.LED(1)
green=pyb.LED(2)
ball_threshold =  (46, 61, 60, 81, 67, 13)#颜色阈值(minL, maxL, minA, maxA, minB, maxB)

sensor.reset()#重置感光元件和摄像机
sensor.set_pixformat(sensor.RGB565)#设置颜色格式为RGB565
sensor.set_framesize(sensor.QQVGA)#设置图像大小QVGA
sensor.skip_frames(time = 100)#跳过一些刚开始不稳定的时候再开始读取图像
sensor.set_auto_gain(False)#关闭白平衡
sensor.set_auto_whitebal(False)#关闭自动增益
sensor.set_windowing((16,12,128,98))
clock = time.clock()

uart = UART(3, 115200)
fx=0;fy=0;back_h=0;

class Coordinate: #对应颜色相对图片左下角的坐标（图片左下角为原点）
    def __init__(self,xx,yy):
        self.xx=xx
        self.yy=98-yy
        pass
    def x(self):
        return self.xx
    def y(self):
        return self.yy
    pass

def Actual_size(f_x,f_y):
    k=0.3 #像素坐标与实际坐标的比例
    output = "%d %d" % (f_x / k + 10, f_y / k + 20)
    return output

def find_max(max_img):
    max_size=0
    for blob in max_img:
        if blob.pixels() > max_size:
            max_blob=blob
            max_size = blob.pixels()
    return max_blob

while(True):
    img = sensor.snapshot()
    #img = sensor.snapshot().lens_corr(strength = 1.8, zoom = 1.0)
    #img.bilateral(3, color_sigma=0.1, space_sigma=1)
    ball = img.find_blobs([ball_threshold],merge=True)
    img.draw_rectangle( (22,10,81,81))
    if ball:
        i=find_max(ball)
        img.draw_rectangle( i.rect())
        img.draw_cross(i.cx(), i.cy())
        ball_c=Coordinate(i.cx(),i.cy())
        #j = find_max(back)
        #img.draw_rectangle(j.rect())
        #img.draw_cross(j.cx(), j.cy())
        back=Coordinate(22,10)
        #back_c=Coordinate(j.cx(),j.cy())
        back_h=81
        fx=ball_c.x()-back.x()
        fy=ball_c.y()-back.y()+back_h
        #print('板y长度',back_h,'板中心',back_c.x()-back.x(),back_c.y()-back.y()+back_h,'球坐标',fx,fy)
        output_str=Actual_size(fx,fy)
        print('真实坐标2',output_str)
        time.sleep_ms(5)
        uart.write(output_str+'\r\n')
        red.off()
        green.on()
    else :
        red.on()
        green.off()