为什么舵机没反应，供电正常
-
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()