import sensor, image, time, math,lcd

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
sensor.skip_frames(30)
sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
clock = time.clock()
lcd.init()
while(True):
    clock.tick()
    lcd.rotation(2)
    img = sensor.snapshot()
    for tag in img.find_apriltags(families=image.TAG16H5): 
        img.draw_rectangle(tag.rect(), color = (255, 0, 0))
        img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
        degress = 180 * tag.rotation() / math.pi
        print(tag.id(),degress)
        a=img.draw_string(0, 0, "id:%d"%(tag.id()), color=(0,0,0), scale=2)
        lcd.display(a)
    lcd.display(img)