import time, os, sys, math
from media.sensor import *
from media.display import *
from media.media import *
from machine import PWM, FPIOA
sensor = None
fps = time.clock()
os.exitpoint(os.EXITPOINT_ENABLE) # 初始化退出点检测
# 屏幕参数
SCREEN_WIDTH = 320 # QVGA宽度
SCREEN_HEIGHT = 240 # QVGA高度
SCREEN_CENTER_X = SCREEN_WIDTH // 2
SCREEN_CENTER_Y = SCREEN_HEIGHT // 2
# 中心区域参数
CENTER_ZONE_RATIO = 0.4 # 中心区域占屏幕的比例
CENTER_ZONE_WIDTH = int(SCREEN_WIDTH * CENTER_ZONE_RATIO)
CENTER_ZONE_HEIGHT = int(SCREEN_HEIGHT * CENTER_ZONE_RATIO)
CENTER_ZONE_X = (SCREEN_WIDTH - CENTER_ZONE_WIDTH) // 2
CENTER_ZONE_Y = (SCREEN_HEIGHT - CENTER_ZONE_HEIGHT) // 2
# A4纸长宽比参数
A4_RATIO = 1.4142
RATIO_TOLERANCE = 0.08
# 舵机参数
HORIZONTAL_MAX_ANGLE = 270 # 水平舵机最大角度(度)
VERTICAL_MAX_ANGLE = 180 # 垂直舵机最大角度(度)
HORIZONTAL_CENTER = 135 # 水平中心位置
VERTICAL_CENTER = 90 # 垂直中心位置
# 初始化FPIOA
fpioa = FPIOA()
fpioa.set_function(47, FPIOA.PWM3) # 水平舵机 -> PWM3 (引脚47)
fpioa.set_function(46, FPIOA.PWM2) # 垂直舵机 -> PWM2 (引脚46)
# 初始化PWM（频率50Hz）
pwm_lr = PWM(3, freq=50, duty=0) # 通道3对应PWM3
pwm_ud = PWM(2, freq=50, duty=0) # 通道2对应PWM2
# 舵机控制模块 PID
class PID:
def __init__(self, kp, ki, kd, max_i=1000, max_out=10):
self.kp = kp #静态参数
self.ki = ki
self.kd = kd
self.max_i = max_i # 积分限幅
self.max_out = max_out # 输出限幅
self.error_sum = 0 #当前误差
self.last_error = 0 #之前误差
def compute(self, target, current): # 计算PID输出
error = target - current
# P项
p_term = self.kp * error
# I项（带限幅）
self.error_sum += error
self.error_sum = max(-self.max_i, min(self.error_sum, self.max_i))
i_term = self.ki * self.error_sum
# D项
d_term = self.kd * (error - self.last_error)
self.last_error = error
# 总和并限幅
output = p_term + i_term + d_term
return max(-self.max_out, min(output, self.max_out))
# PID参数调整
pid_lr = PID(0.05, 0.0005, 0.03) # 水平方向PID
pid_ud = PID(0.05, 0.0005, 0.03) # 垂直方向PID
# 指数移动平均滤波器
class EMAFilter:
def __init__(self, alpha=0.2):
self.alpha = alpha
self.value = None
def update(self, new_value):
if self.value is None:
self.value = new_value
else:
self.value = self.alpha * new_value + (1 - self.alpha) * self.value
return self.value
# 滤波强度调整（值越小越平滑）
filter_x = EMAFilter(0.15)
filter_y = EMAFilter(0.15)
def angle_to_duty(angle, max_angle): # 将角度转换为占空比（0-100%）
pulse_min = 500 # 0°对应500us
pulse_max = 2500 # max_angle对应2500us
pulse_width = pulse_min + (pulse_max - pulse_min) * angle / max_angle
return max(2.5, min(duty_cycle, 12.5)) # 确保在安全范围内
def set_servo_angles(angle_h, angle_v): # 设置舵机角度（带限幅保护）
angle_h = max(0, min(angle_h, HORIZONTAL_MAX_ANGLE))
angle_v = max(0, min(angle_v, VERTICAL_MAX_ANGLE))
pwm_lr.duty(angle_to_duty(angle_h, HORIZONTAL_MAX_ANGLE))
pwm_ud.duty(angle_to_duty(angle_v, VERTICAL_MAX_ANGLE))
try:
# 初始化传感器
sensor = Sensor()
sensor.reset()
# 主通道配置 (800x480 YUV420SP)
sensor.set_framesize(width = 800, height = 480)
sensor.set_pixformat(Sensor.YUV420SP)
bind_info = sensor.bind_info()
Display.bind_layer(**bind_info, layer=Display.LAYER_VIDEO1)
# 副通道配置 (QVGA 320x240 RGB565)
sensor.set_framesize(Sensor.QVGA, chn=CAM_CHN_ID_2)
sensor.set_pixformat(Sensor.RGB565, chn=CAM_CHN_ID_2)
# 初始化显示和媒体系统
Display.init(Display.ST7701)
MediaManager.init()
sensor.run()
fps = time.clock()
sensor.set_hmirror(True)
sensor.set_vflip(True)
sensor.skip_frames(500) # 跳过初始帧稳定摄像头
# 初始化舵机位置
current_angle_h = HORIZONTAL_CENTER # 水平居中
current_angle_v = VERTICAL_CENTER # 垂直居中
set_servo_angles(current_angle_h, current_angle_v)
# 目标跟踪状态
target_lost_count = 0
MAX_TARGET_LOST = 10 # 连续10帧丢失目标后复位
clock = time.clock()
last_detection_time = time.time()
# 主循环
while True:
fps.tick() # 更新帧率计时
os.exitpoint() # 检查退出信号
clock.tick()
# 从副通道获取RGB565图像
img = sensor.snapshot(chn=CAM_CHN_ID_2)
# 绘制屏幕中心点
img.draw_circle(SCREEN_CENTER_X, SCREEN_CENTER_Y, 8,
color=(255, 255, 0), thickness=1) # 黄色实心圆点
# 高效灰度转换与二值化（合并操作）
gray = img.to_grayscale(copy=False) # 避免复制新图像
gray.binary([(0, 40)], invert=False) # 直接操作原灰度图，调整阈值范围，适应不同光照
# 形态学优化（增强连通性）
gray.dilate(2) # 膨胀填补小孔
gray.erode(1) # 腐蚀平滑边缘
# 在图像中查找矩形
rects = img.find_rects(threshold = 60000) #精度
# 矩形处理逻辑
valid_rects = []
for rect in rects:
x, y, w, h = rect.rect()
center_x, center_y = x + w//2, y + h//2
area = w * h
# 长宽比检测（防除零）
if min(w, h) > 10:
current_ratio = max(w, h) / min(w, h)
is_a4 = abs(current_ratio - A4_RATIO) < RATIO_TOLERANCE
else:
continue
# 中心区域检测
in_center = (CENTER_ZONE_X <= center_x <= CENTER_ZONE_X + CENTER_ZONE_WIDTH and
CENTER_ZONE_Y <= center_y <= CENTER_ZONE_Y + CENTER_ZONE_HEIGHT)
if is_a4 and in_center:
a4_detected = True
last_detection_time = time.time()
valid_rects.append(rect) # 仅缓存有效矩形
# 绘制:蓝色（1, 147, 230）绿色（0, 255, 0）红色（255, 0, 0）
img.draw_line(SCREEN_CENTER_X, SCREEN_CENTER_Y, center_x, center_y,
color=(0, 255, 255), thickness=3) # 矩形中心到屏幕中心的青色连线
img.draw_rectangle(rect.rect(), color=(1, 147, 230), thickness=5)# 蓝色矩形框
img.draw_circle(center_x, center_y, 10, color=(0, 255, 0), thickness=0) #绿色中心点
print(f"矩形中心({center_x}, {center_y})")
# 舵机控制逻辑
if is_a4:
a4_detected = True
target_lost_count = 0
last_detection_time = time.time()
# 计算偏移误差（像素）
error_x = center_x - SCREEN_CENTER_X
error_y = center_y - SCREEN_CENTER_Y
# 应用滤波
filtered_error_x = filter_x.update(error_x)
filtered_error_y = filter_y.update(error_y)
# 计算PID调整量,注意：舵机方向需要根据实际安装调整符号
adj_x = pid_lr.compute(0, filtered_error_x)
adj_y = pid_ud.compute(0, filtered_error_y)
# 更新舵机角度
current_angle_h -= adj_x # 根据实际安装调整符号
current_angle_v += adj_y # 根据实际安装调整符号
# 限幅保护
current_angle_h = max(0, min(current_angle_h, HORIZONTAL_MAX_ANGLE))
current_angle_v = max(0, min(current_angle_v, VERTICAL_MAX_ANGLE))
# 设置舵机位置
set_servo_angles(current_angle_h, current_angle_v)
# 目标丢失处理
if not a4_detected:
target_lost_count += 1
if target_lost_count > MAX_TARGET_LOST:
# 缓慢返回中心位置
adj_x = pid_lr.compute(0, SCREEN_CENTER_X - current_angle_h)
adj_y = pid_ud.compute(0, SCREEN_CENTER_Y - current_angle_v)
current_angle_h += adj_x * 0.1
current_angle_v += adj_y * 0.1
set_servo_angles(current_angle_h, current_angle_v)
# 完全复位
if target_lost_count > MAX_TARGET_LOST * 2:
current_angle_h = HORIZONTAL_CENTER
current_angle_v = VERTICAL_CENTER
set_servo_angles(current_angle_h, current_angle_v)
target_lost_count = 0
# 将处理后的图像显示在OSD层 (屏幕右侧)
Display.show_image(img, x=800-320, layer=Display.LAYER_OSD1)
except KeyboardInterrupt as e:
print("用户中断:", e)
except BaseException as e:
print(f"异常: {e}")
finally:
# 清理资源
if isinstance(sensor, Sensor):
sensor.stop()
Display.deinit()
MediaManager.deinit()
os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
time.sleep_ms(100)
你是一个对can mv K230代码非常了解的工程师，请帮我找出这个代码中的潜在问题，并合理修改代码，告诉我如何修改以及修改原因
最新发布
扫一扫
5029
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
