import time, os, math
from media.sensor import *
from media.display import *
from media.media import *
from machine import FPIOA, PWM, Pin
# 配置PWM引脚和舵机
fpioa = FPIOA()
fpioa.set_function(47, FPIOA.PWM3) # 上下舵机
pwm_ud = PWM(3, 50, 4.85, enable=True)
fpioa.set_function(46, FPIOA.PWM2) # 左右舵机
pwm_lr = PWM(2, 50, 7.8, enable=True)
# 舵机控制参数
SERVO_UD_MIN_DUTY = 4.0
SERVO_UD_MAX_DUTY = 5.0
SERVO_LR_MIN_DUTY = 7.0
SERVO_LR_MAX_DUTY = 8.0
# 图像参数 - 降低处理分辨率以提高帧率
PROCESS_WIDTH = 176 # 原图350的1/2
PROCESS_HEIGHT = 120 # 原图240的1/2
picture_width = 350
picture_height = 240
sensor_id = 2
sensor = None
# 显示模式设置
DISPLAY_MODE = "LCD" # 可选 "VIRT", "LCD", "HDMI"
# 根据显示模式设置分辨率
if DISPLAY_MODE == "VIRT":
DISPLAY_WIDTH, DISPLAY_HEIGHT = 1920, 1080
elif DISPLAY_MODE == "LCD":
DISPLAY_WIDTH, DISPLAY_HEIGHT = 800, 480
elif DISPLAY_MODE == "HDMI":
DISPLAY_WIDTH, DISPLAY_HEIGHT = 1920, 1080
else:
raise ValueError("无效的显示模式")
# 角点检测阈值
CORNERS_THRESHOLD = 5000
# PID控制参数(需要根据实际调整)
KP = 0.1 # 比例系数
KI = 0.01 # 积分系数
KD = 0.02 # 微分系数
# 舵机角度范围
ANGLE_UD_MIN = 0
ANGLE_UD_MAX = 180
ANGLE_LR_MIN = 0
ANGLE_LR_MAX = 180
# 舵机当前角度
current_angle_ud = 90
current_angle_lr = 90
# PID控制变量
last_error_x = 0
last_error_y = 0
integral_x = 0
integral_y = 0
# 图像中心点(处理分辨率下的中心)
TARGET_CX = PROCESS_WIDTH // 2
TARGET_CY = PROCESS_HEIGHT // 2
def servo_set_angle(pwm, angle, is_ud=True):
"""设置舵机角度"""
min_duty = SERVO_UD_MIN_DUTY if is_ud else SERVO_LR_MIN_DUTY
max_duty = SERVO_UD_MAX_DUTY if is_ud else SERVO_LR_MAX_DUTY
angle = max(0, min(180, angle))
duty = min_duty + (max_duty - min_duty) * angle / 180
pwm.duty(duty)
return angle
def sort_corners(corners):
"""对矩形角点进行排序(左上、右上、右下、左下)"""
if len(corners) != 4:
return corners
# 计算中心点
center_x = sum(c[0] for c in corners) / 4
center_y = sum(c[1] for c in corners) / 4
# 分类角点
top_left = min(corners, key=lambda c: (c[0] - center_x)**2 + (c[1] - center_y)**2 if c[0] < center_x and c[1] < center_y else float('inf'))
top_right = min(corners, key=lambda c: (c[0] - center_x)**2 + (c[1] - center_y)**2 if c[0] > center_x and c[1] < center_y else float('inf'))
bottom_right = min(corners, key=lambda c: (c[0] - center_x)**2 + (c[1] - center_y)**2 if c[0] > center_x and c[1] > center_y else float('inf'))
bottom_left = min(corners, key=lambda c: (c[0] - center_x)**2 + (c[1] - center_y)**2 if c[0] < center_x and c[1] > center_y else float('inf'))
return [top_left, top_right, bottom_right, bottom_left]
def calculate_center(corners):
"""计算矩形中心点"""
if len(corners) < 4:
return None
# 计算几何中心
center_x = sum(c[0] for c in corners) / len(corners)
center_y = sum(c[1] for c in corners) / len(corners)
return (int(center_x), int(center_y))
def draw_corner_info(img, corners, center, scale_x=1.0, scale_y=1.0):
"""在图像上绘制角点和中心信息(注意:绘制在原始图像上,需要坐标缩放)"""
if len(corners) != 4:
return
# 定义角点颜色和标签
corner_colors = [(255, 0, 0), (0, 255, 0), (0, 0, 255), (255, 255, 0)]
labels = ["TL", "TR", "BR", "BL"]
# 绘制角点(坐标需要缩放回原始图像大小)
for i, (x, y) in enumerate(corners):
# 将处理图像的坐标映射回原始图像
orig_x = int(x * scale_x)
orig_y = int(y * scale_y)
color = corner_colors[i]
# 绘制角点标记
img.draw_circle(orig_x, orig_y, 8, color=color, thickness=2)
# 绘制角点标签
img.draw_string(orig_x + 10, orig_y - 10, labels[i], color=color, scale=2)
# 显示坐标
img.draw_string(orig_x - 30, orig_y + 15, f"({orig_x},{orig_y})", color=color, scale=1.5)
# 绘制中心点
if center:
cx, cy = center
orig_cx = int(cx * scale_x)
orig_cy = int(cy * scale_y)
# 绘制中心十字
img.draw_cross(orig_cx, orig_cy, size=15, color=(255, 0, 255), thickness=2)
img.draw_circle(orig_cx, orig_cy, 5, color=(255, 0, 255), thickness=-1)
# 显示中心坐标
img.draw_string(orig_cx - 40, orig_cy - 30, f"Center: ({orig_cx},{orig_cy})", color=(255, 0, 255), scale=2)
def process_frame(img, process_width, process_height):
"""处理图像帧,检测矩形角点和中心"""
# 创建处理图像(缩小尺寸以提高处理速度)
img_process = img.copy(process_width, process_height)
# 转换为灰度图
img_gray = img_process.to_grayscale(copy=True)
# 二值化处理
img_bin = img_gray.binary([(60, 180)]) # 调整阈值以适应不同光照
# 查找矩形
rects = img_bin.find_rects(threshold=CORNERS_THRESHOLD)
# 找出最大的矩形
max_rect = None
max_area = 0
for rect in rects:
area = rect.w() * rect.h()
if area > max_area:
max_area = area
max_rect = rect
corners = []
center = None
if max_rect:
# 获取角点
corners = max_rect.corners()
# 排序角点
corners = sort_corners(corners)
# 计算中心
center = calculate_center(corners)
# 在原始图像上绘制矩形边框(需要坐标缩放)
# 注意:这里我们将在原始图像上绘制,所以需要将坐标缩放回去
scale_x = img.width() / process_width
scale_y = img.height() / process_height
# 绘制四条边
for i in range(4):
x1, y1 = corners[i]
x2, y2 = corners[(i + 1) % 4]
orig_x1 = int(x1 * scale_x)
orig_y1 = int(y1 * scale_y)
orig_x2 = int(x2 * scale_x)
orig_y2 = int(y2 * scale_y)
img.draw_line(orig_x1, orig_y1, orig_x2, orig_y2, color=(0, 255, 255), thickness=2)
# 绘制角点和中心信息(传入缩放因子)
draw_corner_info(img, corners, center, scale_x, scale_y)
return corners, center
def pid_controller(error, last_error, integral, kp, ki, kd):
"""PID控制器计算输出"""
# 积分项累加
integral += error
# 微分项(当前误差和上一次误差的差值)
derivative = error - last_error
# PID输出
output = kp * error + ki * integral + kd * derivative
return output, integral, error
try:
# 初始化摄像头
sensor = Sensor(id=sensor_id)
sensor.reset()
sensor.set_framesize(width=picture_width, height=picture_height, chn=CAM_CHN_ID_0)
sensor.set_pixformat(Sensor.RGB565, chn=CAM_CHN_ID_0)
# 初始化显示器
if DISPLAY_MODE == "VIRT":
Display.init(Display.VIRT, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, fps=60)
elif DISPLAY_MODE == "LCD":
Display.init(Display.ST7701, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, to_ide=True)
elif DISPLAY_MODE == "HDMI":
Display.init(Display.LT9611, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, to_ide=True)
# 初始化媒体管理器
MediaManager.init()
sensor.run()
clock = time.clock()
# 舵机初始位置
current_angle_ud = 90
current_angle_lr = 90
servo_set_angle(pwm_ud, current_angle_ud, is_ud=True)
servo_set_angle(pwm_lr, current_angle_lr, is_ud=False)
# PID控制变量
last_error_x = 0
last_error_y = 0
integral_x = 0
integral_y = 0
# 舵机控制死区(像素),避免微小移动
DEAD_ZONE = 5
while True:
os.exitpoint()
clock.tick()
# 捕获图像
img = sensor.snapshot(chn=CAM_CHN_ID_0)
# 处理图像并检测角点(使用降低的分辨率)
corners, center = process_frame(img, PROCESS_WIDTH, PROCESS_HEIGHT)
# 显示FPS
img.draw_string(10, 10, f"FPS: {clock.fps():.1f}", color=(255, 0, 0), scale=2)
# 显示图像
img.compressed_for_ide()
Display.show_image(img,
x=int((DISPLAY_WIDTH - picture_width) / 2),
y=int((DISPLAY_HEIGHT - picture_height) / 2))
# 如果检测到中心点,则进行舵机PID控制
if center:
cx, cy = center
# 计算误差(目标中心点与当前中心点的差值)
error_x = TARGET_CX - cx
error_y = TARGET_CY - cy
# 死区处理
if abs(error_x) < DEAD_ZONE:
error_x = 0
if abs(error_y) < DEAD_ZONE:
error_y = 0
# 使用PID控制器计算舵机角度调整量
# 注意:舵机角度调整方向与误差方向相反(因为需要将中心点移向目标)
# 水平方向(左右舵机):error_x为负,说明当前中心点在目标中心点右侧,需要向左转(增加角度)
# 垂直方向(上下舵机):error_y为负,说明当前中心点在目标中心点下方,需要向上转(减小角度)
# 注意:舵机安装方向可能影响正负,这里假设舵机角度增加时,摄像头向右转(图像中心点向右移)和向下转(图像中心点向下移)
# 因此,水平方向:error_x为负(当前点在目标点右侧)时,舵机角度应增加(向右转,使图像中心点左移?)
# 实际上,我们需要的是:当中心点偏右时,舵机应该向左转,使中心点左移。所以舵机角度应该减小(因为舵机角度0对应最左,180对应最右)
# 所以,水平方向:error_x = 目标中心x - 当前中心x -> 当前中心在目标中心右侧时error_x为负,此时我们希望舵机角度减小(向左转)
# 因此,水平舵机调整量应该为负(error_x为正时舵机角度增加,向右转;为负时舵机角度减小,向左转)
# 同样,垂直方向:error_y = 目标中心y - 当前中心y -> 当前中心在目标中心下方时error_y为负,我们希望舵机角度增加(向下转?)
# 但是,我们希望当前中心点偏下时,舵机应该向上转,使中心点上移。所以舵机角度应该减小(因为舵机角度0对应最上,180对应最下)
# 因此,垂直舵机调整量应该为正(error_y为正时,当前中心在目标中心上方,舵机应该向下转,角度增加;error_y为负时,舵机角度减小)
# 根据以上分析,我们调整舵机的方向:
# 水平舵机:调整量 = - (PID输出)
# 垂直舵机:调整量 = - (PID输出) # 注意:这里取决于舵机安装方向,可能需要调整符号
# 由于我们之前假设舵机角度增加方向与需要移动的方向相反,所以我们用负号
# 或者,我们可以通过调整PID系数的符号来适应,这里我们使用负号
# 水平方向PID
adjustment_x, integral_x, last_error_x = pid_controller(error_x, last_error_x, integral_x, KP, KI, KD)
# 垂直方向PID
adjustment_y, integral_y, last_error_y = pid_controller(error_y, last_error_y, integral_y, KP, KI, KD)
# 更新舵机角度(注意:舵机角度范围限制)
# 水平舵机(左右):调整量取负,因为舵机角度增加时摄像头右转(图像中心右移),而我们需要左移(减小角度)来使中心点向左移动
# 垂直舵机(上下):调整量取负,因为舵机角度增加时摄像头下转(图像中心下移),而我们需要上移(减小角度)来使中心点向上移动
current_angle_lr -= adjustment_x
current_angle_ud -= adjustment_y
# 限制舵机角度范围
current_angle_lr = max(ANGLE_LR_MIN, min(ANGLE_LR_MAX, current_angle_lr))
current_angle_ud = max(ANGLE_UD_MIN, min(ANGLE_UD_MAX, current_angle_ud))
# 设置舵机角度
servo_set_angle(pwm_ud, current_angle_ud, is_ud=True)
servo_set_angle(pwm_lr, current_angle_lr, is_ud=False)
# 打印调试信息
print(f"Error: ({error_x}, {error_y}), Adjust: ({adjustment_x:.2f}, {adjustment_y:.2f}), Angles: ({current_angle_ud:.1f}, {current_angle_lr:.1f})")
# 控制舵机的频率可以降低,例如每帧都控制可能太快,可以每2帧控制一次(这里每帧都控制,但PID参数较小)
except KeyboardInterrupt:
print("程序被用户中断")
except Exception as e:
print(f"发生错误: {e}")
finally:
# 清理资源
if sensor:
sensor.stop()
Display.deinit()
pwm_ud.enable(False)
pwm_lr.enable(False)
MediaManager.deinit()
此代码显示如下错误:find sensor gc2093_csi2, type 24, output 1920x1080@60
vb common pool count 4
sensor(0), mode 0, buffer_num 4, buffer_size 0
发生错误: image mmz alloc failed: -1610383348
MPY: soft reboot 修改使其正常运行
最新发布