from pyb import Servo
import time
s1 = Servo(1) # servo on position 1 (P8)设置下方转动舵机
s2 = Servo(2) # servo on position 1 (P7)设置上方俯仰舵机
import sensor, image, math
red   = (11, 30, 14, 35, 0, 28)             # 红色阈值设置
green = (29, 44, -37, -17, 14, 37)          # 绿色阈值设置
blue  = (40, 65, -36, -10, -17, 7)          # 蓝色阈值设置
# 颜色识别区域
rectX0 =  120#120
rectY0 =  120
rectXL =  20#80 320
rectYL =  100#50
r_num = 0
r_x_weighted = 0
r_x = 0
r_y = 150
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False, rgb_gain_db = (-6.02073, -5.886325, -4.30291)) # must be turned off for color tracking
clock = time.clock()

while(True):
    #s2.angle(-22) # move to 45 degrees 俯仰  平放桌面可看到6个色卡的角度  度数增加摄像头下弯 度数减少摄像头上仰
    #time.sleep(1800)
    #s1.angle(-40) # move to 45 degrees -40度正中间度数 转动  俯视往左度数增加 往右度数减小
    #time.sleep(1800)
    #s1.angle(-50) # move to 45 degrees 右转转动识别第三个干旱色卡
    #time.sleep(1800)
    #s1.angle(-30) # move to 45 degrees 左转转动识别第一个干旱色卡
    #time.sleep(1800)
    clock.tick()
    img = sensor.snapshot()
    for blobs in img.find_blobs([red],  merge=True,y_stride = 10,area_threshold = 50,roi = [rectX0,rectY0,rectXL,rectYL]): #merge=True不合并所有没有被过滤掉的色块，这些色块的边界矩形不互相交错重叠 y_stride查找某色块时需要跳过的y像素的数量 若一个色块的边界框区域小于 area_threshold ，则会被过滤掉  roi 是感兴趣区域的矩形元组(x，y，w，h)。
        if blobs:
            img.draw_rectangle(blobs.rect(),(255,0,0))  
            r_x += blobs.cx() * blobs.w()   #blob.w()返回色块的边界框的w坐标(int)。  blob.cx()返回色块(int)的中心x位置。 LHL
            #r_y += blobs.cy()
            r_x_weighted += blobs.w()
            r_num += 1
            img.draw_cross(r_x, r_y, (255,0,0), size=5)#画准星
    print(clock.fps())