执行机构角度就近取角与相对运动; 增加归零等待与协议说明

components/actuator/actuator.c

@@ -24,6 +24,9 @@ static float s_scale_x     = ACTUATOR_SCALE_X;
 static float s_scale_y     = ACTUATOR_SCALE_Y;
 static float s_scale_angle = ACTUATOR_SCALE_ANGLE;
 
+/* Z 轴当前命令位置 (度, 绝对), 初始与驱动零位一致; 用于行程内选近路 */
+static float s_cur_angle_deg = 0.0f;
+
 /* Z 旋转轴心 → 工具末端 偏移补偿 (mm) */
 static float s_offset_x_mm = ACTUATOR_OFFSET_X_MM;
 static float s_offset_y_mm = ACTUATOR_OFFSET_Y_MM;
@@ -92,6 +95,8 @@ void actuator_init(void)
     }
 
     s_initialized = true;
+    /* Z 轴命令位置跟踪与驱动零位对齐 (重新初始化时复位) */
+    s_cur_angle_deg = 0.0f;
     ESP_LOGI(TAG, "初始化完成 (Angle=addr%d X=addr%d Y=addr%d/%d, servo=GPIO%d)",
              ACTUATOR_ADDR_ANGLE,
              ACTUATOR_ADDR_X,
@@ -186,6 +191,68 @@ static void _actuator_move_abs(uint8_t addr, uint8_t dir, uint32_t pulses,
                        snF);           /* 多机同步标志 */
 }
 
+/* ================================================================
+ * 内部: Z 旋转轴角度运动 (绝对角, ±行程内选最近可达代表角)
+ * ================================================================ */
+
+/** 在 ±limit 行程内选择与当前位置最近的等价角 (mod 360) */
+static float _closest_reachable_angle(float target_deg, float cur_deg, float limit)
+{
+    target_deg = fmodf(target_deg, 360.0f);
+    if (target_deg < 0.0f)
+        target_deg += 360.0f;
+
+    float best = target_deg;
+    float best_dist = 1e30f;
+    for (int k = -1; k <= 1; k++) {
+        float rep = target_deg + 360.0f * (float)k;
+        if (rep < -limit || rep > limit)
+            continue;
+        float dist = fabsf(rep - cur_deg);
+        if (dist < best_dist - 1e-4f ||
+            (fabsf(dist - best_dist) <= 1e-4f && rep > best)) {
+            best_dist = dist;
+            best = rep;
+        }
+    }
+    return best;
+}
+
+/** Z 轴绝对运动到目标朝向 (用于 source 归位); 负角自动翻转方向, 并更新当前位置跟踪 */
+static void _actuator_move_angle_abs(float angle_deg, uint16_t vel, uint8_t acc)
+{
+    float target = _closest_reachable_angle(angle_deg, s_cur_angle_deg,
+                                            ACTUATOR_ANGLE_LIMIT_DEG);
+    uint8_t dir = ACTUATOR_DIR_ANGLE;
+    float abs_deg = fabsf(target);
+    uint32_t pulses = (uint32_t)(abs_deg * 10.0f * s_scale_angle + 0.5f);
+    if (target < 0.0f)
+        dir = (uint8_t)!dir;
+
+    _actuator_move_abs(ACTUATOR_ADDR_ANGLE, dir, pulses, vel, acc, false);
+    s_cur_angle_deg = target;
+    ESP_LOGI(TAG, "Z 角度: 目标 %.1f° -> 最近可达 %.1f° (%lu pulses, %s)",
+             (double)angle_deg, (double)target, (unsigned long)pulses,
+             dir ? "CCW" : "CW");
+}
+
+/** Z 轴相对运动 delta (带符号度): 正负自动选方向, 语义明确, 用于片间旋转 */
+static void _actuator_move_angle_rel(float delta_deg, uint16_t vel, uint8_t acc)
+{
+    uint8_t dir = ACTUATOR_DIR_ANGLE;
+    float abs_deg = fabsf(delta_deg);
+    uint32_t pulses = (uint32_t)(abs_deg * 10.0f * s_scale_angle + 0.5f);
+    if (delta_deg < 0.0f)
+        dir = (uint8_t)!dir;
+
+    Emm_V5_Pos_Control(ACTUATOR_ADDR_ANGLE, dir, vel, acc, pulses,
+                       false,           /* raF=false: 相对运动 */
+                       false);
+    s_cur_angle_deg += delta_deg;
+    ESP_LOGI(TAG, "Z 角度: 相对 %.1f° (%lu pulses, %s)",
+             (double)delta_deg, (unsigned long)pulses, dir ? "CCW" : "CW");
+}
+
 /* ================================================================
  * 内部: Y 轴双电机同步运动
  * ================================================================ */
@@ -226,31 +293,34 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
     uint16_t tgt_y     = frag->target.y;
     uint16_t tgt_angle = frag->target.angle_x10;
 
