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

led = LED(3)


uart = UART(1, 19200)
uart.init(115200,bits=8,parity=None,stop=1)


thresholds = [(23, 90, -62, -5, 9, 68), #绿色
              (10,70, -5, 37, -71, -30),#蓝色
              (0, 15, -19, 2, 2, 21),# 黑色
              (0, 100, -128, 127, -128, 127)]#全色


sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 200)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
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


while(True):
    clock.tick()
    img = sensor.snapshot()

    blobs=[]

#    nothing=img.find_blobs([thresholds[3]])
    blobs=img.find_blobs([thresholds[0],thresholds[1],thresholds[2]], pixels_threshold=5, area_threshold=5, merge=True)

    max_b=find_max(blobs)

    if max_b.code()==1:
             led.off()
             uart.write('1')
             img.draw_rectangle(max_b[0:4],color=(0,255,0))
             img.draw_cross(max_b[5],max_b[6],color=(255,0,0))
             print("green")
    elif max_b.code()==2:
            led.off()
            uart.write('2')
            img.draw_rectangle(max_b[0:4],color=(255,0,0))
            img.draw_cross(max_b[5],max_b[6],color=(0,255,0))
            print("blue")
    else:
          led.off()


#    if max_b.code()==3:
#            uart.write('1')
#            img.draw_rectangle(max_b[0:4],color=(0,0,255))
#            img.draw_cross(max_b[5],max_b[6],color=(0,0,255))
#            print("black")