执行加速（部分动作连接处过快）

components/actuator/actuator.c

@@ -11,6 +11,7 @@
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
 #include "esp_log.h"
+#include "esp_timer.h"
 #include <math.h>
 #include <string.h>
 
@@ -69,6 +70,112 @@ static void _wait_flash(uint32_t ms, uint8_t r, uint8_t g, uint8_t b, uint32_t p
     rgb_off();
 }
 
+/* 到位等待 (闭环反馈)
+ *
+ * 实测发现这台 X42S 的 S_FLAG Prf_TF 到位标志只有 X 轴会置位 (0x03),
+ * Z/Y1/Y2 始终为 0x01 (仅使能位), 轮询 Prf_TF 无法判定到位。
+ * 因此改为直接比对电机实时位置 (S_CPOS) 与目标位置 (S_TPOS):
+ * 偏差 < ACTUATOR_ARRIVAL_TOL_RAW 即认为到位 —— 不依赖 Prf_TF。
+ * 每轮重新读 S_TPOS (电机处理命令后目标才更新, 首轮可能是旧目标)。
+ * 需连续 2 次在容差内才确认: 确保电机已稳定停下 (减速途中/振荡时
+ * 位置可能短暂进入容差, 单独一次会"没到位就进行下一步")。
+ *
+ * ⚠️ 防坑: 轮询第一帧前必须留帧间隔 —— 运动命令最后一条是 Y 同步广播
+ *     触发 (00 FF 66 6B), 若紧跟发轮询帧会"零间隔背靠背", 驱动板丢字节
+ *     吞掉触发帧, 电机不启动。
+ *
+ * @param base_wait_ms 纯运动时间估算 (兜底超时基准)
+ * @param moved_mask   实际有行程的电机位掩码 (bit0=Z, bit1=X, bit2/3=Y1/Y2)
+ * @return true=全部到位, false=超时
+ */
+bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask)
+{
+    const uint8_t addrs[4] = {
+        ACTUATOR_ADDR_ANGLE, ACTUATOR_ADDR_X,
+        ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2
+    };
+    bool arrived[4] = {false, false, false, false};
+    uint8_t set_cnt[4] = {0, 0, 0, 0};   /* 连续读到在容差内的次数 (≥2 确认已停稳) */
+    int32_t last_cpos[4] = {-1, -1, -1, -1}; /* 诊断 */
+    int32_t last_tpos[4] = {-1, -1, -1, -1}; /* 诊断 */
+    uint32_t polls = 0;                    /* 诊断: 轮询轮数 */
+    uint32_t timeout_ms = base_wait_ms + ACTUATOR_SETTLE_MARGIN_MS;
+    int64_t deadline = esp_timer_get_time() + (int64_t)timeout_ms * 1000;
+
+    for (int i = 0; i < 4; i++) {
+        if (!(moved_mask & (1u << i)))
+            arrived[i] = true;   /* 无行程: 视为已到位 */
+    }
+
+    /* 与上一条运动命令 (尤其 Y 同步触发) 之间留帧间隔, 防丢字节 */
+    vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS));
+
+    while (esp_timer_get_time() < deadline) {
+        bool all = true;
+        for (int i = 0; i < 4; i++) {
+            if (arrived[i])
+                continue;
+
+            bool at_target = false;
+            int32_t tpos = Emm_V5_Read_TPOS(addrs[i]);
+            last_tpos[i] = tpos;
+            if (tpos >= 0) {
+                int32_t cpos = Emm_V5_Read_CPOS(addrs[i]);
+                last_cpos[i] = cpos;
+                if (cpos >= 0) {
+                    int32_t diff = (int32_t)((uint16_t)cpos - (uint16_t)tpos);
+                    if (diff < 0) diff = -diff;
+                    if (diff > 32768) diff = 65536 - diff;   /* 环形距离 */
+                    at_target = (diff <= (int32_t)ACTUATOR_ARRIVAL_TOL_RAW);
+                }
+            } else {
+                /* 目标位置读失败: 退回 Prf_TF 到位标志 (仅 X 可靠) */
+                uint8_t flags = Emm_V5_Read_Status_Flag(addrs[i], 50);
+                at_target = (flags != 0xFF && (flags & 0x02));
+            }
+
+            if (at_target) {
+                /* 连续 2 次在容差内才确认: 电机已稳定停下,
+                   防止减速途中/振荡时误判"到位"提前进入下一步 */
+                if (++set_cnt[i] >= 2) {
+                    arrived[i] = true;
+                } else {
+                    all = false;
+                }
+            } else {
+                set_cnt[i] = 0;
+                all = false;
+            }
+            vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS));  /* 帧间隔防粘包 */
+        }
+        polls++;
+        if (all)
+            return true;
+
+        /* 诊断: 前 3 轮打印各电机位置 (调试用, 定位后删除) */
+        if (polls <= 3) {
+            ESP_LOGI(TAG, "到位轮询#%u: cpos Z=%ld X=%ld Y1=%ld Y2=%ld  "
+                     "tpos Z=%ld X=%ld Y1=%ld Y2=%ld",
+                     polls,
+                     (long)last_cpos[0], (long)last_cpos[1], (long)last_cpos[2], (long)last_cpos[3],
+                     (long)last_tpos[0], (long)last_tpos[1], (long)last_tpos[2], (long)last_tpos[3]);
+        }
+
+        /* 红灯快闪反馈 (与旧 _wait_flash 一致) */
+        rgb_set(128, 0, 0);
+        vTaskDelay(pdMS_TO_TICKS(10));
+        rgb_off();
+        vTaskDelay(pdMS_TO_TICKS(10));
+    }
+
+    ESP_LOGW(TAG, "到位超时 (%u ms, %u 轮): cpos Z=%ld X=%ld Y1=%ld Y2=%ld  "
+             "tpos Z=%ld X=%ld Y1=%ld Y2=%ld",
+             timeout_ms, polls,
+             (long)last_cpos[0], (long)last_cpos[1], (long)last_cpos[2], (long)last_cpos[3],
+             (long)last_tpos[0], (long)last_tpos[1], (long)last_tpos[2], (long)last_tpos[3]);
+    return false;
+}
+
 /* ================================================================
  * 运动到位时间估算
  *
@@ -110,18 +217,28 @@ uint32_t actuator_settle_ms(uint32_t pulses, uint16_t vel, uint8_t acc)
     return actuator_motion_time_ms(pulses, vel, acc) + ACTUATOR_SETTLE_MARGIN_MS;
 }
 
-uint32_t actuator_mm_settle_ms(float distance_mm, uint16_t vel, uint8_t acc)
+uint32_t actuator_mm_motion_time_ms(float distance_mm, uint16_t vel, uint8_t acc)
 {
     if (distance_mm < 0.0f) distance_mm = -distance_mm;
     uint32_t pulses = (uint32_t)(distance_mm * s_scale_x + 0.5f);
-    return actuator_settle_ms(pulses, vel, acc);
+    return actuator_motion_time_ms(pulses, vel, acc);
+}
+
+uint32_t actuator_angle_motion_time_ms(float angle_deg, uint16_t vel, uint8_t acc)
+{
+    if (angle_deg < 0.0f) angle_deg = -angle_deg;
+    uint32_t pulses = (uint32_t)(angle_deg * 10.0f * s_scale_angle + 0.5f);
+    return actuator_motion_time_ms(pulses, vel, acc);
+}
+
+uint32_t actuator_mm_settle_ms(float distance_mm, uint16_t vel, uint8_t acc)
+{
+    return actuator_mm_motion_time_ms(distance_mm, vel, acc) + ACTUATOR_SETTLE_MARGIN_MS;
 }
 
 uint32_t actuator_angle_settle_ms(float angle_deg, uint16_t vel, uint8_t acc)
 {
-    if (angle_deg < 0.0f) angle_deg = -angle_deg;
+    return actuator_angle_motion_time_ms(angle_deg, vel, acc) + ACTUATOR_SETTLE_MARGIN_MS;
-    uint32_t pulses = (uint32_t)(angle_deg * 10.0f * s_scale_angle + 0.5f);
-    return actuator_settle_ms(pulses, vel, acc);
 }
 
 /* ================================================================
@@ -378,15 +495,20 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
              (double)tgt_angle_deg,
              tgt_x, tgt_y);
 
-    /* 按实际行程/速度/加速度计算到位等待 (Z/X/Y 并发, 取最长者, 含安全裕量) */
+    /* 到位轮询掩码: 只等待实际有行程的电机 (脉冲>0), 无行程直接视为已到位 */
     float src_target = _closest_reachable_angle(src_angle_deg, s_cur_angle_deg,
                                                  ACTUATOR_ANGLE_LIMIT_DEG);
