# 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")
![0_1747124658069_屏幕截图 2025-05-13 162350.png](https://fcdn.singtown.com/1d01489f-920b-4c28-9faf-d6a8e708d49c.png)