import sensor, image, time
from pyb import Pin, Timer

sensor.reset()  # 重置摄像头传感器
sensor.set_pixformat(sensor.RGB565)  # 设置像素格式为RGB565，可以根据需要选择其他支持的格式
sensor.set_framesize(sensor.QVGA)  # 设置帧大小为QVGA，可以根据需要选择其他支持的帧大小
sensor.skip_frames(time = 2000)  # 等待摄像头稳定（2秒）


red_threshold =(24, 100, -128, 127, 16, 127)
yellow_threshold = (74, 100, -128, 127, 9, 127)
green_threshold =(51, 100, -69, 127, 18, 127)

red_color_code = 1
yellow_color_code = 2
green_color_code = 8

servo1_pin = Pin('P7') # 初始化舵机1引脚P7
servo2_pin = Pin('P8') # 初始化舵机2引脚P8

timer1 = Timer(4, freq=50) # 设置定时器4的频率为50Hz
timer2 = Timer(4, freq=50) # 设置定时器3的频率为50Hz


servo1_pwm = timer1.channel(1, Timer.PWM, pin=servo1_pin, pulse_width=1500) # 设置舵机1的PWM信号输出通道
servo2_pwm = timer2.channel(1, Timer.PWM, pin=servo2_pin, pulse_width=1500) # 设置舵机2的PWM信号输出通道

while True:
    img = sensor.snapshot()

    blobs = img.find_blobs([red_threshold, yellow_threshold, green_threshold], area_threshold=100)

    if blobs:
        for blob in blobs:
            x = blob[0]
            y = blob[1]
            width = blob[2]
            height = blob[3]

            center_x = blob[5]
            center_y = blob[6]

            color_code = blob[8]

            if color_code == red_color_code:
                img.draw_string(x, y - 10, "red", color=(0xFF, 0x00, 0x00))
                print("3")
                servo1_pwm.pulse_width(1500 + 30 * 11.11) # 舵机1旋转30度
                servo2_pwm.pulse_width(1500 + 30 * 11.11) # 舵机2旋转30度
            elif color_code == yellow_color_code:
                img.draw_string(x, y - 10, "yellow", color=(0xFF, 0x00, 0x00))
                print("2")
                servo1_pwm.pulse_width(1500) # 舵机1保持不动
                servo2_pwm.pulse_width(1500 + 30 * 11.11) # 舵机2旋转30度
            elif color_code == green_color_code:
                img.draw_string(x, y - 10, "green", color=(0xFF, 0x00, 0x00))
                print("1")
                servo1_pwm.pulse_width((1500 + 30 * 11.11)) # 舵机1旋转30度
                servo2_pwm.pulse_width(1500 ) # 舵机2保持不动


            img.draw_rectangle([x, y, width, height])
            img.draw_cross(center_x, center_y)








#，我接的是P7和P8引脚，代码如下，代码不报错，但是舵机也不转动，我的舵机是MG995，旋转180度的，请问是为什么