import sensor, lcd, time
from machine import Timer, PWM, UART, Timer
lcd.init(freq=15000000)                 # 初始化LCD
sensor.reset()                          # 复位和初始化摄像头
sensor.set_vflip(1)                     # 将摄像头设置成后置方式（所见即所得）
sensor.set_pixformat(sensor.RGB565)     # 设置像素格式为彩色 RGB565
sensor.set_framesize(sensor.QVGA)       # 设置帧大小为 QVGA (320x240)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
# sensor.set_auto_gain(0,0)
sensor.set_brightness(-1)

sensor.skip_frames(time = 200)          # 等待设置生效

# 阈值设置
# thresholds = [(35, 100, 6, 127, 0, 127),  # 红色阈值
#               (25, 100, -6, 127, 5, 43),  # 黄色阈值
#               (87, 100, -59, 127, -10, 127)]  # 绿色阈值
# thresholds = [(33, 55, 21, 93, -28, 56, "红色"),  # 红色阈值
#                (20, 100, -30, 34, 0, 45, "黄色"),  # 黄色阈值
#                (52, 100, -128, -5, 5, 127, "绿色")]  # 绿色阈值

# thresholds = [(24, 61, 37, 85, 15, 90, "红色"),  # 红色阈值     中午
#                (20, 100, -30, 34, 0, 45, "黄色"),  # 黄色阈值
#                (52, 100, -128, -5, 5, 127, "绿色")]  # 绿色阈值

# thresholds = [(21, 57, 15, 108, 12, 126, "红色"),  # 红色阈值     4：30
#                (23, 100, -28, 62, 2, 58, "黄色"),  # 黄色阈值
#                (52, 100, -128, -5, 5, 127, "绿色")]  # 绿色阈值

thresholds = [(56, 99, 13, 63, -18, 37, "红色"),  # 红色阈值
               (69, 100, -7, 14, 1, 46, "黄色"),  # 黄色阈值
               (55, 100, -51, -16, -9, 83, "绿色")]  # 绿色阈值
def Servo(angle):
    '''
    说明：舵机控制函数
    功能：180度舵机：angle:-90至90 表示相应的角度
         360连续旋转度舵机：angle:-90至90 旋转方向和速度值。
        【duty】占空比值：0-100
    '''
    #PWM通过定时器配置，接到IO17引脚
    tim_pwm = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
    S1 = PWM(tim_pwm, freq=50, duty=0, pin=17)
    S1.duty((angle+90)/180*10+2.5)

Servo(24)
while True:
    img = sensor.snapshot()
    max_blobs = []
    img = img.crop((160, 0, 80, 224))
    max_blob_color = "无"
    for threshold in thresholds:
        # 查找每种颜色的色块
        blobs = img.find_blobs([threshold[:-1]], pixels_threshold=200, area_threshold=200, merge=True)
        if blobs:
            # 找到最大的色块
            max_blob = max(blobs, key=lambda b: b.pixels())
            max_blobs.append(max_blob)
            max_blob_color = threshold[-1]  # 颜色标签


    # 绘制每种颜色最大色块的矩形和中心十字
    for b in max_blobs:
        img.draw_rectangle(b[0:4])
        img.draw_cross(b[5], b[6])
    print("最大颜色：", max_blob_color)

    lcd.display(img)