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

初始化摄像头

sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QVGA) # 320x240分辨率
sensor.set_auto_gain(False) # 关闭自动增益
sensor.set_auto_whitebal(False) # 关闭自动白平衡
sensor.set_auto_exposure(False, exposure_us=20000) # 固定曝光时间
sensor.skip_frames(time=1000) # 等待设置生效

系统参数配置

CENTER_X = 160 # 图像中心X坐标(320/2)
CENTER_Y = 120 # 图像中心Y坐标(240/2)
DEAD_ZONE = 5 # 死区阈值(像素)，避免微小抖动
MAX_ANGLE_CHANGE = 2.0 # 单次最大角度变化量(度)

创建PID控制器

class PIDController:
def init(self, kp, ki, kd, max_i=100):
self.kp = kp # 比例系数
self.ki = ki # 积分系数
self.kd = kd # 微分系数
self.max_i = max_i # 积分项上限
self.reset()

def reset(self):
    self.last_error = 0
    self.integral = 0

def compute(self, error):
    # 比例项
    p_term = self.kp * error
    
    # 积分项(带限幅)
    self.integral += error
    self.integral = max(min(self.integral, self.max_i), -self.max_i)
    i_term = self.ki * self.integral
    
    # 微分项
    d_term = self.kd * (error - self.last_error)
    self.last_error = error
    
    # PID输出
    return p_term + i_term + d_term

初始化PID控制器(X和Y轴独立控制)

pid_x = PIDController(kp=0.08, ki=0.001, kd=0.02)
pid_y = PIDController(kp=0.08, ki=0.001, kd=0.02)

舵机初始化

def setup_servos():
# 创建50Hz PWM定时器(周期20ms)
# 使用Timer 2，通道1和2
tim_servo = Timer(4, freq=50) # 50Hz频率

# 舵机1(水平方向) - P7对应Timer 2 Channel 1
servo1_ch = tim_servo.channel(1, Timer.PWM, pin=Pin('P7'))

# 舵机2(垂直方向) - P8对应Timer 2 Channel 2
servo2_ch = tim_servo.channel(2, Timer.PWM, pin=Pin('P8'))

return servo1_ch, servo2_ch

角度到PWM转换函数

def angle_to_pwm(angle):
"""将角度(0-180度)转换为PWM占空比"""
angle = max(0, min(180, angle))
pulse_width = 1000 + angle * (1000 / 180) # 脉冲宽度(us)
return pulse_width / 20000 * 100 # 占空比(%)

设置舵机角度

def set_servo_angle(channel, angle):
duty_cycle = angle_to_pwm(angle)
channel.pulse_width_percent(int(duty_cycle))

激光控制

laser_pin = Pin('P6', Pin.OUT_PP)

def control_laser(state):
laser_pin.value(1 if state else 0)

矩形质量评估函数

def evaluate_rect_quality(rect):
"""评估矩形质量(面积+长宽比)"""
area = rect.w() * rect.h()
aspect_ratio = min(rect.w()/rect.h(), rect.h()/rect.w()) # 标准化为0-1
return area * aspect_ratio # 面积越大、越接近正方形得分越高

主函数

def main():
# 初始化舵机
servo1_ch, servo2_ch = setup_servos()

# 初始角度(中间位置)
servo1_angle = 90
servo2_angle = 90
set_servo_angle(servo1_ch, servo1_angle)
set_servo_angle(servo2_ch, servo2_angle)

# 帧率统计
clock = time.clock()
tracking_counter = 0  # 连续跟踪计数器

while True:
    clock.tick()
    img = sensor.snapshot()
    
    # 自适应二值化处理
    img.binary([(0, 160)])
    
    # 寻找矩形
    best_rect = None
    max_quality = 0
    
    for r in img.find_rects(threshold=10000):
        # 过滤小矩形
        if r.w() < 20 or r.h() < 20:
            continue
            
        # 质量评估
        quality = evaluate_rect_quality(r)
        if quality > max_quality:
            best_rect = r
            max_quality = quality
    
    # 目标跟踪逻辑
    if best_rect:
        tracking_counter = min(tracking_counter + 1, 10)  # 增加计数器(上限10)
        control_laser(True)  # 开启激光
        
        # 计算中心点
        cx = best_rect.x() + best_rect.w() / 2
        cy = best_rect.y() + best_rect.h() / 2
        
        # 计算偏差
        dx = cx - CENTER_X
        dy = cy - CENTER_Y
        
        # 死区处理
        if abs(dx) < DEAD_ZONE: dx = 0
        if abs(dy) < DEAD_ZONE: dy = 0
        
        # PID控制计算
        dx_adj = pid_x.compute(dx)
        dy_adj = pid_y.compute(dy)
        
        # 限制最大变化量
        dx_adj = max(min(dx_adj, MAX_ANGLE_CHANGE), -MAX_ANGLE_CHANGE)
        dy_adj = max(min(dy_adj, MAX_ANGLE_CHANGE), -MAX_ANGLE_CHANGE)
        
        # 更新舵机角度
        servo1_angle -= dx_adj  # 注意方向调整
        servo2_angle += dy_adj
        servo1_angle = max(0, min(180, servo1_angle))  # 限制角度范围
        servo2_angle = max(0, min(180, servo2_angle))
        
        set_servo_angle(servo1_ch, servo1_angle)
        set_servo_angle(servo2_ch, servo2_angle)
        
        # 绘制调试信息
        img.draw_rectangle(best_rect.rect(), color=255)
        img.draw_cross(int(cx), int(cy), color=255, size=10)
        img.draw_string(10, 10, f"DX:{dx:.1f} DY:{dy:.1f}", color=255)
    else:
        # 目标丢失处理
        tracking_counter = max(tracking_counter - 2, 0)
        if tracking_counter <= 0:
            control_laser(False)  # 关闭激光
            pid_x.reset()  # 重置PID积分项
            pid_y.reset()
    
    # 显示帧率
    img.draw_string(280, 10, f"FPS:{clock.fps():.1f}", color=255)

if name == "main":
main()