已经初始化都置低电平了

# Blob Detection Example with Progressive Stopping

import sensor, image, time
import car
from pid import PID
import pyb  # 用于硬件操作


from pyb import Pin

ain1 =  Pin('P0', Pin.OUT_PP)
ain2 =  Pin('P1', Pin.OUT_PP)
bin1 =  Pin('P2', Pin.OUT_PP)
bin2 =  Pin('P3', Pin.OUT_PP)
ain1.low()
ain2.low()
bin1.low()
bin2.low()


# 颜色阈值设置
red_threshold = (0, 76, -2, 127, 13, 114)
size_threshold = 2000  # PID控制的目标大小
STOP_THRESHOLD = 10000  # 完全停止的色块大小
SLOW_THRESHOLD = 8000  # 开始减速的色块大小

# 初始化传感器
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(10)
sensor.set_auto_whitebal(False)
clock = time.clock()

# 初始化串口通信
uart = pyb.UART(3, 115200)  # 使用串口3，波特率115200
uart.init(115200, bits=8, parity=None, stop=1)  # 配置串口参数

# PID控制器初始化
x_pid = PID(p=0.2, i=0.3, imax=30)  # 水平位置控制
h_pid = PID(p=0.01, i=0.02, imax=20)  # 大小(距离)控制

def find_max(blobs):
    max_size = 0
    for blob in blobs:
        if blob[2] * blob[3] > max_size:
            max_blob = blob
            max_size = blob[2] * blob[3]
    return max_blob

while(True):
    clock.tick()
    img = sensor.snapshot()

    blobs = img.find_blobs([red_threshold])
    if blobs:
        max_blob = find_max(blobs)
        blob_area = max_blob[2] * max_blob[3]  # 计算色块面积
        x_error = max_blob[5] - img.width() / 2

        # 绘制检测到的色块
        img.draw_rectangle(max_blob[0:4])
        img.draw_cross(max_blob[5], max_blob[6])

        print("x error:", x_error, "area:", blob_area)

        if blob_area >= STOP_THRESHOLD:
            # 情况1：色块足够大，完全停止
            car.run(0, 0)
            print("完全停止 - 目标足够近")
            uart.write("STOP\n")  # 向STM32发送停止消息
             # 退出循环，不再继续移动
        elif blob_area >= SLOW_THRESHOLD:
            # 情况2：色块较大，开始减速
            speed_factor = 1 - (blob_area - SLOW_THRESHOLD) / (STOP_THRESHOLD - SLOW_THRESHOLD)
            h_error = blob_area - size_threshold
            x_output = x_pid.get_pid(x_error, 1) * speed_factor
            h_output = h_pid.get_pid(h_error, 1) * speed_factor
            car.run(-h_output - x_output, -h_output + x_output)
            print("减速接近 - 速度因子:", speed_factor)
        else:
            # 情况3：正常PID控制
            h_error = blob_area - size_threshold
            x_output = x_pid.get_pid(x_error, 1)
            h_output = h_pid.get_pid(h_error, 1)
            car.run(-h_output - x_output, -h_output + x_output)
            print("正常追踪 - h_output:", h_output)
    else:
        # 没有检测到色块，原地旋转寻找
        # car.run(18, -18)
        ain1.low()
        ain2.low()
        bin1.low()
        bin2.low()
        print("寻找目标...")