v1.4实现机器人一起踢乒乓球功能，使用maixcam，离线运行。

2026-01-01 20:56:41 +08:00
1 parent 650797fe28
commit 9d3aa2684b
@@ -63,3 +63,5 @@ servo 文件夹是用于写入舵机初始化程序
 2025-11-23 v1.3增加一个简单的越障模式，和斜坡自平衡功能.

 2025-12-26 v1.4增加建议云台人脸跟踪效果
+
+2026-01-01 v1.4实现机器人一起踢乒乓球功能，使用maixcam，离线运行。
@@ -0,0 +1,48 @@
+# ------------------------------
+# LED配置
+# ------------------------------
+
+from maix import camera, display, image, nn, app, sys
+from maix import uart, pinmap, time, err, gpio
+import traceback
+
+LED_PIN_NAME = "A14"
+LED_GPIO_NAME = "GPIOA14"
+BLINK_INTERVAL = 500  # LED闪烁间隔(ms)
+
+
+# ------------------------------
+# 全局业务状态变量
+# ------------------------------
+led = None                       # LED实例
+serial_dev = None                # 舵机串口全局变量
+previous_millis = 0              # LED闪烁计时
+
+# ------------------------------
+# LED控制
+# ------------------------------
+def init_led():
+    """初始化LED引脚"""
+    global led
+    try:
+        err.check_raise(
+            pinmap.set_pin_function(LED_PIN_NAME, LED_GPIO_NAME),
+            f"设置LED引脚{LED_PIN_NAME}功能失败"
+        )
+        led = gpio.GPIO(LED_GPIO_NAME, gpio.Mode.OUT)
+        led.value(0)  # 初始熄灭
+        print(f"LED初始化完成：引脚{LED_PIN_NAME}（{LED_GPIO_NAME}）")
+    except Exception as e:
+        print(f"LED初始化失败: {e}")
+        led = None
+
+def update_led():
+    """更新LED状态：改为固定闪烁模式"""
+    global previous_millis
+    if led is None:
+        return
+    
+    current_millis = time.time_ms()
+    if current_millis - previous_millis >= BLINK_INTERVAL:
+        previous_millis = current_millis
+        led.toggle()
@@ -1,288 +1,19 @@
-'''
- 主题：MAIXCAM 云台
- 作者：Tommy
- 日期：2025年-12月-21日
-'''
+# main.py - 主业务逻辑（人脸跟踪、舵机控制、LED）
 from maix import camera, display, image, nn, app, sys
-from maix import uart, pinmap, time, err, gpio, network
-import paho.mqtt.client as mqtt
-import asyncio
-import gc
+from maix import uart, pinmap, time, err, gpio
+from maix import app, nn
 import traceback
-
-
-DEVICE_ID = "XXX"
-
-# 两套WiFi/MQTT配置
-WIFI_CONFIGS = [
-    {
-        "SSID": "X",
-        "PASSWORD": "Y",
-        "MQTT_BROKER": "Z", 
-    },
-]
-
-MQTT_PORT = 1883
-
-# 发送的主题
-MQTT_PUB_TOPIC_IMAGE = "hangtianyuan/head_video"  # 图像发布主题
-
-# 订阅的主题
-MQTT_SUB_TOPIC_TEXT = "hangtianyuan/text"         # 文本指令订阅主题
-MQTT_SUB_TOPIC_MOVE = "hangtianyuan/set_move"     # 舵机移动指令订阅主题
-# 批量订阅
-MQTT_SUB_TOPICS = [
-    (MQTT_SUB_TOPIC_TEXT, 0),
-    (MQTT_SUB_TOPIC_MOVE, 0),
-]
-
-# LED配置
-LED_PIN_NAME = "A14"
-LED_GPIO_NAME = "GPIOA14"
-BLINK_INTERVAL = 500  # LED闪烁间隔(ms)
-
-# 人脸追踪参数
-kp_x  = 0.05 # 水平方向 0.04
-kp_y = 0.07 # 垂直方向
-angle_threshold = 1
-s1_range = [0, 180]
-s2_range = [0, 180]
-no_face_threshold = 30
-
-# 滤波缓存
-last_dev_x = 0.0
-last_dev_y = 0.0
-filter_alpha = 0.85
-
-# ------------------------------
-# 全局状态变量（新增track_mode控制跟踪开关）
-# ------------------------------
-mqtt_connected = False          
-wifi_connected = False          
-current_wifi_idx = 0            
-led = None                      
-mqtt_client = None              
-previous_millis = 0             
-send_frame_interval = 8         
-frame_count = 0                 
-track_mode = 1  # 0:关闭追踪，1:人脸跟踪模式
-serial_dev = None  # 新增：舵机串口全局变量
-
-# ------------------------------
-# 工具函数
-# ------------------------------
-def constrain(val, min_val, max_val):
-    return min(max_val, max(min_val, val))
-
-# ------------------------------
-# LED控制
-# ------------------------------
-def init_led():
-    """初始化LED引脚"""
-    global led
-    try:
-        err.check_raise(
-            pinmap.set_pin_function(LED_PIN_NAME, LED_GPIO_NAME),
-            f"设置LED引脚{LED_PIN_NAME}功能失败"
-        )
-        led = gpio.GPIO(LED_GPIO_NAME, gpio.Mode.OUT)
-        led.value(0)  # 初始熄灭
-        print(f"LED初始化完成：引脚{LED_PIN_NAME}（{LED_GPIO_NAME}）")
-    except Exception as e:
-        print(f"LED初始化失败: {e}")
-        led = None
-
-def update_led():
-    """更新LED状态：MQTT未连接闪烁，连接成功常亮"""
-    global previous_millis, mqtt_connected
-    if led is None:
-        return
-    
-    current_millis = time.time_ms()
-    if not mqtt_connected:
-        if current_millis - previous_millis >= BLINK_INTERVAL:
-            previous_millis = current_millis
-            led.toggle()
-    else:
-        if led.value() != 1:
-            led.value(1)
-
-# ------------------------------
-# WiFi连接（异步）
-# ------------------------------
-async def connect_wifi_async():
-    """异步尝试连接WiFi（自动切换两套配置）"""
-    global wifi_connected, current_wifi_idx
-    w = network.wifi.Wifi()
-    
-    wifi_connected = False
-    current_wifi_idx = 0
-    
-    while current_wifi_idx < len(WIFI_CONFIGS) and not wifi_connected:
-        config = WIFI_CONFIGS[current_wifi_idx]
-        ssid = config["SSID"]
-        password = config["PASSWORD"]
-        
-        print(f"[WiFi] 尝试连接 {ssid} (第{current_wifi_idx+1}套)")
-        try:
-            e = w.connect(ssid, password, wait=True, timeout=10)
-            err.check_raise(e, f"WiFi连接失败")
-            wifi_connected = True
-            ip = w.get_ip()
-            print(f"[WiFi] 连接成功！IP: {ip}")
-        except Exception as e:
-            print(f"[WiFi] {ssid} 连接失败: {e}")
-            current_wifi_idx += 1
-            await asyncio.sleep(1)
-    
-    if not wifi_connected:
-        print("[WiFi] 所有配置连接失败，无联网状态")
-
-# ------------------------------
-# MQTT回调函数（新增track_mode控制逻辑）
-# ------------------------------
-def on_mqtt_connect(client, userdata, flags, rc):
-    """MQTT连接回调"""
-    global mqtt_connected
-    if rc == 0:
-        mqtt_connected = True
-        print("[MQTT] 连接成功")
-        # 订阅主题（如有）
-        if MQTT_SUB_TOPICS:
-            client.subscribe(MQTT_SUB_TOPICS)
-            print(f"[MQTT] 已订阅主题: {MQTT_SUB_TOPICS}")
-    else:
-        mqtt_connected = False
-        print(f"[MQTT] 连接失败，错误码: {rc}")
-
-def on_mqtt_message(client, userdata, msg):
-    global track_mode, serial_dev  
-    if msg.topic == MQTT_SUB_TOPIC_TEXT:
-        payload = msg.payload.decode().strip()  # 去除首尾空格，提高容错
-        print(f"文本指令: {payload}")
-        # 解析跟踪模式指令
-        if payload.lower() == "track_off":
-            track_mode = 0
-            print("[跟踪模式] 已关闭人脸跟踪，停止绘制人脸框和追踪")
-        elif payload.lower() == "track_face":
-            track_mode = 1
-            print("[跟踪模式] 已开启人脸跟踪，开始绘制人脸框和追踪")
-        elif payload.lower() == "track_hand":
-            track_mode = 2
-            print("[跟踪模式] 已开启手部跟踪，开始绘制手部框和追踪")
-    elif msg.topic == MQTT_SUB_TOPIC_MOVE:
-        payload = msg.payload.decode().strip()
-        # 修复：判空保护 + 使用全局serial_dev
-        if serial_dev is not None:
-            try:
-                serial_dev.write_str(payload)
-                print(f"运动指令已发送: {payload}")
-            except Exception as e:
-                print(f"发送运动指令失败: {e} | 指令内容: {payload}")
-        else:
-            print(f"运动指令: {payload}，但串口未初始化")
-
-def on_mqtt_disconnect(client, userdata, rc):
-    """MQTT断开回调"""
-    global mqtt_connected
-    mqtt_connected = False
-    if rc != 0:
-        print(f"[MQTT] 意外断开，错误码: {rc}")
-
-# ------------------------------
-# MQTT初始化（异步）
-# ------------------------------
-async def init_mqtt_async():
-    """异步初始化MQTT（依赖WiFi，自动重试）"""
-    global mqtt_client, mqtt_connected, current_wifi_idx
-    mqtt_connected = False
-    mqtt_client = None
-    
-    # 等待WiFi连接（最多20秒）
-    wait_time = 0
-    while not app.need_exit() and not wifi_connected and wait_time < 20:
-        await asyncio.sleep(1)
-        wait_time += 1
-    
-    if app.need_exit() or not wifi_connected:
-        print("[MQTT] WiFi未连接，跳过MQTT初始化")
-        return
-    
-    # 兜底索引
-    if current_wifi_idx >= len(WIFI_CONFIGS):
-        current_wifi_idx = 0
-    current_broker = WIFI_CONFIGS[current_wifi_idx]["MQTT_BROKER"]
-    
-    try:
-        mqtt_client = mqtt.Client(DEVICE_ID)
-        mqtt_client.on_connect = on_mqtt_connect
-        mqtt_client.on_message = on_mqtt_message
-        mqtt_client.on_disconnect = on_mqtt_disconnect
-        
-        # 重试连接
-        retry_count = 0
-        while not app.need_exit() and not mqtt_connected and retry_count < 5:
-            try:
-                print(f"[MQTT] 尝试连接 {current_broker}:{MQTT_PORT} (重试{retry_count+1}/5)")
-                mqtt_client.connect(current_broker, MQTT_PORT, 60)
-                mqtt_client.loop_start()
-                
-                # 等待连接结果
-                t = time.time()
-                while not mqtt_connected and time.time() - t < 5:
-                    await asyncio.sleep(0.1)
-                
-                if mqtt_connected:
-                    break
-                else:
-                    mqtt_client.disconnect()
-                    retry_count += 1
-                    await asyncio.sleep(3)
-            except Exception as e:
-                print(f"[MQTT] 连接异常: {e}")
-                retry_count += 1
-                await asyncio.sleep(3)
-        
-        if not mqtt_connected:
-            print("[MQTT] 多次重试后仍无法连接")
-    except Exception as e:
-        print(f"[MQTT] 初始化失败: {e}")
-        mqtt_client = None
-        mqtt_connected = False
-
-# ------------------------------
-# 图像MQTT发送
-# ------------------------------
-def send_image_via_mqtt(img):
-    """发送图像到MQTT（JPEG压缩）"""
-    global mqtt_connected, mqtt_client
-    if not mqtt_connected or mqtt_client is None:
-        return False
-    
-    try:
-        jpeg_data = img.to_jpeg(quality=85).to_bytes()
-        # 二进制发送（高效）
-        mqtt_client.publish(MQTT_PUB_TOPIC_IMAGE, jpeg_data)
-        gc.collect()
-        return True
-    except Exception as e:
-        print(f"[MQTT] 图像发送失败: {e}")
-        try:
-            if mqtt_client:
-                mqtt_client.reconnect()
-        except:
-            mqtt_connected = False
-        return False
+from config import *
+from test_face import *
+from test_ball import *

 # ------------------------------
 # 系统初始化
 # ------------------------------
 def init_system():