-    /* Z 旋转轴心偏移补偿 (仅对 target 做) */
+    /* 协议角度: source=0 (保持当前), target=相对转角 [0,360);
+       执行时自动选行程内与当前位置最近的等价角 */
+    float src_angle_deg = (float)src_angle * 0.1f;
     float tgt_angle_deg = (float)tgt_angle * 0.1f;
+
+    /* Z 旋转轴心偏移补偿 (仅对 target 做, 与旧协议行为保持一致) */
     actuator_apply_offset_compensation(&tgt_x, &tgt_y, tgt_angle_deg);
 
     /* source 坐标 → 脉冲数 */
     uint32_t src_pulses_x     = (uint32_t)(src_x     * s_scale_x);
     uint32_t src_pulses_y     = (uint32_t)(src_y     * s_scale_y);
-    uint32_t src_pulses_angle = (uint32_t)(src_angle * s_scale_angle);
 
     /* target 坐标 → 脉冲数 */
     uint32_t dst_pulses_x     = (uint32_t)(tgt_x     * s_scale_x);
     uint32_t dst_pulses_y     = (uint32_t)(tgt_y     * s_scale_y);
-    uint32_t dst_pulses_angle = (uint32_t)(tgt_angle * s_scale_angle);
 
-    ESP_LOGI(TAG, "Fragment: SRC(X=%lu Y=%lu A=%lu) -> DST(X=%lu Y=%lu A=%lu comp x=%u y=%u)",
-             src_pulses_x, src_pulses_y, src_pulses_angle,
-             dst_pulses_x, dst_pulses_y, dst_pulses_angle,
+    ESP_LOGI(TAG, "Fragment: SRC(X=%lu Y=%lu A=%.1f) -> DST(X=%lu Y=%lu A=%.1f comp x=%u y=%u)",
+             (unsigned long)src_pulses_x, (unsigned long)src_pulses_y,
+             (double)src_angle_deg,
+             (unsigned long)dst_pulses_x, (unsigned long)dst_pulses_y,
+             (double)tgt_angle_deg,
              tgt_x, tgt_y);
 
     /* ---- 1. 磁铁掉电 ---- */
     magnet_set(false);
 
     /* ---- 2. 运动到 source 位置 ---- */
-    _actuator_move_abs(ACTUATOR_ADDR_ANGLE, ACTUATOR_DIR_ANGLE,
-                       src_pulses_angle, vel_angle, acc_angle, false);
+    _actuator_move_angle_abs(src_angle_deg, vel_angle, acc_angle);
     _frame_gap();
     _actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X,
                        src_pulses_x, vel_xy, acc_xy, false);
@@ -273,8 +343,11 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
     _wait_flash(1000, 128, 0, 0, 200);
 
     /* ---- 7. 运动到 target 位置 ---- */
-    _actuator_move_abs(ACTUATOR_ADDR_ANGLE, ACTUATOR_DIR_ANGLE,
-                       dst_pulses_angle, vel_angle, acc_angle, false);
+    /* 目标角度取行程内与当前位置最近的等价角, 再用相对运动执行:
+       终点朝向与旧协议 (绝对转到 rotate mod 360) 完全一致, 且不会绕远路 */
+    float dst_rep = _closest_reachable_angle(tgt_angle_deg, s_cur_angle_deg,
+                                             ACTUATOR_ANGLE_LIMIT_DEG);
+    _actuator_move_angle_rel(dst_rep - s_cur_angle_deg, vel_angle, acc_angle);
     _frame_gap();
     _actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X,
                        dst_pulses_x, vel_xy, acc_xy, false);
