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

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       电机地址