以下是main.py的代码：
import sensor, time
from pid import PID
from pyb import Servo
# 舵机初始化（修改了舵机方向定义，添加初始位置设置）
pan_servo = Servo(1) # 水平舵机
tilt_servo = Servo(2) # 俯仰舵机
# 校准舵机（标准舵机参数）
pan_servo.calibration(500, 2500, 500)
tilt_servo.calibration(500, 2500, 500)
# 设置初始位置（重要！防止启动时抖动）
pan_servo.angle(90) # 中间位置
tilt_servo.angle(90) # 中间位置
time.sleep_ms(500) # 等待舵机到位
# 红色阈值（保持原有）
red_threshold = (13, 49, 18, 61, 6, 47)
# 增强PID参数（大幅提高比例系数）
pan_pid = PID(p=0.7, i=0, imax=90) # 水平PID（P值增大10倍）
tilt_pid = PID(p=0.5, i=0, imax=90) # 俯仰PID（P值增大10倍）
# 增益参数（新增）
PAN_GAIN = 3.0 # 水平输出放大倍数
TILT_GAIN = 2.0 # 俯仰输出放大倍数
MAX_STEP = 25 # 单次最大变化角度
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(20) # 增加跳帧确保稳定
sensor.set_auto_whitebal(False)
clock = time.clock()
def find_max(blobs):
max_size = 0
for blob in blobs:
if blob.w() * blob.h() > max_size:
max_blob = blob
max_size = blob.w() * blob.h()
return max_blob
# 死区阈值（防止微小振动）
DEADZONE = 5
while(True):
clock.tick()
img = sensor.snapshot()
blobs = img.find_blobs([red_threshold])
if blobs:
max_blob = find_max(blobs)
# 计算中心误差
pan_error = max_blob.cx() - img.width()/2
tilt_error = max_blob.cy() - img.height()/2
# 绘制识别框
img.draw_rectangle(max_blob.rect())
img.draw_cross(max_blob.cx(), max_blob.cy())
# 计算PID输出
pan_output = pan_pid.get_pid(pan_error, 1) * PAN_GAIN
tilt_output = tilt_pid.get_pid(tilt_error, 1) * TILT_GAIN
# 应用死区（误差过小时归零）
if abs(pan_error) < DEADZONE:
pan_output = 0
if abs(tilt_error) < DEADZONE:
tilt_output = 0
# 限幅处理（确保每次变化不超过MAX_STEP）
pan_output = max(-MAX_STEP, min(MAX_STEP, pan_output))
tilt_output = max(-MAX_STEP, min(MAX_STEP, tilt_output))
# 获取当前角度并计算新角度
current_pan = pan_servo.angle()
current_tilt = tilt_servo.angle()
new_pan = current_pan + pan_output
new_tilt = current_tilt - tilt_output # 注意符号匹配硬件
# 最终角度限幅（0-180度保护）
new_pan = max(0, min(180, new_pan))
new_tilt = max(0, min(180, new_tilt))
# 设置舵机
pan_servo.angle(new_pan)
tilt_servo.angle(new_tilt)
# 增强调试输出
print("误差: H={:.1f}, V={:.1f} | 输出: ΔH={:.2f}°, ΔV={:.2f}° | 位置: H={:.1f}°, V={:.1f}°".format(
pan_error, tilt_error, pan_output, tilt_output, new_pan, new_tilt))
else:
# 没有检测到物体时重置PID
pan_pid.reset_I()
tilt_pid.reset_I()
print("目标丢失，PID重置")
# 控制频率（约20Hz）
time.sleep_ms(50)
以下是对应的pid.py代码：
from pyb import millis
from math import pi, isnan
class PID:
_kp = _ki = _kd = _integrator = _imax = 0
_last_error = _last_derivative = _last_t = 0
_RC = 1/(2 * pi * 20)
def __init__(self, p=0, i=0, d=0, imax=0):
self._kp = float(p)
self._ki = float(i)
self._kd = float(d)
self._imax = abs(imax)
self._last_derivative = float('nan')
def get_pid(self, error, scaler):
tnow = millis()
dt = tnow - self._last_t
output = 0
if self._last_t == 0 or dt > 1000:
dt = 0
self.reset_I()
self._last_t = tnow
delta_time = float(dt) / float(1000)
output += error * self._kp
if abs(self._kd) > 0 and dt > 0:
if isnan(self._last_derivative):
derivative = 0
self._last_derivative = 0
else:
derivative = (error - self._last_error) / delta_time
derivative = self._last_derivative + \
((delta_time / (self._RC + delta_time)) * \
(derivative - self._last_derivative))
self._last_error = error
self._last_derivative = derivative
output += self._kd * derivative
output *= scaler
if abs(self._ki) > 0 and dt > 0:
self._integrator += (error * self._ki) * scaler * delta_time
if self._integrator < -self._imax: self._integrator = -self._imax
elif self._integrator > self._imax: self._integrator = self._imax
output += self._integrator
return output
def reset_I(self):
self._integrator = 0
self._last_derivative = float('nan')