-
    """初始化相机、舵机、串口、LED"""
    global serial_dev  # 声明使用全局serial_dev

-    """初始化相机、舵机、串口、LED"""
    init_led()
    
    # 人脸检测器
@@ -304,167 +35,45 @@ def init_system():
                      f"Failed set pin{pin} function to {func}")
    serial_dev = uart.UART(device, 115200)
    
-    # 舵机初始角度
-    current_s1, current_s2 = 90, 90
-    initial_cmd = f"S1:{current_s1},S2:{current_s2};"
+    # 初始化值 距离中间点像素距离
+    initial_cmd = f"DX:0,DY:0;"
    serial_dev.write_str(initial_cmd)
    print(f"[舵机] 初始角度: {initial_cmd}")
    time.sleep(1)
    
    return {
        "detector": detector, "cam": cam, "disp": disp, "serial_dev": serial_dev,
-        "img_width": img_width, "img_height": img_height, "image_center": image_center,
-        "current_s1": current_s1, "current_s2": current_s2
+        "img_width": img_width, "img_height": img_height, "image_center": image_center
    }

-# ------------------------------
-# 人脸检测处理（新增track_mode判断，关闭时不绘制人脸框）
-# ------------------------------
-def process_faces(img, objs):
+def callback(data: list[float], len: int):
+
    global track_mode
-    # 跟踪模式关闭时，直接返回无脸坐标，不绘制任何框
-    if not track_mode :
-        return (-1, -1, -1)

-    max_face = None
-    max_width = 0
+    # 2. 找到概率最大的关键词索引
+    max_prob = max(data)  # 获取最大概率值
+    max_index = data.index(max_prob)  # 获取最大概率对应的kw索引

-    if objs:
-        for obj in objs:
-            if obj.w > max_width:
-                max_width = obj.w
-                max_face = obj
-        
-        # 绘制人脸框（仅跟踪模式开启时执行）
-        for obj in objs:
-            color = image.COLOR_GREEN if obj == max_face else image.COLOR_RED
-            img.draw_rect(obj.x, obj.y, obj.w, obj.h, color=color, thickness=2)
-    
-    if max_face:
-        center_x = int(max_face.x + max_face.w / 2)
-        center_y = int(max_face.y + max_face.h / 2)
-        img.draw_keypoints([center_x, center_y], image.COLOR_GREEN, size=3)
-        return (center_x, center_y, int(max_face.w))
-    else:
-        return (-1, -1, -1)
+    # 3. 判断：仅当最大概率>0时，输出对应value
+    if max_prob > 0:
+        # 根据索引映射到kw_list的value（kw0→0，kw1→1，kw2→2）
+        output_value = [0, 1, 2][max_index]
+        print(f"识别结果：{output_value}")  # 输出最终的value
+        track_mode = output_value 

 # ------------------------------
-# 人脸追踪
+# 系统执行
 # ------------------------------
-def track_face(center, image_center, current_angles, serial_dev):
-    global last_dev_x, last_dev_y
-    center_x, center_y = center
-    current_s1, current_s2 = current_angles

-    # 偏差计算+滤波
-    deviation_x = center_x - image_center[0]
-    deviation_y = center_y - image_center[1]
-    smoothed_x = filter_alpha * last_dev_x + (1 - filter_alpha) * deviation_x
-    smoothed_y = filter_alpha * last_dev_y + (1 - filter_alpha) * deviation_y
-    last_dev_x, last_dev_y = smoothed_x, smoothed_y
-    
-    # 角度计算+限制
-    # 金属舵机
-    # new_s1 = constrain(int(current_s1 + smoothed_x * kp_x), s1_range[0], s1_range[1])
-    # new_s2 = constrain(int(current_s2 + smoothed_y * kp_y), s2_range[0], s2_range[1])
-    # 塑料舵机
-    new_s1 = constrain(int(current_s1 - smoothed_x * kp_x), s1_range[0], s1_range[1])
-    new_s2 = constrain(int(current_s2 - smoothed_y * kp_y), s2_range[0], s2_range[1])
-
-    # 发送舵机指令
-    if abs(new_s1 - current_s1) > angle_threshold or abs(new_s2 - current_s2) > angle_threshold:
-        cmd = f"S1:{new_s1},S2:{new_s2};"
-        serial_dev.write_str(cmd)
-        return (new_s1, new_s2, cmd)
-    else:
-        return (current_s1, current_s2, None)
-
-# ------------------------------
-# 主循环（新增track_mode判断和状态显示）
-# ------------------------------
-async def main_loop_async(resources):
-    """异步主循环"""
-    detector = resources["detector"]
-    cam = resources["cam"]
-    disp = resources["disp"]
-    serial_dev = resources["serial_dev"]
-    img_width = resources["img_width"]
-    img_height = resources["img_height"]
-    image_center = resources["image_center"]
-    current_s1, current_s2 = resources["current_s1"], resources["current_s2"]
-    
-    # 启动网络异步任务
-    asyncio.create_task(connect_wifi_async())
-    asyncio.create_task(init_mqtt_async())
-    
-    # 主循环
-    global frame_count, mqtt_connected, wifi_connected, track_mode
-    while not app.need_exit():
-        # 图像读取+人脸检测
-        img = cam.read().flip(image.FlipDir.Y)
-        objs = detector.detect(img, conf_th=0.4, iou_th=0.45)
-        center_x, center_y, face_width = process_faces(img, objs)
-        
-        # 绘制基础状态
-        img.draw_string(20, 20, f"Center: ({center_x}, {center_y})", image.COLOR_GREEN, 1.0)
-        img.draw_string(20, 40, f"Max Width: {face_width}", image.COLOR_YELLOW, 1.0)
-        
-        # 绘制跟踪模式状态（直观显示当前模式）
-        track_status = "ON" if track_mode  else "OFF"
-        track_color = image.COLOR_GREEN if track_mode else image.COLOR_RED
-        img.draw_string(20, 100, f"Track Mode: {track_status}", track_color, 1.0)
-        
-        # 人脸追踪（仅跟踪模式开启时执行）
-        if track_mode and center_x != -1 and center_y != -1:
-            new_s1, new_s2, cmd = track_face((center_x, center_y), image_center, (current_s1, current_s2), serial_dev)
-            current_s1, current_s2 = new_s1, new_s2
-            if cmd:
-                img.draw_string(20, 60, f"Tracking: {cmd}", image.COLOR_BLUE, 1.0)
-        elif track_mode and center_x == -1:
-            # 跟踪模式开启但无脸
-            img.draw_string(20, 60, "No face detected", image.COLOR_RED, 1.0)
-        else:
-            # 跟踪模式关闭
-            img.draw_string(20, 60, "Track Mode: OFF", image.COLOR_ORANGE, 1.0)
-        
-        # 网络状态显示
-        net_status = f"WiFi: {'OK' if wifi_connected else 'NO'} | MQTT: {'OK' if mqtt_connected else 'NO'}"
-        img.draw_string(20, 80, net_status, image.COLOR_YELLOW, 1.0)
-        
-        # LED更新+图像发送
-        update_led()
-        frame_count += 1
-        if frame_count >= send_frame_interval:
-            send_image_via_mqtt(img)
-            frame_count = 0
-        
-        disp.show(img)
-        await asyncio.sleep(0.001)
-
-# ------------------------------
-# 程序入口
-# ------------------------------
-if __name__ == "__main__":
 try:
+    # 初始化系统资源
    system_resources = init_system()
