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

GRAYSCALE_THRESHOLD = [(0, 64)]

ROIS = [ # [ROI, weight]

        (0, 100, 160, 20, 0.7), # You'll need to tweak the weights for you app

        (0, 050, 160, 20, 0.3), # depending on how your robot is setup.

        (0, 000, 160, 20, 0.1)

       ]


for r in ROIS: weight_sum += r[4] # r[4] is the roi weight.

#计算权值和。遍历上面的三个矩形，r[4]即每个矩形的权值。



# Camera setup...

sensor.reset() # Initialize the camera sensor.

sensor.set_pixformat(sensor.GRAYSCALE) # use grayscale.

sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.

sensor.skip_frames(30) # Let new settings take affect.

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() # Tracks FPS.

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

while(True):

    clock.tick() # Track elapsed milliseconds between snapshots().

    img = sensor.snapshot() # Take a picture and return the image.



    centroid_sum = 0

    #利用颜色识别分别寻找三个矩形区域内的线段

    for r in ROIS:

        blobs = img.find_blobs(GRAYSCALE_THRESHOLD, roi=r[0:4], merge=True)

       
       

        if blobs:

            # Find the index of the blob with the most pixels.

            most_pixels = 0

            largest_blob = 0

            for i in range(len(blobs)):

           

                if blobs[i].pixels() > most_pixels:

                    most_pixels = blobs[i].pixels()

                 
                    largest_blob = i



            # Draw a rect around the blob.

            img.draw_rectangle(blobs[largest_blob].rect())

            img.draw_rectangle((0,0,30, 30))

            #将此区域的像素数最大的颜色块画矩形和十字形标记出来

            img.draw_cross(blobs[largest_blob].cx(),

                           blobs[largest_blob].cy())



            centroid_sum += blobs[largest_blob].cx() * r[4] # r[4] is the roi weight.

            #计算centroid_sum，centroid_sum等于每个区域的最大颜色块的中心点的x坐标值乘本区域的权值



    center_pos = (centroid_sum / weight_sum) # Determine center of line.

    deflection_angle = 0

    #机器人应该转的角度



    
    deflection_angle = -math.atan((center_pos-80)/60)

   

    deflection_angle = math.degrees(deflection_angle)

   
    print(deflection_angle)
    uart.write(deflection_angle)
    
    time.sleep(1000)