import sensor
import image
import time
import math
import pyb
# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA) # 320x240分辨率
sensor.set_auto_gain(False) # 关闭自动增益
sensor.set_auto_whitebal(False) # 关闭自动白平衡
sensor.skip_frames(time=1000) # 等待设置生效
# 初始化串口通信
uart = pyb.UART(3, 115200, timeout_char=1000)
# 矩形检测参数
RECT_THRESHOLD = 3000
MIN_RECT_AREA = 5000
ASPECT_RATIO = math.sqrt(2) # A4纸长宽比≈1.414
ASPECT_TOLERANCE = 0.2
# 云台控制参数
MAX_OFFSET_X = 160
MAX_OFFSET_Y = 120
SMOOTHING_FACTOR = 0.2
# 历史位置记录
prev_offset_x = 0
prev_offset_y = 0
# 扫描状态机定义
SCAN_IDLE = 0 # 空闲状态
SCAN_LEFT = 1 # 向左扫描
SCAN_RIGHT = 2 # 向右扫描
SCAN_UP = 3 # 向上扫描
SCAN_DOWN = 4 # 向下扫描
# 扫描控制变量
scan_state = SCAN_IDLE
scan_progress = 0
scan_direction = 1
rect_lost_count = 0
RECT_LOST_THRESH = 5 # 连续5帧未检测到视为目标丢失
def calculate_center(rect):
"""计算矩形中心点坐标"""
center_x = rect.x() + rect.w() // 2
center_y = rect.y() + rect.h() // 2
return center_x, center_y
def send_to_gimbal(offset_x, offset_y):
"""通过串口发送偏移量给云台控制器"""
# 限制偏移量范围
offset_x = max(min(offset_x, MAX_OFFSET_X), -MAX_OFFSET_X)
offset_y = max(min(offset_y, MAX_OFFSET_Y), -MAX_OFFSET_Y)
# 二进制协议传输
data = bytearray([0xFF,
(offset_x >> 8) & 0xFF, offset_x & 0xFF,
(offset_y >> 8) & 0xFF, offset_y & 0xFF])
uart.write(data)
print("Offset: ({}, {})".format(offset_x, offset_y))
def is_a4_paper(rect):
"""判断矩形是否为A4纸（根据长宽比）"""
w, h = rect.w(), rect.h()
if w < h: # 确保w为长边
w, h = h, w
aspect = w / h
return abs(aspect - ASPECT_RATIO) < ASPECT_TOLERANCE
def get_rect_area(rect):
"""计算矩形面积（w*h）"""
return rect.w() * rect.h()
def gimbal_scan():
"""云台扫描控制函数"""
global scan_state, scan_progress, scan_direction
# 动态扫描参数（根据光照条件调整）
stats = img.get_statistics()
if stats.l_mean() < 50: # 环境黑暗
SCAN_STEP = 3
HORIZONTAL_LIMIT = 80
VERTICAL_LIMIT = 60
else: # 正常光照
SCAN_STEP = 5
HORIZONTAL_LIMIT = 100
VERTICAL_LIMIT = 80
if scan_state == SCAN_IDLE:
scan_state = SCAN_RIGHT
scan_progress = 0
scan_direction = 1
print("Start scanning...")
elif scan_state == SCAN_RIGHT:
offset_x = SCAN_STEP * scan_direction
send_to_gimbal(offset_x, 0)
scan_progress += SCAN_STEP
if scan_progress >= HORIZONTAL_LIMIT:
scan_state = SCAN_LEFT
scan_direction *= -1
scan_progress = 0
elif scan_state == SCAN_LEFT:
offset_x = SCAN_STEP * scan_direction
send_to_gimbal(offset_x, 0)
scan_progress += SCAN_STEP
if scan_progress >= HORIZONTAL_LIMIT:
scan_state = SCAN_DOWN
scan_direction = 1
scan_progress = 0
elif scan_state == SCAN_DOWN:
offset_y = SCAN_STEP * scan_direction
send_to_gimbal(0, offset_y)
scan_progress += SCAN_STEP
if scan_progress >= VERTICAL_LIMIT:
scan_state = SCAN_UP
scan_direction *= -1
scan_progress = 0
elif scan_state == SCAN_UP:
offset_y = SCAN_STEP * scan_direction
send_to_gimbal(0, offset_y)
scan_progress += SCAN_STEP
if scan_progress >= VERTICAL_LIMIT:
scan_state = SCAN_RIGHT
scan_direction = 1
scan_progress = 0
while True:
img = sensor.snapshot()
# 自适应图像增强
img.histeq(adaptive=True, clip_limit=3.0)
# 矩形检测
rects = img.find_rects(threshold=RECT_THRESHOLD)
target_found = False
if rects:
# 筛选符合条件的矩形
valid_rects = []
for rect in rects:
area = get_rect_area(rect)
if area > MIN_RECT_AREA and is_a4_paper(rect):
valid_rects.append(rect)
if valid_rects:
# 选择面积最大的矩形
largest_rect = max(valid_rects, key=get_rect_area)
# 计算中心点坐标
center_x, center_y = calculate_center(largest_rect)
# 绘制矩形和中心点
img.draw_rectangle(largest_rect.rect(), color=(255, 0, 0))
img.draw_cross(center_x, center_y, color=(0, 255, 0), size=10)
# 计算与画面中心的偏移量
img_center_x = img.width() // 2
img_center_y = img.height() // 2
offset_x = center_x - img_center_x
offset_y = center_y - img_center_y
# 应用平滑滤波
filtered_x = int(SMOOTHING_FACTOR * offset_x + (1 - SMOOTHING_FACTOR) * prev_offset_x)
filtered_y = int(SMOOTHING_FACTOR * offset_y + (1 - SMOOTHING_FACTOR) * prev_offset_y)
prev_offset_x = filtered_x
prev_offset_y = filtered_y
# 发送控制指令
send_to_gimbal(filtered_x, filtered_y)
target_found = True
rect_lost_count = 0 # 重置丢失计数器
scan_state = SCAN_IDLE # 发现目标后停止扫描
# 目标丢失处理
if not target_found:
rect_lost_count += 1
# 达到丢失阈值后启动扫描
if rect_lost_count >= RECT_LOST_THRESH:
gimbal_scan()
else:
send_to_gimbal(0, 0) # 短暂丢失时保持静止
time.sleep_ms(50) # 控制循环频率约20Hz为这个代码写一个对应的STM32做中转的，能使二维云台追踪矩形中心点的代码，给我一个详细完整的方案及操作步骤和代码
最新发布
博客围绕gsensor direction调试展开，但具体内容缺失，推测是关于gsensor方向调试的相关信息技术内容。
扫一扫
2422
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
