import sensor
import image
import time
from pyb import Servo

# 初始化OpenMV摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.skip_frames(10)

# 初始化舵机
s1 = Servo(1)  # 在X1引脚连接舵机
s2 = Servo(2)

red_count = 0  # 红球计数器
blue_count = 0  # 蓝球计数器

while red_count < 6:
    img = sensor.snapshot()  # 拍摄一张照片
    red_blobs = img.find_blobs([(30, 100, 15, 127, 15, 127)])  # 寻找红色区域
    
    if red_blobs:
        s2.angle(60)  # 转动舵机
        time.sleep(1)
        for blob in red_blobs:
            # 在红色区域中心绘制矩形和十字形标记
            img.draw_rectangle(blob.rect())
            img.draw_cross(blob.cx(), blob.cy())
            
            s1.angle(0)
            time.sleep_ms(500)
            s1.angle(60)
            time.sleep_ms(400)
            s1.angle(0)
        red_count += 1  # 红球计数加一
        print("红球计数:", red_count)  # 打印红球计数值
     
    if red_count == 6:
        break  # 红球数量达到6个，跳出循环

while blue_count < 6:
    img = sensor.snapshot()  # 拍摄一张照片
    blue_blobs = img.find_blobs([(15, 100, -128, 0, -41, 3)])  # 寻找蓝色区域
    
    if blue_blobs:        
        s2.angle(0)  # 转回0度
        time.sleep(1)
        for blob in blue_blobs:
            # 在蓝色区域中心绘制矩形和十字形标记
            img.draw_rectangle(blob.rect())
            img.draw_cross(blob.cx(), blob.cy())
            
            s1.angle(0)
            time.sleep_ms(500)
            s1.angle(60)
            time.sleep_ms(400)
            s1.angle(0)
        blue_count += 1  # 蓝球计数加一
        print("蓝球计数:", blue_count)  # 打印蓝球计数值