actuator应用层基本完成

components/emm_v5/emm_v5.c

@@ -5,6 +5,7 @@
 
 #include "emm_v5.h"
 #include <string.h>
+#include <math.h>
 #include "freertos/FreeRTOS.h"
 #include "freertos/task.h"
 #include "esp_timer.h"
@@ -161,6 +162,18 @@ void Emm_V5_Pos_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc,
     Emm_V5_Send(cmd, 13);
 }
 
+void Emm_V5_Pos_Control_Angle(uint8_t addr, uint8_t dir, uint16_t vel,
+                              uint8_t acc, float angle_deg, bool raF, bool snF)
+{
+    /* 负角度 → 取绝对值 + 翻转方向 */
+    float    abs_deg = fabsf(angle_deg);
+    uint32_t clk     = EMM_V5_DEG_TO_CLK(abs_deg);
+    if (angle_deg < 0.0f) dir = (uint8_t)(!dir);
+    ESP_LOGD("EMM_V5", "角度→脉冲: %.1f° (%s) → %lu",
+             (double)angle_deg, dir ? "CCW" : "CW", (unsigned long)clk);
+    Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, snF);
+}
+
 void Emm_V5_Stop_Now(uint8_t addr, bool snF)
 {
     uint8_t cmd[5] = {addr, 0xFE, 0x98, (uint8_t)snF, 0x6B};