5.3 色块追踪小车

5.3.1 简介

色块追踪小车，AI视觉模块对色块进行锁定并且根据AI视觉模块给出的色块XY以及宽高进行左右前后的跟踪。

5.3.2 流程图

5.3.3 代码

from machine import I2C, Pin, PWM
import time
from Sengo1 import *          # 请确保 Sengo1.py 已保存在 ESP32 上

# 等待 Sengo1 初始化（重要！）
time.sleep(3)

# ----- ESP32 适配：硬件 I2C（可任意映射引脚） -----
# 根据实际接线调整 scl/sda 引脚，此处沿用原例的 GPIO22/21
port = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)

# 创建 Sengo1 对象
sengo1 = Sengo1(0x60)

# 启动传感器
err = sengo1.begin(port)
if err != SENTRY_OK:
    print(f"Initialization failed, error code: {err}")
else:
    print("Initialization succeeded")

sengo1.SetParam(sengo1_vision_e.kVisionBlob,
                [0, 0, 6, 8, color_label_e.kColorRed], 1)  # 红色

# 等待新参数生效（不小于算法一帧时间）
time.sleep(0.1)

# 开启 Blob 检测算法
err = sengo1.VisionBegin(sengo1_vision_e.kVisionBlob)
if err != SENTRY_OK:
    print(f"Starting algo Blob failed, error code: {err}")
else:
    print("Starting algo Blob succeeded")

# ----- ESP32 电机驱动引脚（可根据实际接线修改） -----
# 右轮
pin1 = Pin(32, Pin.OUT)        # 方向
pin2 = PWM(Pin(25), freq=50)   # 速度（50Hz，适合舵机/电机驱动）
# 左轮
pin3 = Pin(33, Pin.OUT)
pin4 = PWM(Pin(26), freq=50)

# 电机控制函数（占空比 0~65535）
def car_forward():
    pin1.value(0)
    pin2.duty_u16(20000)
    pin3.value(0)
    pin4.duty_u16(20000)

def car_back():
    pin1.value(1)
    pin2.duty_u16(30000)
    pin3.value(1)
    pin4.duty_u16(30000)

def car_left():
    pin1.value(0)
    pin2.duty_u16(5000)
    pin3.value(1)
    pin4.duty_u16(45000)

def car_right():
    pin1.value(1)
    pin2.duty_u16(45000)
    pin3.value(0)
    pin4.duty_u16(5000)

def car_stop():
    pin1.value(0)
    pin2.duty_u16(0)
    pin3.value(0)
    pin4.duty_u16(0)

# ----- 主循环（视觉追踪与小车控制） -----
try:
    while True:
        obj_num = sengo1.GetValue(sengo1_vision_e.kVisionBlob,
                                  sentry_obj_info_e.kStatus)

        if obj_num:
            x = sengo1.GetValue(sengo1_vision_e.kVisionBlob,
                                sentry_obj_info_e.kXValue, 1)
            y = sengo1.GetValue(sengo1_vision_e.kVisionBlob,
                                sentry_obj_info_e.kYValue, 1)
            w = sengo1.GetValue(sengo1_vision_e.kVisionBlob,
                                sentry_obj_info_e.kWidthValue, 1)
            h = sengo1.GetValue(sengo1_vision_e.kVisionBlob,
                                sentry_obj_info_e.kHeightValue, 1)

            if y >= 80:
                car_back()
            elif x <= 20:
                car_left()
            elif x >= 80:
                car_right()
            elif w <= 15 and h <= 15:
                car_forward()
            elif w >= 70 and h >= 70:
                car_back()
            else:
                car_stop()
        else:
            car_stop()

        time.sleep(0.1)

except KeyboardInterrupt:
    sengo1.VisionEnd(sengo1_vision_e.kVisionBlob)
    print("Program stopped")

5.3.4 代码结果

上传代码成功后，AI视觉模块会对拍到的画面进行识别，判断是否有红色块，如果有红色块便从这个色块的x，y，w，h值上判断小车的动作，比如红色块靠显示屏的右边小车便往右边转动，红色快靠左小车就往左边转到，红色快靠下小车就后退。（使用我们提供的颜色卡片）