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

# 设置引脚对象
pin1 = Pin('P0', Pin.OUT_PP)
pin2 = Pin('P1', Pin.OUT_PP)
pin3 = Pin('P2', Pin.OUT_PP)
pin4 = Pin('P3', Pin.OUT_PP)

# 初始化摄像头对象
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)

# 定义Aruco的字典
aruco_dict = image.aruco_custom_dict(6, 3)

while(True):
    img = sensor.snapshot()
    codes = img.find_apriltags(families=image.TAG25H9)

    # 检测是否有Aruco二维码
    if len(codes) > 0:
        for code in codes:
            # 获取Aruco二维码ID
            code_id = code.payload()

            # 控制引脚输出高电平
            if code_id == 0:
                pin1.high()
            elif code_id == 1:
                pin2.high()
            elif code_id == 2:
                pin3.high()
            elif code_id == 3:
                pin4.high()

    # 延迟2秒
    time.sleep(2000)

    # 将引脚输出恢复为低电平
    pin1.low()
    pin2.low()
    pin3.low()
    pin4.low()