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

thresholds = [(100, 255)] # grayscale thresholds
uart = UART(3, 115200)

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
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()

def send_position(x,y):
buffer = struct.pack( "bbbb",0x13,0x14,x,y)
uart.write(buffer)
print("x:{},y:{}".format(x,y))

def find_max(circles):
max_size=0
for circle in circles:
if circle.r() > max_size:
max_circle = circle
max_size = circle.r()
return max_circle

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

circles = img.find_circles(threshold = 3500, x_margin = 10, y_margin = 10, r_margin = 10,r_min = 2, r_max = 100, r_step = 2)

if(circles):
    max_circle = find_max(circles)
    img.draw_circle(max_circle.x(), max_circle.y(), max_circle.r(), color = (255, 0, 0))
    print(max_circle.x(), max_circle.y(), max_circle.r(),img.width()/2,img.height()/2)
    x1 = max_circle.x()-img.width()/2
    y1 = max_circle.y()-img.height()/2
    if x1 > img.height()/2 or x1 < -img.height()/2:
        continue
    x1 = int(x1*256/img.height())
    y1 = int(y1*256/img.height())
    send_position(x1,y1)
#print("FPS %f" % clock.fps())