import sensor, image, time
from pyb import UART


all_threshold   = (47, 100, -128, 127, -128, 127)#这里是对图形区域的选择以及对一些区域进行找到
target_threshold   = (93, 100, -3, 2, -1, 2)#目标区域找到位置
sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
#sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
clock = time.clock() # Tracks FPS.
uart = UART(3, 115200)

K=600

while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.
    #img.binary([all_threshold])
    blobs = img.find_blobs([target_threshold])
    d=0
    m = None
    n = None
    
    if blobs:
    #如果找到了目标颜色
        for b in blobs:
        #迭代找到的目标颜色区域
            # Draw a rect around the blob.
            img.draw_rectangle(b[0:4]) # rect
            #用矩形标记出目标颜色区域
            img.draw_cross(b[5], b[6]) # cx, cy
            #在目标颜色区域的中心画十字形标记
           # print(b[5], b[6])
            #输出目标物体中心坐标
            d=d+1
            if d == 1:
                a1 = b[5]
                c1 = b[6]
                print("a1 location:",a1,c1)
            if d == 2:
                a2 = b[5]
                c2 = b[6]
               # a = ((int)((c1-c2)/3))
                print("a2 location:",a2,c2)
            if d == 3:
                a3 = b[5]
                c3 = b[6]
                print("a3 location:",a3,c3)
               # a=((int)((c1-c3)/3))
                x=((int)(a1+(2*(a2-a1))/3))
                y=((int)(c2+(c3-c2)/2))
                print("final location:",x,y)
                d = 0
                if x>23 and x<31:
                    m='D'
                if x>30 and x<38:
                    m='E'
                if x>37 and x<46:
                    m='F'
                if x>45 and x<56:
                    m='G'
                if x>55 and x<63:
                    m='H'
                if x>62 and x<70:
                    m='I'
                if x>69 and x<77:
                    m='J'
                if x>76 and x<85:
                    m='K'
                if x>84 and x<93:
                    m='L'


                if y>91  and y<100:
                    n='D'
                if y>84  and y<92:
                    n='E'
                if y>76  and y<85:
                    n='F'
                if y>68  and y<77:
                    n='G'
                if y>60  and y<69:
                    n='H'
                if y>52  and y<61:
                    n='I'
                if y>45 and y<53:
                    n='J'
                if y>37 and y<46:
                    n='K'
               # uart.write("m,n")
                 #print(d)
            #if a<300 and c<200:       #这里的判断需要根据你的看到的数据进行细分
            #uart.write("(A,A)")
                #uart.write("A,A")
                #output_str="(%d,%d)"%(m,n) #方式一
                output_str="(%c,%c)"%(m,n) #方式一
                #output_str=json.dumps([blob.cx(),blob.cy()]) #方式二
                #if ([blob.cx(),blob.cy()] not in data):
                uart.write(output_str)
                #data.append((blob.cx(),blob.cy()))


            #time.sleep(100)
            #uart.write("11")
            #time.sleep(500)
    #print(clock.fps()) # Note: Your OpenMV Cam runs about half as fast while
    # connected to your computer. The FPS should increase once disconnected.