# OpenMV Object Tracking with UART Communication to STM32 (保持目标检测显示不变)
#
# This work is licensed under the MIT license.
# Copyright (c) 2013-2024 OpenMV LLC. All rights reserved.
import sensor, image, time, ml, math, uos, gc
from pyb import UART
import pyb
# 初始化摄像头
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing((240, 240))
sensor.skip_frames(time=2000)
# 初始化UART (波特率115200)
uart = UART(3, 115200)
uart.init(115200, bits=8, parity=None, stop=1)
# 目标区域设置
TARGET_CENTER_X = 120 # 图像中心X坐标
TARGET_WIDTH = 20 # 目标区域宽度
MAX_SERVO_ANGLE = 45 # 最大舵机角度
# 数据包格式定义
HEADER = 0xAA # 数据包头
FOOTER = 0x55 # 数据包尾
# 加载模型和标签 (保持不变)
net = None
labels = None
min_confidence = 0.5
try:
net = ml.Model("trained.tflite", load_to_fb=uos.stat('trained.tflite')[6] > (gc.mem_free() - (64*1024)))
except Exception as e:
raise Exception('Failed to load model: ' + str(e))
try:
labels = [line.rstrip('\n') for line in open("labels.txt")]
except Exception as e:
raise Exception('Failed to load labels: ' + str(e))
# 颜色定义
colors = [
(255, 0, 0), (0, 255, 0), (255, 255, 0),
(0, 0, 255), (255, 0, 255), (0, 255, 255), (255, 255, 255)
]
threshold_list = [(math.ceil(min_confidence * 255), 255)]
# 后处理函数
def fomo_post_process(model, inputs, outputs):
ob, oh, ow, oc = model.output_shape[0]
x_scale = inputs[0].roi[2] / ow
y_scale = inputs[0].roi[3] / oh
scale = min(x_scale, y_scale)
x_offset = ((inputs[0].roi[2] - (ow * scale)) / 2) + inputs[0].roi[0]
y_offset = ((inputs[0].roi[3] - (ow * scale)) / 2) + inputs[0].roi[1]
l = [[] for i in range(oc)]
for i in range(oc):
img = image.Image(outputs[0][0, :, :, i] * 255)
blobs = img.find_blobs(threshold_list, x_stride=1, y_stride=1, area_threshold=1, pixels_threshold=1)
for b in blobs:
rect = b.rect()
x, y, w, h = rect
score = img.get_statistics(thresholds=threshold_list, roi=rect).l_mean() / 255.0
x = int((x * scale) + x_offset)
y = int((y * scale) + y_offset)
w = int(w * scale)
h = int(h * scale)
l[i].append((x, y, w, h, score))
return l
def send_angle_to_stm32(angle):
"""发送角度数据给STM32 (二进制协议)"""
angle_int = int(angle * 100) # 转换为0.01度单位
data = bytearray([
HEADER,
(angle_int >> 8) & 0xFF, # 角度高字节
angle_int & 0xFF, # 角度低字节
FOOTER
])
uart.write(data)
# print("Sent:AA %02X %02X 55 (Angle=%.2f°)" % (data[1],data[2],angle))
def calculate_servo_angle(target_x):
"""计算舵机角度"""
angle = (target_x - TARGET_CENTER_X) * (MAX_SERVO_ANGLE / TARGET_CENTER_X)
angle = max(-MAX_SERVO_ANGLE, min(MAX_SERVO_ANGLE, angle))
print("Servo angle requireed:{:.2f}".format(angle))
return angle
clock = time.clock()
while True:
clock.tick()
img = sensor.snapshot()
# 绘制目标区域
img.draw_rectangle((TARGET_CENTER_X-TARGET_WIDTH//2, 0, TARGET_WIDTH, img.height()), color=(0, 255, 0))
detected_objects = []
# 目标检测逻辑
for i, detection_list in enumerate(net.predict([img], callback=fomo_post_process)):
if i == 0 or len(detection_list) == 0:
continue
print(f"----- {labels[i]} -----")
for x, y, w, h, score in detection_list:
center_x = math.floor(x + w/2)
center_y = math.floor(y + h/2)
detected_objects.append((center_x, center_y, score))
# 绘制逻辑
img.draw_circle((center_x, center_y, 12), color=colors[i])
img.draw_string(x, y-10, f"{score:.2f°}", color=colors[i])
# 如果有检测到目标，计算平均位置
if detected_objects:
# 加权平均计算
total_weight = sum(score for _, _, score in detected_objects)
avg_x = sum(x*score for x, _, score in detected_objects) / total_weight
# 计算角度并打印
angle = calculate_servo_angle(avg_x)
# 计算角度并发送给STM32
angle = calculate_servo_angle(avg_x)
send_angle_to_stm32(angle)
# # 模拟目标位置（中心右侧30像素）
# test_x = TARGET_CENTER_X + 30
# test_angle = (test_x - TARGET_CENTER_X) * (MAX_SERVO_ANGLE / TARGET_CENTER_X)
# # 发送虚拟数据
# send_angle_to_stm32(test_angle)
# # 保持可视化（可选）
# img.draw_cross(int(test_x), 120, color=(255,0,0), size=20)
# img.draw_string(10, 10, "调试模式: Angle=%.1f°" % test_angle)
# time.sleep_ms(100) # 控制输出频率
img.draw_cross(int(avg_x), color=(255, 0, 0), size=20)
img.draw_string(10, 10, f"Target: {int(avg_x)}", color=(255, 255, 255))
img.draw_string(10, 25, f"Angle: {angle:.1f}°", color=(255, 255, 255))
print(f"FPS: {clock.fps():.1f}\n")