-    uint32_t src_wait_ms = actuator_settle_ms(
+    uint32_t src_pulses_z = (uint32_t)(fabsf(src_target) * 10.0f * s_scale_angle + 0.5f);
-        (uint32_t)(fabsf(src_target) * 10.0f * s_scale_angle + 0.5f),
+    uint32_t src_mask = 0;
-        vel_angle, acc_angle);
+    if (src_pulses_z > 0) src_mask |= (1u << 0);                 /* Z 旋转 */
-    uint32_t t_wait = actuator_settle_ms(src_pulses_x, vel_xy, acc_xy);
+    if (src_pulses_x > 0) src_mask |= (1u << 1);                 /* X */
+    if (src_pulses_y > 0) src_mask |= (1u << 2) | (1u << 3);     /* Y 双电机 */
+
+    /* 纯运动时间 (无裕量), 仅作到位等待的兜底超时基准 */
+    uint32_t src_wait_ms = actuator_motion_time_ms(src_pulses_z, vel_angle, acc_angle);
+    uint32_t t_wait = actuator_motion_time_ms(src_pulses_x, vel_xy, acc_xy);
     if (t_wait > src_wait_ms) src_wait_ms = t_wait;
-    t_wait = actuator_settle_ms(src_pulses_y, vel_xy, acc_xy);
+    t_wait = actuator_motion_time_ms(src_pulses_y, vel_xy, acc_xy);
     if (t_wait > src_wait_ms) src_wait_ms = t_wait;
 
     /* ---- 1. 磁铁掉电 ---- */