-        asyncio.run(main_loop_async(system_resources))
+    # 进入主循环
+    # trace_face_main(system_resources)
+    trace_ball_main(system_resources)
+
 except Exception as e:
    print(f"[异常] 程序异常: {e}")
    traceback.print_exc()
-    finally:
-        # 清理MQTT
-        if mqtt_client:
-            try:
-                mqtt_client.loop_stop()
-                mqtt_client.disconnect()
-                print("[MQTT] 客户端已断开")
-            except Exception as e:
-                print(f"[MQTT] 断开失败: {e}")
-        # 清理LED
-        if led:
-            try:
-                led.value(0)
-                print("[LED] 已关闭")
-            except Exception as e:
-                print(f"[LED] 关闭失败: {e}")
-        print("[系统] 程序已退出")
+
+
@@ -0,0 +1,134 @@
+from maix import image, app, time
+
+# 橘黄色乒乓球的颜色阈值配置（HSV色域）
+thresholds = [
+    [73, 93, 7, 27, 13, 33],
+    [40, 65, 30, 60, 40, 70],
+    [39, 59, 41, 61, 48, 68],
+]
+
+# 新增：丢失目标的阈值配置（可根据实际帧率调整，建议5-10帧）
+LOST_FRAME_THRESHOLD = 10  # 连续x帧没识别到才判定为真正丢失
+SEND_INTERVAL = 5         # 控制指令发送间隔（避免高频发送，单位：ms）
+
+def select_largest_blob(blobs):
+    """
+    筛选并选取最大的有效blob（替换原融合逻辑）
+    1. 筛除宽/高小于10px的小blob（过滤噪声）
+    2. 从剩余blob中选取面积最大的单个blob
+    :param blobs: 识别到的所有blob列表
+    :return: 最大blob的矩形参数 (x, y, width, height) 和中心点 (cx, cy)；未识别到则返回None, None
+    """
+    if not blobs:
+        return None, (-1,-1)
+    
+    # 步骤1：过滤掉宽/高小于10px的无效blob
+    valid_blobs = []
+    for b in blobs:
+        x, y, w, h = b[0], b[1], b[2], b[3]
+        if w >= 10 and h >= 10:  # 筛除边距（宽/高）小于10px的blob
+            valid_blobs.append(b)
+    
+    # 过滤后无有效blob，返回None
+    if not valid_blobs:
+        return None, (-1,-1)
+    
+    # 步骤2：选取面积（w*h）最大的blob
+    largest_blob = max(valid_blobs, key=lambda b: b[2] * b[3])
+    
+    # 提取最大blob的矩形参数和中心点
+    x, y, w, h = largest_blob[0], largest_blob[1], largest_blob[2], largest_blob[3]
+    cx = x + w // 2
+    cy = y + h // 2
+    
+    return (x, y, w, h), (cx, cy)
+
+def calculate_deviation_ball(center, image_center):
+    """
+    优化：仅计算偏移量，不直接发送指令（将指令发送逻辑移到主循环统一处理）
+    :return: (deviation_x, deviation_y) 或 None（无有效目标）
+    """
+    center_x, center_y = center
+
+    if center_x == -1 and center_y == -1 :
+        return None  # 无有效目标
+    
+    # 计算x/y方向的偏移偏差（目标中心 - 图像中心）
+    deviation_x = center_x - image_center[0]
+    deviation_y = center_y - image_center[1]
+    
+    # 仅当偏移超过阈值时返回有效偏移，否则返回None（避免微小抖动）
+    if abs(deviation_x) > 10 or abs(deviation_y) > 10 :
+        return (deviation_x, deviation_y)
+    else:
+        return None
+
+def trace_ball_main(resources):
+    detector = resources["detector"]
+    cam = resources["cam"]
+    disp = resources["disp"]
+    serial_dev = resources["serial_dev"]
+    img_width = resources["img_width"]
+    img_height = resources["img_height"]
+    image_center = resources["image_center"]
+
+    # 新增：状态变量初始化
+    last_valid_deviation = None  # 记录最后一次有效偏移量
+    lost_frames = 0              # 丢失帧数计数
+    last_send_time = 0           # 最后一次发送指令的时间（防高频）
+
+    while not app.need_exit():
+        current_time = time.ticks_ms()
+        img = cam.read().flip(image.FlipDir.Y)
+        
+        # 识别色块（像素阈值设为10，过滤小噪声）
+        blobs = img.find_blobs(thresholds, pixels_threshold=10)
+        
+        # 筛选并选取最大的有效blob
+        largest_rect, target_center = select_largest_blob(blobs)
+        
+        # 计算当前帧的偏移量
+        current_deviation = calculate_deviation_ball(target_center, image_center)
+
+        # -------------------------- 核心优化逻辑 --------------------------
+        if current_deviation is not None:
+            # 1. 识别到有效目标：重置丢失计数，更新最后一次有效偏移
+            lost_frames = 0
+            last_valid_deviation = current_deviation
+            # 控制发送频率，避免串口堵塞
+            if current_time - last_send_time >= SEND_INTERVAL:
+                dx, dy = current_deviation
+                cmd = f"DX:{dx},DY:{dy};"
+                serial_dev.write_str(cmd)
+                print(f"识别到目标: {cmd}")
+                last_send_time = current_time
+        else:
+            # 2. 未识别到目标：累计丢失帧数
+            lost_frames += 1
+            if lost_frames < LOST_FRAME_THRESHOLD and last_valid_deviation is not None:
+                # 2.1 丢失帧数未达阈值：继续发送最后一次有效偏移
+                if current_time - last_send_time >= SEND_INTERVAL:
+                    dx, dy = last_valid_deviation
+                    cmd = f"DX:{dx},DY:{dy};"
+                    serial_dev.write_str(cmd)
+                    print(f"短暂丢失，沿用最后偏移: {cmd} (丢失帧数: {lost_frames})")
+                    last_send_time = current_time
+            else:
+                # 2.2 丢失帧数达阈值：发送9999，重置最后有效偏移
+                if current_time - last_send_time >= SEND_INTERVAL:
+                    cmd = f"DX:9999,DY:9999;"
+                    serial_dev.write_str(cmd)
+                    print(f"目标丢失超过阈值: {cmd} (丢失帧数: {lost_frames})")
+                    last_valid_deviation = None
+                    last_send_time = current_time
+        # -----------------------------------------------------------------
+
+        # 绘制识别结果
+        if largest_rect:
+            x, y, w, h = largest_rect
+            img.draw_rect(x, y, w, h, image.COLOR_GREEN, 2)
+            cx, cy = target_center
+            img.draw_circle(cx, cy, 3, image.COLOR_RED, -1)
+        
+        disp.show(img)
+        time.sleep_ms(1)
@@ -0,0 +1,88 @@
+# ------------------------------
+# 人脸检测处理
+# ------------------------------
+from maix import image, app, time
+from config import *
+
+def process_faces(img, objs):
+    
+    max_face = None
+    max_width = 0
+    
+    if objs:
+        for obj in objs:
+            if obj.w > max_width:
+                max_width = obj.w
+                max_face = obj
+        
+        # 绘制人脸框（仅跟踪模式开启时执行）
+        for obj in objs:
+            color = image.COLOR_GREEN if obj == max_face else image.COLOR_RED
+            img.draw_rect(obj.x, obj.y, obj.w, obj.h, color=color, thickness=2)
+    
+    if max_face:
+        center_x = int(max_face.x + max_face.w / 2)
+        center_y = int(max_face.y + max_face.h / 2)
+        img.draw_keypoints([center_x, center_y], image.COLOR_GREEN, size=3)
+        return (center_x, center_y, int(max_face.w))
+    else:
+        return (-1, -1, -1)
+
+# ------------------------------
+# 人脸追踪
+# ------------------------------
+def calculate_deviation_face(center, image_center, serial_dev):
+
+    center_x, center_y = center
+    
+    # 偏差计算
+    deviation_x = center_x - image_center[0]
+    deviation_y = center_y - image_center[1]
+    
+    # 输出像素偏差
+    if abs(deviation_x) > 10 or abs(deviation_y) > 10 :
+        cmd = f"DX:{deviation_x},DY:{deviation_y};"  # 图像尺寸: w320xh224
+        serial_dev.write_str(cmd)
+        print(cmd)
+        return (cmd)
+    else:
+        return (None)
+
+# ------------------------------
+# 主循环（同步）
+# ------------------------------
+def trace_face_main(resources):
+
+    detector = resources["detector"]
+    cam = resources["cam"]
+    disp = resources["disp"]
+    serial_dev = resources["serial_dev"]
+    img_width = resources["img_width"]
+    img_height = resources["img_height"]
+    image_center = resources["image_center"]
+    
+    while not app.need_exit():
+
+        img = cam.read().flip(image.FlipDir.Y)
+        objs = detector.detect(img, conf_th=0.4, iou_th=0.45)
+        center_x, center_y, face_width = process_faces(img, objs)
+
+        # 绘制基础状态
+        img.draw_string(20, 20, f"Center: ({center_x}, {center_y})", image.COLOR_GREEN, 1.0)
+        img.draw_string(20, 40, f"Max Width: {face_width}", image.COLOR_YELLOW, 1.0)
+        
+        # 人脸追踪（仅跟踪模式开启时执行）
+        if  center_x != -1 and center_y != -1:
+            cmd = calculate_deviation_face((center_x, center_y), image_center,  serial_dev)
+            if cmd:
+                img.draw_string(20, 60, f"Tracking: {cmd}", image.COLOR_BLUE, 1.0)
+        elif  center_x == -1:
+            cmd = f"DX:9999,DY:9999;"  # 未找到
+            serial_dev.write_str(cmd)
+            print(cmd)
+            img.draw_string(20, 60, "No face detected", image.COLOR_RED, 1.0)
+        else:
+            img.draw_string(20, 60, "Track Mode: OFF", image.COLOR_ORANGE, 1.0)
+        
+        disp.show(img)
+        time.sleep_ms(1)
@@ -1,274 +0,0 @@
-<!DOCTYPE html>
-<html>
-<head>
-    <meta charset="utf-8">
-    <meta name="viewport" content="width=device-width, initial-scale=1.0">
-    <title>Wrobot</title>
-    <link rel="stylesheet" href="{{url_for('static', filename='bootstrap/dist/css/bootstrap.min.css')}}">
-    <link rel="stylesheet" href="{{url_for('static', filename='css/main.min.css')}}">
-    <link rel="stylesheet" href="{{url_for('static', filename='font-awesome-4.7.0/css/font-awesome.min.css')}}">
-    <script src="{{url_for('static', filename='js/jquery-3.6.0.min.js')}}"></script>
-    <script src="{{url_for('static', filename='bootstrap/dist/js/bootstrap.bundle.min.js')}}"></script>
-    <style>
-        #nipple-log{height:auto;min-height:50px;max-height:200px;overflow-y:auto;border:1px solid #ddd;padding:5px}
-        .joystick-container{width:300px;height:300px;margin:0 auto;border:2px solid #ddd;border-radius:2px;position:relative;background-color:#f8f9fa;cursor:grab;touch-action:none;user-select:none}
-        .joystick-container:active{cursor:grabbing}
-        .joystick-container::before{content:'';position:absolute;left:10%;top:50%;width:80%;height:1px;background-color:#ddd;transform:translateY(-50%);z-index:1}
-        .joystick-container::after{content:'';position:absolute;left:50%;top:10%;width:1px;height:80%;background-color:#ddd;transform:translateX(-50%);z-index:1}
-        .joystick-btn{width:50px;height:50px;background-color:#0d6efd;border-radius:50%;position:absolute;left:50%;top:50%;transform:translate(-50%,-50%);box-shadow:0 2px 8px rgba(0,0,0,.3);transition:background-color .2s;will-change:transform;z-index:2}
-        .joystick-btn:hover{background-color:#0b5ed7}
-        .joystick-btn.dragging{background-color:#0a58ca;box-shadow:0 4px 12px rgba(0,0,0,.4)}
-        .angle-mark{position:absolute;color:#666;font-size:12px;font-weight:500;z-index:1}
-        .mark-x-0{left:5px;top:50%;transform:translateY(-50%)}
-        .mark-x-180{right:5px;top:50%;transform:translateY(-50%)}
-        .mark-y-0{top:5px;left:50%;transform:translateX(-50%)}
-        .mark-y-180{bottom:5px;left:50%;transform:translateX(-50%)}
-        .angle-display{text-align:center;margin-top:15px;font-size:16px;color:#333;font-weight:500}
-        .angle-item{margin:5px 0}
-        #head_video{background-color:#f1f1f1;transition:opacity .1s ease}
-    </style>
-</head>
-<body>
-    <div class="container h-100">
-        <!-- 设备&控制栏 -->
-        <div class="row gx-3 my-3">
-            <div class="col-12 d-flex align-items-center gap-3"> 
-                <span class="text-secondary d-flex">设备ID:</span>
-                <select class="form-select form-select-sm h3 blue1 w-auto " id="device_id">
-                    {% for k in device_list %}
-                        <option value="{{k}}" {{ 'selected' if k == device_id else '' }}>{{k}}</option>
-                    {% endfor %}    
-                </select>
-
-                <span class="text-secondary d-flex">跟踪:</span>
-                <select class="form-select form-select-sm h3 blue1 w-auto" id="follow_flag">
-                    {% for k,v in follow_list.items() %}
-                        <option value="{{v}}" {{ 'selected' if k == follow_id else '' }}>{{k}}</option>
-                    {% endfor %}
-                </select>
-
-                <button id="reset_btn" class="btn btn-outline-primary btn-sm">还原姿态</button>
-            </div>
-        </div>
-
-        <!-- 摄像头+摇杆布局 -->
-        <div class="row gx-3 justify-content-center align-items-center">
-            <div class="col-md-6">
-                <!-- 核心优化：直接指向实时流地址，浏览器自动刷新 -->
-                <img class="img-fluid" id="head_video" src="/wrobot_cam/{{device_id}}" alt="摄像头视频" style="width:100%;height:auto;">
-            </div>
-            <div class="col-md-3">
-                <div class="joystick-container" id="joystickContainer">
-                    <span class="angle-mark mark-x-0">180°</span>
-                    <span class="angle-mark mark-x-180">0°</span>
-                    <span class="angle-mark mark-y-0">180°</span>
-                    <span class="angle-mark mark-y-180">0°</span>
-                    <div class="joystick-btn" id="joystickBtn"></div>
-                </div>
-                <!-- 舵机角度显示 -->
-                <div class="angle-display">
-                    <div class="angle-item">S1舵机（X轴）：<span id="s1Angle">90</span>°</div>
-                    <div class="angle-item">S2舵机（Y轴）：<span id="s2Angle">90</span>°</div>
-                </div>
-            </div>
-        </div>
-
-        <!-- 日志框 -->
-        <div class="d-flex gap-2 my-3">
-            <div class="small w-100" id="nipple-log"></div>
-        </div>
-    </div>
-
-<script>
-    // ===================== 全局配置 =====================
-    const AJAX_SEND_INTERVAL = 100;  // AJAX节流间隔(ms)
-    const angleUpdateInterval = 500; // 角度显示节流间隔(ms)
-    let isDragging = false;
-    let lastSendTime = 0;
-    let lastAngleUpdateTime = 0;
-    let hasSetTrackOff = false; // 新增：标记是否已设置跟踪为关闭，避免重复触发
-
-    // ===================== DOM元素缓存 =====================
-    const $deviceId = $('#device_id');
-    const $followFlag = $('#follow_flag');
-    const $resetBtn = $('#reset_btn');
-    const $headVideo = $('#head_video');
-    const $joystickContainer = $('#joystickContainer');
-    const $joystickBtn = $('#joystickBtn');
-    const $s1Angle = $('#s1Angle');
-    const $s2Angle = $('#s2Angle');
-    const $nippleLog = $('#nipple-log');
-
-    // 摇杆按钮尺寸缓存
-    const btnHalfWidth = $joystickBtn.width() / 2;
-    const btnHalfHeight = $joystickBtn.height() / 2;
-
-    // ===================== AJAX核心函数（所有交互） =====================
-    function sendActionTxt(device_id, action_text) {
-        const now = Date.now();
-        if (now - lastSendTime < AJAX_SEND_INTERVAL) return false;
-        lastSendTime = now;
-        $.post("/wrobot/api/SendActionTxt", {
-            device_id: device_id,
-            action_text: action_text,
-            ignoreMe: new Date().getTime()
-        }, function (data, textstatus) {
-            if (data['ret'] != 0) {
-                return false;
-            }
-            addLogToNippleLog(data);
-        }) 
-    }
-
-    function sendVoiceTxt(device_id, voice_text) {
-        $.post("/wrobot/api/SendVoiceTxt", {
-            device_id: device_id,
-            voice_text: voice_text,
-            ignoreMe: new Date().getTime()
-        }, function (data, textstatus) {
-            if (data['ret'] != 0) {
-                return false;
-            }
-            addLogToNippleLog(data);
-        }) 
-
-    }
-
-    function setFollowFlag(device_id, follow_flag) {
-        $.post("/wrobot/api/SetFollowFlag", {
-            device_id: device_id,
-            follow_flag: follow_flag,
-            ignoreMe: new Date().getTime()
-        }, function (data, textstatus) {
-            if (data['ret'] != 0) {
-                return false;
-            }
-            addLogToNippleLog(data);
-        })    
-    }
-
-    // ===================== 摇杆控制逻辑 =====================
-    function resetJoystick() {
-        // 复位摇杆位置
-        $joystickBtn.removeClass('dragging').css('transform', 'translate(-50%, -50%)');
-        const resetAngle = 90;
-        $s1Angle.text(resetAngle);
-        $s2Angle.text(resetAngle);
-        
-        // 发送复位指令
-        const deviceId = $deviceId.val();
-        sendActionTxt(deviceId, `S1:${resetAngle},S2:${resetAngle};`);
-        setFollowFlag(deviceId, 'none');
-        
-        addLogToNippleLog({ type: 'info', msg: '摇杆复位，舵机角度重置为90°' });
-    }
-
-    // 摇杆按下
-    $joystickBtn.on('mousedown touchstart', function(e) {
-        e.preventDefault();
-        isDragging = true;
-        $joystickBtn.addClass('dragging');
-    });
-
-    // 摇杆移动
-    $joystickContainer.on('mousemove touchmove', function(e) {
-        if (!isDragging) return;
-
-        // 适配触摸/鼠标事件
-        const event = e.type.includes('touch') ? e.touches[0] : e;
-        const clientX = event.clientX;
-        const clientY = event.clientY;
-
-        // 计算容器位置和偏移
-        const containerRect = $joystickContainer[0].getBoundingClientRect();
-        const containerWidth = containerRect.width;
-        const containerHeight = containerRect.height;
-        const containerHalfWidth = containerWidth / 2;
-        const containerHalfHeight = containerHeight / 2;
-
-        const offsetX = clientX - (containerRect.left + containerHalfWidth);
-        const offsetY = clientY - (containerRect.top + containerHalfHeight);
-
-        // 边界限制
-        const maxOffsetX = containerHalfWidth - btnHalfWidth;
-        const maxOffsetY = containerHalfHeight - btnHalfHeight;
-        const clampedOffsetX = Math.max(-maxOffsetX, Math.min(offsetX, maxOffsetX));
-        const clampedOffsetY = Math.max(-maxOffsetY, Math.min(offsetY, maxOffsetY));
-
-        // 更新摇杆位置
-        $joystickBtn.css('transform', `translate(calc(-50% + ${clampedOffsetX}px), calc(-50% + ${clampedOffsetY}px))`);
-
-        // 计算舵机角度
-        const s1Angle = Math.round(180 - ((clampedOffsetX + maxOffsetX) / (2 * maxOffsetX)) * 180);
-        const s2Angle = Math.round(180 - ((clampedOffsetY + maxOffsetY) / (2 * maxOffsetY)) * 180);
-
-        // 新增核心逻辑：拖动摇杆时自动切换跟踪模式为关闭（track_off）
-        if (!hasSetTrackOff) {
-            const trackOffVal = 'track_off'; // 请确认该值与follow_list中「关闭跟踪」对应的value一致
-            // 如果当前不是关闭跟踪模式，则切换并触发change事件
-            if ($followFlag.val() !== trackOffVal) {
-                $followFlag.val(trackOffVal).trigger('change');
-                addLogToNippleLog({ type: 'info', msg: '拖动摇杆，自动关闭跟踪模式' });
-            }
-            hasSetTrackOff = true; // 标记已设置，避免重复触发
-        }
-
-        // 节流更新角度和发送指令
-        const now = Date.now();
-        if (now - lastAngleUpdateTime > angleUpdateInterval) {
-            lastAngleUpdateTime = now;
-            $s1Angle.text(s1Angle);
-            $s2Angle.text(s2Angle);
-            sendActionTxt($deviceId.val(), `S1:${s1Angle},S2:${s2Angle};`);
-        }
-    });
-
-    // 摇杆松开/离开
-    $joystickContainer.on('mouseup mouseleave touchend touchcancel', function() {
-        if (isDragging) {
-            isDragging = false;
-            $joystickBtn.removeClass('dragging');
-            hasSetTrackOff = false; // 重置标记，下次拖动可再次触发
-            // 如需松开复位，取消注释下面这行
-            // resetJoystick();
-        }
-    });
-
-    // ===================== 日志辅助函数 =====================
-    function addLogToNippleLog(logData) {
-        const timeStr = new Date().toLocaleTimeString();
-        const logHtml = `
-            <div class="log-item ${logData.type}">
-                <small>[${timeStr}]</small> ${logData.msg} 
-                ${logData.data ? '<br>' + JSON.stringify(logData.data, null, 2) : ''}
-            </div>
-        `;
-        $nippleLog.prepend(logHtml);
-        
-        // 最多保留20条日志，避免性能问题
-        if ($nippleLog.children().length > 20) {
-            $nippleLog.children().last().remove();
-        }
-        $nippleLog.scrollTop(0);
-    }
-
-    // ===================== 事件绑定 =====================
-    $(function() {
-        // 复位按钮
-        $resetBtn.on('click', resetJoystick);
-
-        // 跟踪模式变更
-        $followFlag.on('change', function() {
-            setFollowFlag($deviceId.val(), $(this).val());
-        });
-
-        // 设备ID变更时更新图像流地址
-        $deviceId.on('change', function() {
-            const deviceId = $(this).val();
-            $headVideo.attr('src', `/wrobot_cam/${deviceId}`);
-        });
-
-    });
-</script>
-</body>
-</html>
@@ -1,28 +0,0 @@
-from flask import Blueprint,session,render_template
-from app.glo import *
-
-
-wrobot = Blueprint('wrobot', __name__)
-
-@wrobot.route('/wrobot/')
-def main():
- 
-    device_list = GetUserDeviceIdList(session['user']['user_id'])
-
-    follow_list = {
-        'face': 'track_face',
-        # 'hand': 'track_hand',
-        'none': 'track_off',
-    }
-    data = {
-        "page_title": "%s" % ( "Wrobot") , 
-
-        "device_id": device_list[0],
-
-        "follow_id": 'face',
-
-        "device_list": device_list, 
-        "follow_list": follow_list, 
-    }
-
-    return render_template('/cn/aiot/wrobot.html', **data)
@@ -1,145 +0,0 @@
-import threading
-import time
-import re
-import traceback
-import paho.mqtt.client as mqtt
-from flask import Blueprint, Response, session
-
-from app.glo import *
-
-# ==================== 全局配置 ====================
-mqtt_client = None
-MQTT_SUB_TOPIC = ""
-latest_frame_cache = {}
-cache_lock = threading.Lock()
-mqtt_sub_lock = threading.Lock()
-
-wrobot_cam = Blueprint('wrobot_cam', __name__)
-
-
-# ==================== MQTT 核心逻辑 ====================
-def on_mqtt_connect(client, userdata, flags, rc):
-    """MQTT连接成功回调：此时暂不订阅（等第一次页面访问时再订阅）"""
-    if rc == 0:
-        print("MQTT连接成功（暂未订阅任何主题，等待页面访问）")
-    else:
-        print(f"MQTT连接失败  错误码: {rc}")
-
-def on_mqtt_message(client, userdata, message):
-    """MQTT消息回调：仅处理已订阅的主题"""
-    try:
-        if not MQTT_SUB_TOPIC or message.topic != MQTT_SUB_TOPIC:
-            return
-        
-        device_id = message.topic.split('/')[0]
-        if len(message.payload) < 100:
-            return
-        
-        with cache_lock:
-            latest_frame_cache[device_id] = message.payload
-
-    except Exception as e:
-        print(f"MQTT消息处理异常: {e}")
-        traceback.print_exc()
-
-def start_mqtt_background():
-    """启动MQTT客户端（仅连接，不订阅）"""
-    global mqtt_client
-    if mqtt_client is not None:
-        return
-
-    mqtt_client = mqtt.Client()
-    mqtt_client.on_connect = on_mqtt_connect
-    mqtt_client.on_message = on_mqtt_message
-
-    try:
-        mqtt_client.connect(MQTT_BROKER, MQTT_PORT, keepalive=60)
-        mqtt_client.loop_start()
-        print(f"MQTT客户端已启动 🚀（仅连接，未订阅主题）")
-    except Exception as e:
-        print(f"MQTT启动失败: {e}")
-        traceback.print_exc()
-
-def subscribe_first_topic(new_topic):
-    """第一次订阅主题（初始化订阅）"""
-    global MQTT_SUB_TOPIC
-    if not mqtt_client or not mqtt_client.is_connected():
-        raise RuntimeError("MQTT客户端未连接")
-
-    with mqtt_sub_lock:
-        MQTT_SUB_TOPIC = new_topic
-        mqtt_client.subscribe(MQTT_SUB_TOPIC)
-        print(f"MQTT首次订阅: {MQTT_SUB_TOPIC}")
-
-def update_mqtt_subscription(new_topic):
-    """更新订阅主题（非首次）"""
-    global MQTT_SUB_TOPIC
-    if not mqtt_client or not mqtt_client.is_connected():
-        raise RuntimeError("MQTT客户端未连接")
-
-    with mqtt_sub_lock:
-        # 取消旧主题订阅
-        mqtt_client.unsubscribe(MQTT_SUB_TOPIC)
-        print(f"MQTT取消订阅: {MQTT_SUB_TOPIC}")
-        
-        # 订阅新主题
-        MQTT_SUB_TOPIC = new_topic
-        mqtt_client.subscribe(MQTT_SUB_TOPIC)
-        print(f"MQTT重新订阅: {MQTT_SUB_TOPIC}")
-
-# ==================== Flask 路由核心逻辑 ====================
-def generate_video_frame(device_id):
-    """生成视频流"""
-    blank_jpeg = b'\xff\xd8\xff\xe0\x00\x10JFIF\x00\x01\x01\x00\x00\x01\x00\x01\x00\x00\xff\xd9'
-    
-    while True:
-        try:
-            with cache_lock:
-                frame_data = latest_frame_cache.get(device_id, blank_jpeg)
-            
-            yield (b'--frame\r\n'
-                   b'Content-Type: image/jpeg\r\n\r\n' + frame_data + b'\r\n')
-            
-            time.sleep(0.01)
-        
-        except GeneratorExit:
-            break
-        except Exception as e:
-            print(f"视频流生成异常: {e}")
-            yield (b'--frame\r\n'
-                   b'Content-Type: image/jpeg\r\n\r\n' + blank_jpeg + b'\r\n')
-            time.sleep(0.1)
-
-@wrobot_cam.route('/wrobot_cam/<string:device_id>')
-@wrobot_cam.route('/wrobot_cam')
-def main(device_id=None):
-    """核心逻辑：第一次访问时初始化订阅，后续访问时更新订阅"""
-    # 1. 校验设备ID（无参数时直接报错，因为启动时未知默认设备）
-    if not device_id:
-        return Response("必须指定设备ID（访问方式：/wrobot_cam/设备ID）", status=400, mimetype='text/plain')
-    
-    device_list = GetUserDeviceIdList(session['user']['user_id'])
-    if device_id not in device_list:
-        return Response(f"设备ID不合法", status=400, mimetype='text/plain')
-
-    
-    # 2. 构建真实主题
-    new_mqtt_topic = f"{device_id}/head_video"
-    
-    # 3. 区分「首次订阅」和「更新订阅」
-    try:
-        if not MQTT_SUB_TOPIC:  # 初始值为空 → 第一次访问，初始化订阅
-            threading.Thread(target=subscribe_first_topic, args=(new_mqtt_topic,), daemon=True).start()
-        elif new_mqtt_topic != MQTT_SUB_TOPIC:  # 非首次，更新订阅
-            threading.Thread(target=update_mqtt_subscription, args=(new_mqtt_topic,), daemon=True).start()
-    except RuntimeError as e:
-        return Response(f"MQTT客户端异常：{str(e)}", status=500, mimetype='text/plain')
-
-    # 4. 返回视频流
-    return Response(
-        generate_video_frame(device_id),
-        mimetype='multipart/x-mixed-replace; boundary=frame'
-    )
-
-# ==================== 启动初始化 ====================
-start_mqtt_background()
@@ -95,4 +95,28 @@ ActionStep step_left[] = {
 };
 RobotAction action_step_left = CREATE_ACTION("action_step_left", step_left);

+
+ActionStep step_turn_right[] = {
+  // {左+右-，前+后-，跳跃，平衡20左-右+，高度20升-降+，持续时间, 停顿时间0，
+  { -100, 0, 0, 0, -10, 50, 0,  -1, -1, -1, -1}, 
+  END_MARKER                       // 结束标记
+};
+RobotAction action_step_turn_right = CREATE_ACTION("action_step_turn_right", step_turn_right);
+
+ActionStep step_turn_left[] = {
+  // {左+右-，前+后-，跳跃，平衡20左-右+，高度20升-降+，持续时间, 停顿时间0，
+  { 100, 0, 0, 0, -10, 50, 0,  -1, -1, -1, -1}, 
+  END_MARKER                       // 结束标记
+};
+RobotAction action_step_turn_left = CREATE_ACTION("action_step_turn_left", step_turn_left);
+
+
+ActionStep step_forward[] = {
+  // {左+右-，前+后-，跳跃，平衡20左-右+，高度20升-降+，持续时间, 停顿时间0，
+  { 0, 50, 0, 0, 10, 500, 0,  -1, -1, -1, -1}, 
+  END_MARKER                       // 结束标记
+};
+RobotAction action_step_forward = CREATE_ACTION("action_step_forward", step_forward);
+
+
 #endif
@@ -17,7 +17,9 @@ void serialReceiveProcess() {
      if (strchr(serial_buf, '=') != NULL) {
        parseSingleParam(serial_buf); // 处理原格式指令
      } else if (strstr(serial_buf, "S1:") != NULL && strstr(serial_buf, "S2:") != NULL) {
-        parseServoCommand(serial_buf); // 处理舵机指令
+        parseServoAngleCommand(serial_buf); // 处理舵机角度控制指令 S1:90,S2:90;
+      } else if (strstr(serial_buf, "DX:") != NULL && strstr(serial_buf, "DY:") != NULL) {
+        parseCameraDeviationCommand(serial_buf); // 处理摄像头偏差控制指令 DX:90,DY:45;
      } else if (buf_idx > 0) {
        Serial.println("指令格式错误！请使用正确格式");
      }
@@ -29,7 +31,7 @@ void serialReceiveProcess() {
 }

 // 添加舵机指令解析函数
-void parseServoCommand(char* cmd) {
+void parseServoAngleCommand(char* cmd) {
  char* s1Ptr = strstr(cmd, "S1:");
  char* s2Ptr = strstr(cmd, "S2:");

@@ -43,8 +45,26 @@ void parseServoCommand(char* cmd) {
  int angle1 = atoi(s1Ptr + 3);  // "S1:"占3个字符，跳过它们取数值
  int angle2 = atoi(s2Ptr + 3);  // "S2:"占3个字符，跳过它们取数值

-  // 调用摄像头舵机控制函数
-  setCameraServoAngles(angle1, angle2);
+  // 舵机控制函数
+  setServoAngles(angle1, angle2);
+ 
+}
+void parseCameraDeviationCommand(char* cmd) {
+  char* dxPtr = strstr(cmd, "DX:");
+  char* dyPtr = strstr(cmd, "DY:");
+
+  // 检查指令格式是否正确
+  if (dxPtr == NULL || dyPtr == NULL) {
+    Serial.println("摄像头指令格式错误！请使用\"DX:角度,DY:角度;\"格式（例如\"DX:90,DY:45;\"）");
+    return;
+  }
+
+  // 转换为角度数值（跳过"S1:"和"S2:"字符）
+  int dx = atoi(dxPtr + 3);  // "DX:"占3个字符，跳过它们取数值
+  int dy = atoi(dyPtr + 3);  // "DY:"占3个字符，跳过它们取数值
+
+  // 控制摄像头上下左右
+  setCameraMoveAngles(dx, dy);
 
 }

@@ -102,11 +122,25 @@ void parseSingleParam(char* cmd) {
  } else if (strcmp(param_name, "high_motor_target") == 0) {
    high_motor_target = param_val;

-  } else if (strcmp(param_name, "servo1_joyx_factor") == 0) {
-    servo1_joyx_factor = param_val;
+  } else if (strcmp(param_name, "pid_trace_joyx_p") == 0) {
+    pid_trace_joyx.P = param_val;
+  } else if (strcmp(param_name, "pid_trace_joyx_d") == 0) {
+    pid_trace_joyx.D = param_val;
+
+  } else if (strcmp(param_name, "pid_trace_joyy_p") == 0) {
+    pid_trace_joyy_p = param_val;
+  } else if (strcmp(param_name, "pid_trace_joyy_d") == 0) {
+    pid_trace_joyy_d = param_val;
+
+  } else if (strcmp(param_name, "pitch_adjust") == 0) {
+    pitch_adjust = param_val;
+
+
+  // } else if (strcmp(param_name, "step_joyx") == 0) {
+  //   step_joyx = param_val;
+  // } else if (strcmp(param_name, "step_duration") == 0) {
+  //   step_duration = param_val;

-  } else if (strcmp(param_name, "servo2_joyy_factor") == 0) {
-    servo2_joyy_factor = param_val;

  } else if (strcmp(param_name, "mac") == 0) {
    // 1. 获取传入的 MAC 字符串（注意：eq_pos+1 是字符串，不能用 atof 转浮点）
@@ -7,12 +7,20 @@

 // -------------------------- 全局对象声明（与 main.cpp 中定义一致）--------------------------
 // 这些对象在 main.cpp 中定义，此处用 extern 声明供 SerialParser 访问
-extern PIDController pid_super_balance;
+
 extern PIDController pid_speed;
+
 extern float zaccel_threshold;
 extern float dragon_joyx_factor;


+extern PIDController pid_trace_joyx;
+extern float pid_trace_joyy_p;
+extern float pid_trace_joyy_d;
+
+extern float pitch_adjust;
+
+
 // extern float obstacle_pitch;
 // extern int servo_up_height;
 // extern int servo_down_height;
@@ -27,8 +35,9 @@ extern float high_obstacle_pitch;
 extern int high_servo_up_height;
 extern int high_servo_down_height; 
 extern float high_motor_target;
-extern float servo1_joyx_factor;
-extern float servo2_joyy_factor;
+
+// extern int step_joyx;
+// extern int step_duration;

 extern char serial_buf[32];
 extern uint8_t buf_idx;
@@ -38,9 +47,12 @@ void serialReceiveProcess();
 void parseSingleParam(char* cmd);

 // 舵机控制函数声明
-void parseServoCommand(char* cmd);
+void parseServoAngleCommand(char* cmd);
+void parseCameraDeviationCommand(char* cmd);

-void setCameraServoAngles(int s1, int s2);
+void setServoAngles(int s1, int s2);
+void setCameraMoveAngles(int dx, int dy);
+void setCarmeraFixServoAngles(int servo_angle);
 void setFrontierServoAngles(int servo_angle);

 #endif // SERIAL_PARSER_H
@@ -25,7 +25,7 @@ void yaw_loop();
 void leg_loop();
 void jump_loop();

-void cancel_super_mode();
+void cancel_trace_mode();

 // 跨越障碍函数
 void obstacle_detect(); // 越障高度检测
@@ -40,8 +40,6 @@ void jump_charge(); // 腿部弯曲（蓄能）
 void processControllerData(const XboxControllerNotificationParser& data);
 void runActionSequence();

-
-
 const int deadZone = 10; // 摇杆死区阈值
 int trigRT = 0;
 int last_trigRT = 0;
@@ -62,7 +60,15 @@ bool last_btnStart = 0;
 bool last_btnShare = 0;
 bool last_joyRHori = 0;
 bool last_joyLHori = 0;
-
+bool last_joyLVert = 0;
+int convertedJ1PotX;
+int convertedJ1PotY;
+int convertedJ2PotX;
+int convertedJ2PotY;
+int last_convertedJ1PotX;
+int last_convertedJ1PotY;
+int last_convertedJ2PotX;
+int last_convertedJ2PotY;

 /************实例定义*************/

@@ -217,9 +223,6 @@ int high_servo_up_height = 380; // +390=MAX
 int high_servo_down_height = 0; 
 float high_motor_target = -9.0f;

-// 超级平衡模式参数
-bool super_balance_mode = false; // 超级平衡模式
-PIDController pid_super_balance(11.5, 0, 0, 100000, 8); // 适配joyy实际需求

 // 舵机中位校准
 int calibrationOfs_flag = 0;
@@ -227,8 +230,6 @@ int calibrationOfs_flag = 0;
 // 倒地恢复计数
 int fallRecoveryResetCounter = 0;

-// 快速下坠状态标志
-
 // 开机默认坐下 修改
 bool robot_enabled = false; // 开机禁用平衡，默认坐下
 bool last_robot_enabled = false; // 同步初始状态
@@ -246,13 +247,8 @@ RobotAction* currentAction = NULL;

 //  龙头模式
 bool dragon_head_mode = false; // 龙头模式标志
-float dragon_joyx_factor = 1.0f;
-int convertedJ1PotX;
-int convertedJ1PotY;
-int convertedJ2PotX;
-int last_convertedJ1PotX;
-int last_convertedJ1PotY;
-int last_convertedJ2PotX;
+float dragon_joyx_factor = 0.8f;
+

 typedef struct
 {
@@ -271,25 +267,31 @@ typedef struct
 Wrobot wrobot;

 // 云台舵机控制引脚
-#define FRONTIER_SERVO_PIN 36  // 前档舵机   S1 4   
-#define CAM_BOTTOM_SERVO_PIN 15  // 云台底座   S1 15   
-#define CAM_FIX_SERVO_PIN 4  // 固定摄像头舵机 S2   4
+#define FRONTIER_SERVO_PIN 15  // 前档舵机  
+#define CAM_FIX_SERVO_PIN 4  // 固定摄像头舵机 S2  ,左右旋转是S1

 // 创建舵机对象
 Servo frontier_servo;
-Servo cam_bottom_servo;
 Servo cam_fix_servo; 

-// 舵机角度变量
-int frontier_servo_angle = 0;      // 前档舵机
+// 追踪模式
+bool trace_mode = false; // 追踪模式整体标志
+bool trace_ob_flag = false; // 是否追踪到位
+int joyx_servo = 9999;

-int cam_bottom_servo_angle = 90;  // 云台底座舵机初始角度 左右旋转 S1
-int cam_fix_servo_angle = 90;     // 摄像头舵机初始角度 上下旋转 S2
+PIDController pid_trace_joyx(0.5, 0, 0, 100000, 100);

-// （暂时无用）
-float servo1_joyx_factor = 0.1f; // 舵机1 云台底座舵机 旋转 系数，水平的，所以是x
-float servo2_joyy_factor = 0.5f; // 舵机2 摄像头舵机 旋转 系数，上下的，所以是y
+// 摄像头舵机
+int current_s2 = 90;       // 记录舵机最新角度，初始为中位
+// float pid_trace_joyy_p = 0.045f;   // P系数
+// float pid_trace_joyy_d = 0.0f;   // D系数

+// 摄像头舵机 PD 控制器参数
+float pid_trace_joyy_p = 0.07f;   // 比例系数（P）
+float pid_trace_joyy_d = 0.01f;    // 微分系数（D），可根据实际调试调整
+int pid_trace_joyy_output_limit = 10; // PD输出限幅（单次最大调整角度）
+int last_dy = 0;                   // 上一次的y轴偏差
+unsigned long last_dy_time = 0;    // 上一次计算偏差的时间戳

 void setup()
 {
@@ -381,16 +383,12 @@ void setup()
  xboxControllerInit();

  // 初始化云台两个舵机
-  cam_bottom_servo.attach(CAM_BOTTOM_SERVO_PIN);
  cam_fix_servo.attach(CAM_FIX_SERVO_PIN);
-
-  // 初始位置：两个舵机都转到90度
-  cam_bottom_servo.write(cam_bottom_servo_angle);  // 正面看90度正面，0度右手，180度左手
-  cam_fix_servo.write(cam_fix_servo_angle);  // 摄像头朝上180度，朝下30度
+  cam_fix_servo.write(current_s2);  // 摄像头朝上180度，朝下0度
  
  // 前档舵机
  frontier_servo.attach(FRONTIER_SERVO_PIN);
-  frontier_servo.write(frontier_servo_angle);  // 摄像头朝上180度，朝下30度
+  frontier_servo.write(0); 

  delay(100);
 }
@@ -516,9 +514,6 @@ void loop()
  // 小车坐下
  if (sitting_down) {

-    // 关闭超级平衡模式
-    cancel_super_mode();
-
    sitting_down ++;

    if (sitting_down < 15) {
@@ -618,13 +613,6 @@ void lqr_balance_loop()
    speed_target_coeff = 0.1;
  }

-  // 超级平衡模式 误差=实际角度 - 默认零点
-  if (super_balance_mode && !is_falling && robot_enabled && !sitting_down && !jump_flag ) {
-    float balance_joyy = pid_super_balance(LQR_angle -pitch_zeropoint);
-    balance_joyy = constrain(balance_joyy, -100.0f, 100.0f); 
-    wrobot.joyy = balance_joyy;
-  }
-
  speed_control = pid_speed(LQR_speed - speed_target_coeff * lpf_joyy(wrobot.joyy));  // 最大8v

  // 检测轮子差速，判断轮子是否离地
@@ -960,12 +948,19 @@ void jump_loop()
 void yaw_loop()
 {
  // 跳跃中，YAW_output 设为0，避免干扰左右旋转
-  if(jump_flag) 
-  {
+  if(jump_flag){
    YAW_output = 0;
    return;
  }

+  if(trace_mode){
+    if(joyx_servo!= 9999){
+      wrobot.joyx = joyx_servo;
+    }else{
+      wrobot.joyx = 0;
+    }   
+  }
+
 // 1. 根据abs(wrobot.joyx)，动态设置yaw_target系数0.1~1
  float coeff = map( abs(wrobot.joyx), 0, 100, 10, 100) / 100.0f; 
  coeff = constrain(coeff, 0.1f, 1.0f); 
@@ -973,7 +968,7 @@ void yaw_loop()

  // 2. 根据abs(wrobot.joyx)，提高PID响应，临时放大系数
  float yaw_angle_fator = 1.0f;
-  if( wrobot.joyy == 0 && abs(wrobot.joyx) > 10 && !dragon_head_mode){ // 原地打转
+  if( wrobot.joyy == 0 && abs(wrobot.joyx) > 10 && !dragon_head_mode && !trace_mode){ // 原地打转
    yaw_angle_fator = map( abs(wrobot.joyx), 0, 100, 1.0, 2.0); 
    // 根据转向速度，智能计算wrobot.height 
    // abs(wrobot.joyx) 范围0到100，默认0，输出范围20-50,默认 30
@@ -1007,7 +1002,6 @@ void yaw_loop()
 void resetZeroPoint() {
  distance_zeropoint =  LQR_distance;
  pid_lqr_u.error_prev = 0;
-  pid_super_balance.error_prev = 0.0f; // 重置积分项
  pitch_adjust = 0.0f;
 }

@@ -1041,14 +1035,16 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
 //   Serial.println(str);

 // 正在执行动作组，忽略其他指令
-  if (isExecutingAction) {
-    return; 
-  }
+  // if (isExecutingAction) {
+  //   return; 
+  // }

  // 坐下
  if (remoteData.btnLB && !last_btnLB && robot_enabled) { 
    robot_enabled = false;
    sitting_down = 1; // 标记开始坐下动作保存当前的角度零点
+
+    pid_trace_joyx.P = 0.5; // 追踪模式
  }
  // 第二次按坐下，让舵机失能
  if (remoteData.btnLB && !last_btnLB && !robot_enabled) { 
@@ -1062,6 +1058,8 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
    robot_enabled = true;
    sitting_down = 0; // 清除坐下动作标记
    stand_up_count ++; // 站立标志位
+
+    pid_trace_joyx.P = 0.25; // 追踪模式
  }
   last_btnRB = remoteData.btnRB;

@@ -1082,7 +1080,7 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
  }

    // 前后移动 右摇杆Y轴
-    int convertedJ2PotY = constrain(
+    convertedJ2PotY = constrain(
      map(remoteData.joyRVert, 0, 65535, 100, -100),  // 原始0~65535，目标100~-100
      -100, 100
    );
@@ -1135,18 +1133,6 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
    last_convertedJ1PotX = convertedJ1PotX;
    last_convertedJ2PotX = convertedJ2PotX;

-    if(dragon_head_mode){ // 龙头模式，左摇杆控制云台
-      cam_bottom_servo_angle = constrain( // 左右
-        map(remoteData.joyLHori, 0, 65535, 0, 180),  
-        0, 180
-      );
-      cam_fix_servo_angle = constrain(
-        map(remoteData.joyLVert, 0, 65535, 180, 0),  
-        0, 180
-      );
-      setCameraServoAngles(cam_bottom_servo_angle, cam_fix_servo_angle);
-    }
-     
    // 智能计算wrobot.height ，根据前后速度，
    // convertedJ2PotY 范围-100到100，默认0，输出范围30（最矮）-50（最高）, 默认 50
    // wrobot.height 0 最矮，50最高
@@ -1154,57 +1140,63 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
    wrobot.height = constrain(wrobot.height, 30, 50);
    wrobot.height = lpf_height(wrobot.height);

-    // // 右摇摆
-    if(remoteData.btnDirRight) { 
-      // roll_adjust += 0.20;
-      pitch_adjust += 0.20;
-      // isExecutingAction = true;          // 标记开始执行动作
-      // currentAction = &action_step_right; // 指定执行的动作
-      // stepStartTime = millis();          // 记录第一步开始时间
-      // currentStepIndex = 0;              // 从第一步开始
-
+    // 向右键 控制左右摇摆角度
+    if( !remoteData.btnShare && remoteData.btnDirRight) { 
+      roll_adjust += 0.20;   // 左右摇摆角度
    }
-    // 左摇摆
-    if(remoteData.btnDirLeft) { 
-      // roll_adjust -= 0.20;
-      pitch_adjust -= 0.20;
-      // isExecutingAction = true;          // 标记开始执行动作
-      // currentAction = &action_step_left; // 指定执行的动作
-      // stepStartTime = millis();          // 记录第一步开始时间
-      // currentStepIndex = 0;              // 从第一步开始
+    // 向左键  控制左右摇摆角度
+    if( !remoteData.btnShare && remoteData.btnDirLeft) { 
+      roll_adjust -= 0.20;   // 左右摇摆角度
    }

-    // 抬升
-    if(remoteData.btnDirUp) { 
-      leg_height_base -= 0.20;
-      // Serial.print("leg_height_base");
-      // Serial.println(leg_height_base);
-
-      // isExecutingAction = true;          // 标记开始执行动作
-      // currentAction = &action_step_up; // 指定执行的动作
-      // stepStartTime = millis();          // 记录第一步开始时间
-      // currentStepIndex = 0;              // 从第一步开始

+    // 向上键 控制机身升高
+    if(!remoteData.btnShare && remoteData.btnDirUp) { 
+      leg_height_base -= 0.20; // 机身升高
+    }
+    // 向下键 控制机身下降
+    if(!remoteData.btnShare && remoteData.btnDirDown) { 
+      leg_height_base += 0.20;  // 机身下降
    }

-    // 下降
-    if(remoteData.btnDirDown) { 
-      leg_height_base += 0.20;
-      // Serial.print("leg_height_base");
-      // Serial.println(leg_height_base);
-
-      // isExecutingAction = true;          // 标记开始执行动作
-      // currentAction = &action_step_down; // 指定执行的动作
-      // stepStartTime = millis();          // 记录第一步开始时间
-      // currentStepIndex = 0;              // 从第一步开始
+    // 截图键 + 向上键
+    if(remoteData.btnShare && remoteData.btnDirUp && !last_btnDirUp) { 
+      int new_s2 = current_s2 + 2; // ±1°调整 测试
+      setCarmeraFixServoAngles(new_s2);  // 摄像头舵机上下运动
+      // Serial.print("new_s2:");
+      // Serial.println(new_s2);
    }

+    // 截图键 + 向下键
+    if(remoteData.btnShare && remoteData.btnDirDown && !last_btnDirDown) { 
+      int new_s2 = current_s2 - 2; // ±1°调整 测试
+      // Serial.print("new_s2:");
+      // Serial.println(new_s2);
+      setCarmeraFixServoAngles(new_s2);  // 摄像头舵机上下运动
+    }
+
+    // 截图键 + 向右键 last_btnDirRight
+    if(remoteData.btnShare && remoteData.btnDirRight && !last_btnDirLeft) { 
+      pitch_adjust += 0.20;  // 向后倾斜
+    }
+    // 截图键 + 向左键 控制向前倾斜
+    if(remoteData.btnShare && remoteData.btnDirLeft && !last_btnDirLeft) { 
+      pitch_adjust -= 0.20;  // 向前倾斜
+    }
+
+    last_btnDirUp = remoteData.btnDirUp;
+    last_btnDirDown = remoteData.btnDirDown;
+    last_btnDirRight = remoteData.btnDirRight;
+    last_btnDirLeft = remoteData.btnDirLeft;
+
+
    // 还原
    if (remoteData.btnLS) {
      roll_adjust = original_roll_adjust; 
+      pitch_adjust = 0.0f;
      leg_height_base = original_leg_height_base;
      pitch_zeropoint = original_pitch_zeropoint;
-      pitch_adjust = 0.0f;
+      setCarmeraFixServoAngles(90);// 摄像头舵机还原
    }

    //  右扳机 高度向下 ：原始0-1023
@@ -1218,14 +1210,10 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
    startColorSequenceBlink();
  }

-  // 控制前档舵机
-  if(remoteData.trigLT){
-    frontier_servo_angle = map(remoteData.trigLT, 0, 1023, 0, 180);
-    setFrontierServoAngles(frontier_servo_angle);
-  }
-  if(!remoteData.trigLT){
-    frontier_servo_angle = 0;
-    setFrontierServoAngles(frontier_servo_angle);
+  // 控制前档舵机，追踪模式下不可用，因为是自动控制
+  if(remoteData.trigLT && !trace_mode){
+    int servo_angle = map(remoteData.trigLT, 0, 1023, 0, 180);
+    setFrontierServoAngles(servo_angle);
  }

  // 跳跃 右摇杆按键
@@ -1288,31 +1276,24 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
  }
  last_trigRT = trigRT;

-  // btnShare 晃头，点头
-  if (remoteData.btnShare && !last_btnShare && robot_enabled && !isExecutingAction) {
-    isExecutingAction = true;          // 标记开始执行动作
-    currentAction = &action_say_hello; // 指定执行的动作
-    stepStartTime = millis();          // 记录第一步开始时间
-    currentStepIndex = 0;              // 从第一步开始
-  }
-  last_btnShare = remoteData.btnShare; // 更新btnY的上一次状态
-
-  // 超级平衡模式切换
+  //  追踪模式 rgb：蓝色
  if (remoteData.btnSelect && !last_btnSelect) { 
-
-      pid_super_balance.error_prev = 0.0f;
-
-      super_balance_mode = !super_balance_mode;
-      if (super_balance_mode) {
+      trace_mode = !trace_mode;
+      if (trace_mode) {
        setLEDColor(CRGB::Blue);
-        zaccel_threshold = super_zaccel_threshold;
-        resetZeroPoint(); // 重置位移零点，确保初始平衡
+        setCarmeraFixServoAngles(120);  // 摄像头舵机上下运动
      }else{
-        cancel_super_mode();
+        cancel_trace_mode();
+      }
+  }
+  // 已经追踪到物体 rgb：绿色
+  if (trace_mode) {
+    if(trace_ob_flag){ 
+        setLEDColor(CRGB::Green); 
+      }else{
+        setLEDColor(CRGB::Blue);
    }
  }
-  last_btnSelect = remoteData.btnSelect;
-

  // 舵机中位校准 中间十字左右键同时按下
  if (calibrationOfs_flag == 0 && remoteData.btnSelect && last_btnSelect && remoteData.btnStart && last_btnStart) { 
@@ -1320,10 +1301,10 @@ void processControllerData(const XboxControllerNotificationParser& remoteData) {
      sms_sts.CalibrationOfs(ID[0]);
      sms_sts.CalibrationOfs(ID[1]);
      XboxKeyVibration(100,2000); // 震动
-      Serial.println("CalibrationOfs");
+      // Serial.println("CalibrationOfs");
  }
  if(calibrationOfs_flag > 0){
-     calibrationOfs_flag ++;;
+     calibrationOfs_flag ++;
  }
  if(calibrationOfs_flag > 500){
    calibrationOfs_flag =0;
@@ -1381,6 +1362,14 @@ void runActionSequence() {
    return;
  }
  
+  // 串口调试
+  // if(step_joyx != -1){
+  //   currentStep.joyx = (currentStep.joyx/abs(currentStep.joyx) * step_joyx);
+  // }
+  // if(step_duration != -1){
+  //   currentStep.stepDuration = step_duration;
+  // }
+
  // 更新当前步骤的控制参数
  wrobot.joyx = currentStep.joyx;
  wrobot.joyy = currentStep.joyy;
@@ -1408,14 +1397,6 @@ void runActionSequence() {
  }
 }

-// 退出超级平衡模式
-void cancel_super_mode(){
-  super_balance_mode = false;
-  zaccel_threshold = normal_zaccel_threshold;
-  setLEDColor(CRGB::Black);
-  resetZeroPoint(); 
-}
-
 // 障碍检测
 void obstacle_detect() {

@@ -1545,26 +1526,104 @@ void obstacle_cancel()
    obstacle_mode = false; // 跳出越障模式
 }

-// 控制摄像头舵机
-void setCameraServoAngles(int s1, int s2) {
-  // 限制角度在0-180度范围内
-  cam_bottom_servo_angle = constrain(s1, 0, 180);
-  cam_fix_servo_angle = constrain(s2, 0, 180);
+// 通过偏差deviation 控制摄像头上下左右运动
+void setCameraMoveAngles(int dx, int dy) {

-  // 控制舵机转动
-  cam_bottom_servo.write(cam_bottom_servo_angle);
-  cam_fix_servo.write(cam_fix_servo_angle);
-  
-  // 反馈执行结果
-  // Serial.print("舵机1转到 ");
-  // Serial.print(cam_bottom_servo_angle);
-  // Serial.print("度，舵机2转到 ");
-  // Serial.print(cam_fix_servo_angle);
-  // Serial.println("度");
+  if(!trace_mode or dx == 9999 or dy == 9999){
+    joyx_servo = 9999;
+    trace_ob_flag = false;
+    // 重置PD控制器状态
+    last_dy = 0;
+    last_dy_time = 0;
+    return;
  }

+  // 判断小车正面是否朝向物体，只需要一个区间就可以，不用必须到图像中心
+  if( abs(dx) < 40){
+    trace_ob_flag = true;
+    joyx_servo = 9999;
+    pitch_adjust = 0.0f;
+  }else{
+    trace_ob_flag = false;
+  }
+
+  // 正面追踪到物体，打开挡板，下降高度
+  if(current_s2 < 60 && dy > -20 ){ // 球到车身下方了，打球
+    setFrontierServoAngles(0); // 打球，挡板回到前方
+    leg_height_base = original_leg_height_base; // 上升
+
+  }else if(trace_ob_flag){ // 追踪到目标
+    setFrontierServoAngles(180); // 打开挡板，收到后方
+    leg_height_base = 30.0f; // 下降
+  }else{
+    leg_height_base = original_leg_height_base; // 原来高度
+  }
+
+  // 上下旋转舵机 - PD控制实现
+  if(abs(dy) > 10){ // 偏差超过阈值才进行调整
+    unsigned long current_time = millis();
+    // 首次执行初始化时间戳
+    if(last_dy_time == 0){
+      last_dy_time = current_time;
+      last_dy = dy;
+    }
+    
+    // 计算new_s2
+    unsigned long dt = max((unsigned long)1, current_time - last_dy_time);
+    float d_term = pid_trace_joyy_d * (dy - last_dy) / (float)dt * 10;
+    float p_term = pid_trace_joyy_p * dy;
+    float pd_output = p_term + d_term;
+    pd_output = constrain(pd_output, -pid_trace_joyy_output_limit, pid_trace_joyy_output_limit);
+    int new_s2 = current_s2 - (int)pd_output;
+    
+    // 更新状态变量
+    last_dy = dy;
+    last_dy_time = current_time;
+
+    // 控制舵机运动
+    setCarmeraFixServoAngles(new_s2);
+    
+  }else{
+    // 偏差过小，重置PD状态
+    last_dy = 0;
+  }
+
+  // 左右旋转，foc电机
+  if(!trace_ob_flag){ 
+    int joyx_derection = (dx > 0) ? -1 : 1;
+    joyx_servo = pid_trace_joyx(abs(dx)) * joyx_derection;
+  }
+  
+}
+
+// 控制摄像头舵机
+void setCarmeraFixServoAngles(int servo_angle) {
+  if (abs(servo_angle - current_s2) > 1) {
+    servo_angle = constrain(servo_angle, 45, 180); // 60 -180
+    cam_fix_servo.write(servo_angle);
+    current_s2 = servo_angle;
+  }
+}
 // 控制前档舵机
 void setFrontierServoAngles(int servo_angle) {
    servo_angle = constrain(servo_angle, 0, 180);
-  frontier_servo.write(frontier_servo_angle);
+    frontier_servo.write(servo_angle);
+}
+
+// 通过角度控制舵机
+void setServoAngles(int s1, int s2) {
+  // s1 前档舵
+  setFrontierServoAngles(s1);
+ 
+  // s2 摄像头舵机
+  setCarmeraFixServoAngles(s2);
+}
+
+// 退出追踪模式
+void cancel_trace_mode(){
+  trace_mode = false;
+  joyx_servo = 9999;
+  setLEDColor(CRGB::Black);
+  leg_height_base = original_leg_height_base; // 上升
+  setFrontierServoAngles(0);
 }