@@ -480,6 +553,9 @@ void actuator_motor_angle(uint8_t addr, float angle_deg, uint16_t vel,
                  addr, peer, (double)angle_deg,
                  raF ? "绝对" : "相对", vel);
     } else {
+        /* 跟踪 Z 轴命令位置 (供 fragment 行程内近路选择) */
+        if (addr == ACTUATOR_ADDR_ANGLE)
+            s_cur_angle_deg = raF ? angle_deg : (s_cur_angle_deg + angle_deg);
         Emm_V5_Pos_Control_Angle(addr, dir, vel, acc, angle_deg, raF, false);
         ESP_LOGI(TAG, "角度: 电机%d %.1f° %s vel=%dRPM",
                  addr, (double)angle_deg,

components/actuator/actuator.h

@@ -101,6 +101,11 @@ extern "C" {
 #define ACTUATOR_LIMIT_Y_MIN       0.0f
 #define ACTUATOR_LIMIT_Y_MAX       335.0f
 
+/** Z 旋转轴软限位 (±°, 机械行程) */
+#ifndef ACTUATOR_ANGLE_LIMIT_DEG
+#define ACTUATOR_ANGLE_LIMIT_DEG  270.0f
+#endif
+
 /** Fragment 间等待 (ms), 暂用固定延时替代到位检测 */
 #ifndef ACTUATOR_FRAG_DELAY_MS
 #define ACTUATOR_FRAG_DELAY_MS   500
@@ -132,10 +137,10 @@ extern "C" {
  * 待机械测量后填入。
  * ================================================================ */
 #ifndef ACTUATOR_OFFSET_X_MM
-#define ACTUATOR_OFFSET_X_MM     8.0f    /* angle=0° 时工具末端偏右 ~8mm */
+#define ACTUATOR_OFFSET_X_MM     8.0f    /* angle=0° 时工具末端偏右 ~8mm (实测) */
 #endif
 #ifndef ACTUATOR_OFFSET_Y_MM
-#define ACTUATOR_OFFSET_Y_MM     -30.0f   /* angle=0° 时工具末端偏上 ~30mm */
+#define ACTUATOR_OFFSET_Y_MM     -30.0f  /* angle=0° 时工具末端偏上 ~30mm (实测) */
 #endif
 
 /**

components/protocol/PROTOCOL.md

@@ -23,6 +23,10 @@ Cycle     = { cycle_id: uint16,  frag_count: uint8 (2~4),  fragments: Fragment[]
 ```
 
 - **Point**: 单个坐标点, `angle_x10 = 角度 × 10` (如 `450` = 45.0°)
+- **角度语义**: `source.angle = 0` 表示保持当前朝向; `target.angle` 为搬运
+  过程中的**相对转角**, 归一化到 `[0, 360.0)` 度 (如 -20° 编码为 3400)。
+  MCU 执行 fragment 时在 ±270° 行程内为 target 选择与当前位置最近的等价角
+  (mod 360°), 就近旋转, 避免把 -20° 当成 +340° 绕远路
 - **Fragment**: 一次运动片段, 从 `source` 移动到 `target`
 - **Cycle**: 一个执行周期, 包含 2~4 个 Fragment
 
@@ -98,10 +102,10 @@ XX XX                                    ← CRC16
 |------|-----|------|
 | cycle_id | `0x0001` = 1 | 第 1 个 Cycle |
 | frag_count | `0x02` = 2 | 2 个 Fragment |
-| F0 src | (100, 200), 45.0° | |
-| F0 dst | (300, 400), 60.0° | |
-| F1 src | (150, 300), 90.0° | |
-| F1 dst | (500, 700), 120.0° | |
+| F0 src | (100, 200), 45.0° | 示例: 抓取点 (角度字段保持当前) |
+| F0 dst | (300, 400), 60.0° | 示例: 放置点 + 相对转角 60.0° |
+| F1 src | (150, 300), 90.0° | 示例: 抓取点 (角度字段保持当前) |
+| F1 dst | (500, 700), 120.0° | 示例: 放置点 + 相对转角 120.0° |
 
 ### 4.2 CYCLE_DONE (MCU → MPU)
 

main/main.c

@@ -38,6 +38,9 @@ static const char *TAG = "MAIN";
 /* ---- 按钮识别任务 ---- */
 #define RECOGNIZE_TIMEOUT_MS  20000
 #define RECOGNIZE_POLL_MS     100
+/* 归零后等待电机运动到位的时间 (无到位反馈, 按最大行程 + 裕量给定时长) */
+#define HOME_Z_SETTLE_MS      1500   /* Z 回 0°: vel=50RPM, 最大 ~270° 约 0.9s */
+#define HOME_XY_SETTLE_MS     4000   /* XY 回零: vel=1500RPM, Y 最大 335mm 约 2s */
 
 static void button_recognize_task(void *pvParam)
 {
@@ -48,12 +51,12 @@ static void button_recognize_task(void *pvParam)
             /* ---- 归零: 舵机抬起 + Z 回 0° ---- */
             actuator_servo_up();
             actuator_motor_angle(ACTUATOR_ADDR_ANGLE, 0.0f, 50, 100, true);
-            vTaskDelay(pdMS_TO_TICKS(500));
+            vTaskDelay(pdMS_TO_TICKS(HOME_Z_SETTLE_MS));
 
             /* ---- XY 回零: X=0, Y=330 (vel=1500 acc=200) ---- */
             actuator_motor_mm(ACTUATOR_ADDR_X, 0.0f, 1500, 200, true);
             actuator_motor_mm(ACTUATOR_ADDR_Y1, 330.0f, 1500, 200, true);
-            vTaskDelay(pdMS_TO_TICKS(2000));
+            vTaskDelay(pdMS_TO_TICKS(HOME_XY_SETTLE_MS));
 
             ESP_LOGI(TAG, "归零完成, 发送 RECOGNIZE ...");
 
@@ -136,4 +139,3 @@ void app_main(void)
         vTaskDelay(pdMS_TO_TICKS(500));
     }
 }
-