@@ -400,8 +522,8 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
     _frame_gap();
     _actuator_move_y_sync(src_pulses_y, vel_xy, acc_xy);
 
-    /* ---- 3. 等待运动到位 (红灯快闪, 按行程实时计算) ---- */
+    /* ---- 3. 等待运动到位 (轮询闭环到位标志, 红灯快闪) ---- */
-    _wait_flash(src_wait_ms, 128, 0, 0, 200);
+    actuator_wait_arrival(src_wait_ms, src_mask);
 
     /* ---- 4. 舵机放下 ---- */
     servo_set_down();
@@ -420,12 +542,16 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
        终点朝向与旧协议 (绝对转到 rotate mod 360) 完全一致, 且不会绕远路 */
     float dst_rep = _closest_reachable_angle(tgt_angle_deg, s_cur_angle_deg,
                                              ACTUATOR_ANGLE_LIMIT_DEG);
-    uint32_t dst_wait_ms = actuator_settle_ms(
+    uint32_t dst_pulses_z = (uint32_t)(fabsf(dst_rep - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f);
-        (uint32_t)(fabsf(dst_rep - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f),
+    uint32_t dst_mask = 0;
-        vel_angle, acc_angle);
+    if (dst_pulses_z > 0) dst_mask |= (1u << 0);
-    t_wait = actuator_settle_ms(dst_pulses_x, vel_xy, acc_xy);
+    if (dst_pulses_x > 0) dst_mask |= (1u << 1);
+    if (dst_pulses_y > 0) dst_mask |= (1u << 2) | (1u << 3);
+
+    uint32_t dst_wait_ms = actuator_motion_time_ms(dst_pulses_z, vel_angle, acc_angle);
+    t_wait = actuator_motion_time_ms(dst_pulses_x, vel_xy, acc_xy);
     if (t_wait > dst_wait_ms) dst_wait_ms = t_wait;
-    t_wait = actuator_settle_ms(dst_pulses_y, vel_xy, acc_xy);
+    t_wait = actuator_motion_time_ms(dst_pulses_y, vel_xy, acc_xy);
     if (t_wait > dst_wait_ms) dst_wait_ms = t_wait;
 
     _actuator_move_angle_rel(dst_rep - s_cur_angle_deg, vel_angle, acc_angle);
@@ -435,8 +561,8 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
     _frame_gap();
     _actuator_move_y_sync(dst_pulses_y, vel_xy, acc_xy);
 
-    /* ---- 8. 等待运动到位 (红灯快闪, 按行程实时计算) ---- */
+    /* ---- 8. 等待运动到位 (轮询闭环到位标志, 红灯快闪) ---- */
-    _wait_flash(dst_wait_ms, 128, 0, 0, 200);
+    actuator_wait_arrival(dst_wait_ms, dst_mask);
 
     /* ---- 9. 舵机放下 ---- */
     servo_set_down();

components/actuator/actuator.h

@@ -106,30 +106,40 @@ extern "C" {
 #define ACTUATOR_ANGLE_LIMIT_DEG  270.0f
 #endif
 
-/** 执行 Cycle / 归零时 XY 轴的速度与加速度档位 (实测 3000RPM + 250 档稳定) */
+/** 执行 Cycle / 归零时 XY 轴的速度与加速度档位
+    2026-08-01 提速: 3000RPM → 3500RPM (加速度保持 250 档, 实测稳定)
+    注: 手册 FD 位置模式速度标称上限 0-3000RPM, 3500 超出文档范围需实测 */
 #ifndef ACTUATOR_EXEC_VEL
-#define ACTUATOR_EXEC_VEL        3000
+#define ACTUATOR_EXEC_VEL        3500
 #endif
 #ifndef ACTUATOR_EXEC_ACC
 #define ACTUATOR_EXEC_ACC        250
 #endif
 
-/** Z 旋转轴执行速度与加速度 (结构刚性有限, 必须保持低速; 300RPM 已偏快) */
+/** Z 旋转轴执行速度与加速度 (结构刚性有限, 低速为主; 300RPM 已偏快)
+    2026-08-01: 50→100RPM / acc 100→150 (仍远低于 300 上限, 可再微调) */
 #ifndef ACTUATOR_EXEC_Z_VEL
-#define ACTUATOR_EXEC_Z_VEL      50
+#define ACTUATOR_EXEC_Z_VEL      100
 #endif
 #ifndef ACTUATOR_EXEC_Z_ACC
-#define ACTUATOR_EXEC_Z_ACC      100
+#define ACTUATOR_EXEC_Z_ACC      150
 #endif
 
-/** 运动到位等待裕量 (ms): 无到位反馈, 在计算运动时间上叠加的安全余量 */
+/** 到位等待超时兜底裕量 (ms): 闭环到位轮询的硬超时上限 (纯运动时间 + 此裕量);
+    正常到位会提前返回, 此裕量仅作保底防卡死 */
 #ifndef ACTUATOR_SETTLE_MARGIN_MS
 #define ACTUATOR_SETTLE_MARGIN_MS  400
 #endif
 
-/** 舵机动作 / 磁铁吸合后的等待时间 (ms) */
+/** 到位判定容差 (原始单位, Emm 固件 0~65535 = 360°; 1.5° ≈ 273)
+    (2° 太松会减速途中误判"已到位"提前下一步; 若某轴一直超时可调大) */
+#ifndef ACTUATOR_ARRIVAL_TOL_RAW
+#define ACTUATOR_ARRIVAL_TOL_RAW   273
+#endif
+
+/** 舵机动作 / 磁铁吸合后的等待时间 (ms) (实测优化: 300 → 150) */
 #ifndef ACTUATOR_GRIP_SETTLE_MS
-#define ACTUATOR_GRIP_SETTLE_MS    300
+#define ACTUATOR_GRIP_SETTLE_MS    150
 #endif
 
 /**
@@ -225,6 +235,31 @@ uint32_t actuator_mm_settle_ms(float distance_mm, uint16_t vel, uint8_t acc);
  */
 uint32_t actuator_angle_settle_ms(float angle_deg, uint16_t vel, uint8_t acc);
 
+/**
+ * @brief  直线轴纯运动时间 (按实际 mm 距离换算脉冲, 不含裕量)
+ * @param  distance_mm  直线距离 (mm, 取绝对值)
+ * @param  vel          速度 RPM
+ * @param  acc          加速度档位 0~255
+ */
+uint32_t actuator_mm_motion_time_ms(float distance_mm, uint16_t vel, uint8_t acc);
+
+/**
+ * @brief  Z 旋转轴纯运动时间 (按实际角度换算脉冲, 不含裕量)
+ * @param  angle_deg  旋转角度 (°, 取绝对值)
+ * @param  vel        速度 RPM
+ * @param  acc        加速度档位 0~255
+ */
+uint32_t actuator_angle_motion_time_ms(float angle_deg, uint16_t vel, uint8_t acc);
+
+/**
+ * @brief  等待指定电机到位 (轮询闭环 S_FLAG 到位标志)
+ * @param  base_wait_ms  纯运动时间估算 (兜底超时基准, 超时=纯运动时间+裕量)
+ * @param  moved_mask    实际有行程的电机位掩码:
+ *                       bit0=Z旋转, bit1=X, bit2=Y1, bit3=Y2
+ * @return true=全部到位, false=超时 (已按最坏情况等待)
+ */
+bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask);
+
 /**
  * @brief  设置 Z 旋转轴心 ↔ 工具末端 XY 偏移量 (mm)
  * @param  offset_x_mm  工具末端相对于旋转中心的 X 偏移 (mm)

components/emm_v5/emm_v5.c

@@ -127,6 +127,94 @@ void Emm_V5_Read_Sys_Params(uint8_t addr, EmmSysParams_t s)
     Emm_V5_Send(cmd, i);
 }
 
+/* ============================================================
+ * 读取电机状态标志 (S_FLAG 0x3A) 并解析返回
+ * ============================================================ */
+
+uint8_t Emm_V5_Read_Status_Flag(uint8_t addr, uint32_t timeout_ms)
+{
+    uint8_t rx[8] = {0};
+    uint8_t cmd[3] = {addr, 0x3A, 0x6B};
+    uint8_t read_to = (timeout_ms > 0) ? timeout_ms : EMM_V5_RX_TIMEOUT_MS;
+
+    /* 清掉历史残留 (运动命令的 ACK/到位帧等), 防止误判 */
+    uart_flush_input(s_uart_port);
+
+    Emm_V5_Send(cmd, 3);
+
+    uint8_t n = 0;
+    int64_t last = esp_timer_get_time();
+    int64_t deadline = last + (int64_t)read_to * 1000;
+    while (esp_timer_get_time() < deadline) {
+        int len = uart_read_bytes(s_uart_port, &rx[n], 1, pdMS_TO_TICKS(5));
+        if (len > 0) {
+            if (n < sizeof(rx) - 1) n++;
+            last = esp_timer_get_time();
+            /* 帧收完整 (地址 + 3A + 状态 + 6B) 立即返回, 不再等空闲超时,
+               把单次轮询从 ~17ms 压到 ~3ms, 减小到位检测抖动 */
+            for (int i = 0; i + 3 < (int)n; i++) {
+                if (rx[i] == addr && rx[i + 1] == 0x3A && rx[i + 3] == 0x6B)
+                    return rx[i + 2];
+            }
+        } else if ((esp_timer_get_time() - last) > 5000) {
+            break;   /* 5ms 无新字节: 认为帧已收完/无响应 */
+        }
+    }
+    return 0xFF;
+}
+
+/* ============================================================
+ * 读取电机位置 (S_TPOS 0x33 / S_CPOS 0x36) 并解析返回
+ *
+ * 返回帧: 地址 + 功能码 + 符号 + 位置(4B) + 6B (共 8 字节)
+ * Emm 固件: 0~65535 对应 0~360°
+ * ============================================================ */
+
+static int32_t Emm_V5_Read_Pos(uint8_t addr, uint8_t func)
+{
+    uint8_t rx[16] = {0};
+    uint8_t cmd[3] = {addr, func, 0x6B};
+    uint8_t read_to = EMM_V5_RX_TIMEOUT_MS;
+
+    /* 清掉历史残留 (ACK/到位帧等), 防止误判 */
+    uart_flush_input(s_uart_port);
+
+    Emm_V5_Send(cmd, 3);
+
+    uint8_t n = 0;
+    int64_t last = esp_timer_get_time();
+    int64_t deadline = last + (int64_t)read_to * 1000;
+    while (esp_timer_get_time() < deadline) {
+        int len = uart_read_bytes(s_uart_port, &rx[n], 1, pdMS_TO_TICKS(5));
+        if (len > 0) {
+            if (n < sizeof(rx) - 1) n++;
+            last = esp_timer_get_time();
+            /* 帧收完整 (地址+功能码+符号+位置4B+6B) 立即返回 */
+            for (int i = 0; i + 7 < (int)n; i++) {
+                if (rx[i] == addr && rx[i + 1] == func &&
+                    (rx[i + 2] == 0 || rx[i + 2] == 1) && rx[i + 7] == 0x6B)
+                    return (int32_t)(((uint32_t)rx[i + 3] << 24) |
+                                     ((uint32_t)rx[i + 4] << 16) |
+                                     ((uint32_t)rx[i + 5] << 8) |
+                                     (uint32_t)rx[i + 6]);
+            }
+        } else if ((esp_timer_get_time() - last) > 5000) {
+            break;   /* 5ms 无新字节: 认为帧已收完/无响应 */
+        }
+    }
+    return -1;
+}
+
+int32_t Emm_V5_Read_TPOS(uint8_t addr)
+{
+    return Emm_V5_Read_Pos(addr, 0x33);
+}
+
+int32_t Emm_V5_Read_CPOS(uint8_t addr)
+{
+    return Emm_V5_Read_Pos(addr, 0x36);
+}
+
 void Emm_V5_Modify_Ctrl_Mode(uint8_t addr, bool svF, uint8_t ctrl_mode)
 {
     uint8_t cmd[6] = {addr, 0x46, 0x69, (uint8_t)svF, ctrl_mode, 0x6B};

components/emm_v5/emm_v5.h

@@ -91,6 +91,30 @@ void Emm_V5_Reset_Clog_Pro(uint8_t addr);
  */
 void Emm_V5_Read_Sys_Params(uint8_t addr, EmmSysParams_t s);
 
+/**
+ * @brief  读取电机状态标志 (S_FLAG, 功能码 0x3A) 并解析返回
+ * @param  addr        电机地址
+ * @param  timeout_ms  等待返回超时 (ms), 0 使用默认 EMM_V5_RX_TIMEOUT_MS
+ * @return 状态标志字节, 读取/解析失败返回 0xFF
+ * @note   位定义: bit0=Ens_TF 使能, bit1=Prf_TF 到位, bit2=Cgi_TF 堵转,
+ *         bit3=Cgp_TF 堵转保护, bit4/bit5=左/右限位, bit7=Oac_TF 掉电
+ */
+uint8_t Emm_V5_Read_Status_Flag(uint8_t addr, uint32_t timeout_ms);
+
+/**
+ * @brief  读取电机目标位置 (S_TPOS, 功能码 0x33) 并解析返回
+ * @param  addr  电机地址
+ * @return 目标位置 (Emm 固件: 0~65535 对应 0~360°), 读取/解析失败返回 -1
+ */
+int32_t Emm_V5_Read_TPOS(uint8_t addr);
+
+/**
+ * @brief  读取电机实时位置 (S_CPOS, 功能码 0x36) 并解析返回
+ * @param  addr  电机地址
+ * @return 实时位置 (Emm 固件: 0~65535 对应 0~360°), 读取/解析失败返回 -1
+ */
+int32_t Emm_V5_Read_CPOS(uint8_t addr);
+
 /**
  * @brief  修改开环/闭环控制模式
  * @param  addr       电机地址

main/main.c

@@ -41,7 +41,7 @@ static const char *TAG = "MAIN";
 #define RECOGNIZE_TIMEOUT_MS  20000
 #define RECOGNIZE_POLL_MS     100
 /* 归零速度: XY 用 ACTUATOR_EXEC_VEL/ACC (3000/250), Z 用 ACTUATOR_EXEC_Z_VEL/Z_ACC (低速);
-   等待时间按最大行程 + 速度/加速度实时计算 */
+   到位等待: 轮询闭环到位标志, 超时按最大行程 + 裕量兜底 */
 
 static void button_recognize_task(void *pvParam)
 {
@@ -50,28 +50,32 @@ static void button_recognize_task(void *pvParam)
             ESP_LOGI(TAG, "按钮按下, 归零后再发送 RECOGNIZE ...");
             buzzer_beep(2000, 100);   /* 短音: 识别开始 */
 
-            /* ---- 归零: 舵机抬起 + Z 回 0° (低速, 最坏行程 270°) ---- */
+            /* ---- 归零: 舵机抬起 + Z/X/Y 三轴并行回零 ----
+               Z 旋转与 X/Y 平移互不干扰 (Fragment 里也是三轴并行),
+               无需等 Z 到位再动 X/Y —— 三轴同时下发、一次到位等待 */
             actuator_servo_up();
+
+            uint32_t z_motion  = actuator_angle_motion_time_ms(ACTUATOR_ANGLE_LIMIT_DEG,
+                                                               ACTUATOR_EXEC_Z_VEL,
+                                                               ACTUATOR_EXEC_Z_ACC);
+            uint32_t xy_motion = actuator_mm_motion_time_ms(ACTUATOR_LIMIT_X_MAX,
+                                                            ACTUATOR_EXEC_VEL,
+                                                            ACTUATOR_EXEC_ACC);
+            uint32_t y_motion  = actuator_mm_motion_time_ms(ACTUATOR_LIMIT_Y_MAX,
+                                                            ACTUATOR_EXEC_VEL,
+                                                            ACTUATOR_EXEC_ACC);
+            uint32_t home_motion = z_motion;
+            if (xy_motion > home_motion) home_motion = xy_motion;
+            if (y_motion  > home_motion) home_motion = y_motion;
+
             actuator_motor_angle(ACTUATOR_ADDR_ANGLE, 0.0f,
                                  ACTUATOR_EXEC_Z_VEL, ACTUATOR_EXEC_Z_ACC, true);
-            vTaskDelay(pdMS_TO_TICKS(actuator_angle_settle_ms(
-                ACTUATOR_ANGLE_LIMIT_DEG,
-                ACTUATOR_EXEC_Z_VEL, ACTUATOR_EXEC_Z_ACC)));
-
-            /* ---- XY 回零: X=0, Y=330 (按最坏行程实时计算等待) ---- */
-            uint32_t xy_settle = actuator_mm_settle_ms(ACTUATOR_LIMIT_X_MAX,
-                                                        ACTUATOR_EXEC_VEL,
-                                                        ACTUATOR_EXEC_ACC);
-            uint32_t y_settle  = actuator_mm_settle_ms(ACTUATOR_LIMIT_Y_MAX,
-                                                        ACTUATOR_EXEC_VEL,
-                                                        ACTUATOR_EXEC_ACC);
-            if (y_settle > xy_settle) xy_settle = y_settle;
-
             actuator_motor_mm(ACTUATOR_ADDR_X, 0.0f,
                               ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC, true);
             actuator_motor_mm(ACTUATOR_ADDR_Y1, 330.0f,
                               ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC, true);
-            vTaskDelay(pdMS_TO_TICKS(xy_settle));
+            actuator_wait_arrival(home_motion,
+                                  (1u << 0) | (1u << 1) | (1u << 2) | (1u << 3));
 
             ESP_LOGI(TAG, "归零完成, 发送 RECOGNIZE ...");