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()