release v1.0

components/actuator/actuator.c

@@ -50,17 +50,78 @@ static inline void _frame_gap(void)
     vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS));
 }
 
-/* 内部: 带 RGB 闪烁的延时 (周期 ms, 亮灭各半) */
+/* 内部: 带 RGB 闪烁的延时 (周期 ms, 亮灭各半; 精确到系统 tick) */
 static void _wait_flash(uint32_t ms, uint8_t r, uint8_t g, uint8_t b, uint32_t period_ms)
 {
-    uint32_t half = pdMS_TO_TICKS(period_ms / 2);
+    TickType_t total = pdMS_TO_TICKS(ms);
-    int cycles = (int)(ms / period_ms);
+    TickType_t half  = pdMS_TO_TICKS(period_ms / 2);
-    for (int i = 0; i < cycles; i++) {
+    if (half == 0) half = 1;
-        rgb_set(r, g, b);
+    bool led_on = true;
-        vTaskDelay(half);
+
-        rgb_off();
+    while (total > 0) {
-        vTaskDelay(half);
+        TickType_t chunk = (total > half) ? half : total;
+        if (led_on) rgb_set(r, g, b);
+        else        rgb_off();
+        vTaskDelay(chunk);
+        total -= chunk;
+        led_on = !led_on;
     }
+    rgb_off();
+}
+
+/* ================================================================
+ * 运动到位时间估算
+ *
+ * Emm_V5 曲线加减速 (官方文档):
+ *   t2 - t1 = (256 - acc) * 50(us), Vt2 = Vt1 + 1(RPM)
+ * 即每提升 1 RPM 耗时 (256-acc)*50us, 按梯形/三角形加减速估算。
+ * ================================================================ */
+
+uint32_t actuator_motion_time_ms(uint32_t pulses, uint16_t vel, uint8_t acc)
+{
+    if (pulses == 0 || vel == 0) return 0;
+
+    float revs  = (float)pulses / (float)EMM_V5_PULSES_PER_REV;
+    float v_rps = (float)vel / 60.0f;
+    float t_ms;
+
+    if (acc == 0) {
+        /* acc=0: 无曲线加减速, 直接按设定速度恒速运行 */
+        t_ms = revs / v_rps * 1000.0f;
+    } else {
+        /* 从 0 加速到目标转速的时间与加速段距离 (线性曲线近似) */
+        float t_ramp_s   = (float)vel * (float)(256 - acc) * 50e-6f;
+        float a_rps2     = v_rps / t_ramp_s;
+        float d_ramp_rev = 0.5f * a_rps2 * t_ramp_s * t_ramp_s;
+
+        if (revs <= 2.0f * d_ramp_rev) {
+            /* 行程不足, 未到目标转速即减速 → 三角形 */
+            t_ms = 2.0f * sqrtf(revs / a_rps2) * 1000.0f;
+        } else {
+            /* 加速 + 恒速 + 减速 */
+            t_ms = (2.0f * t_ramp_s + (revs - 2.0f * d_ramp_rev) / v_rps) * 1000.0f;
+        }
+    }
+    return (uint32_t)(t_ms + 0.999f);
+}
+
+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)
+{
+    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);
+}
+
+uint32_t actuator_angle_settle_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_settle_ms(pulses, vel, acc);
 }
 
 /* ================================================================
@@ -279,11 +340,12 @@ static void _actuator_move_y_sync(uint32_t pulses_y, uint16_t vel, uint8_t acc)
 
 static void _actuator_move_fragment(const edc_fragment_t *frag)
 {
-    /* 固定速度/加速度 */
+    /* XY 执行速度/加速度 (3000RPM + 250 档, 实测稳定);
-    uint16_t vel_xy    = 1500;
+       Z 旋转轴保持低速 (见 ACTUATOR_EXEC_Z_VEL/Z_ACC) */
-    uint8_t  acc_xy    = 200;
+    uint16_t vel_xy    = ACTUATOR_EXEC_VEL;
-    uint16_t vel_angle = 50;
+    uint8_t  acc_xy    = ACTUATOR_EXEC_ACC;
-    uint8_t  acc_angle = 100;
+    uint16_t vel_angle = ACTUATOR_EXEC_Z_VEL;
+    uint8_t  acc_angle = ACTUATOR_EXEC_Z_ACC;
 
     /* ---- 提取 source / target 坐标 (可被补偿修改) ---- */
     uint16_t src_x     = frag->source.x;
@@ -316,6 +378,17 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
              (double)tgt_angle_deg,
              tgt_x, tgt_y);
 
+    /* 按实际行程/速度/加速度计算到位等待 (Z/X/Y 并发, 取最长者, 含安全裕量) */
+    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)(fabsf(src_target) * 10.0f * s_scale_angle + 0.5f),
+        vel_angle, acc_angle);
+    uint32_t t_wait = actuator_settle_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);
+    if (t_wait > src_wait_ms) src_wait_ms = t_wait;
+
     /* ---- 1. 磁铁掉电 ---- */
     magnet_set(false);
 
@@ -327,26 +400,34 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
     _frame_gap();
     _actuator_move_y_sync(src_pulses_y, vel_xy, acc_xy);
 
-    /* ---- 3. 等待 4s 运动到位 (红灯快闪) ---- */
+    /* ---- 3. 等待运动到位 (红灯快闪, 按行程实时计算) ---- */
-    _wait_flash(4000, 128, 0, 0, 200);
+    _wait_flash(src_wait_ms, 128, 0, 0, 200);
 
     /* ---- 4. 舵机放下 ---- */
     servo_set_down();
-    _wait_flash(1000, 128, 0, 0, 200);
+    _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
 
     /* ---- 5. 磁铁吸合 ---- */
     magnet_set(true);
-    _wait_flash(1000, 128, 0, 0, 200);
+    _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
 
     /* ---- 6. 舵机抬起 ---- */
     servo_set_up();
-    _wait_flash(1000, 128, 0, 0, 200);
+    _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
 
     /* ---- 7. 运动到 target 位置 ---- */
     /* 目标角度取行程内与当前位置最近的等价角, 再用相对运动执行:
        终点朝向与旧协议 (绝对转到 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)(fabsf(dst_rep - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f),
+        vel_angle, acc_angle);
+    t_wait = actuator_settle_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);
+    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);
     _frame_gap();
     _actuator_move_abs(ACTUATOR_ADDR_X, ACTUATOR_DIR_X,
@@ -354,20 +435,20 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
     _frame_gap();
     _actuator_move_y_sync(dst_pulses_y, vel_xy, acc_xy);
 
-    /* ---- 8. 等待 4s 运动到位 (红灯快闪) ---- */
+    /* ---- 8. 等待运动到位 (红灯快闪, 按行程实时计算) ---- */
-    _wait_flash(4000, 128, 0, 0, 200);
+    _wait_flash(dst_wait_ms, 128, 0, 0, 200);
 
     /* ---- 9. 舵机放下 ---- */
     servo_set_down();
-    _wait_flash(1000, 128, 0, 0, 200);
+    _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
 
     /* ---- 10. 磁铁掉电 ---- */
     magnet_set(false);
-    _wait_flash(1000, 128, 0, 0, 200);
+    _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
 
     /* ---- 11. 舵机抬起 ---- */
     servo_set_up();
-    _wait_flash(1000, 128, 0, 0, 200);
+    _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
 }
 
 /* ================================================================

components/actuator/actuator.h

@@ -106,9 +106,30 @@ extern "C" {
 #define ACTUATOR_ANGLE_LIMIT_DEG  270.0f
 #endif
 
-/** Fragment 间等待 (ms), 暂用固定延时替代到位检测 */
+/** 执行 Cycle / 归零时 XY 轴的速度与加速度档位 (实测 3000RPM + 250 档稳定) */
-#ifndef ACTUATOR_FRAG_DELAY_MS
+#ifndef ACTUATOR_EXEC_VEL
-#define ACTUATOR_FRAG_DELAY_MS   500
+#define ACTUATOR_EXEC_VEL        3000
+#endif
+#ifndef ACTUATOR_EXEC_ACC
+#define ACTUATOR_EXEC_ACC        250
+#endif
+
+/** Z 旋转轴执行速度与加速度 (结构刚性有限, 必须保持低速; 300RPM 已偏快) */
+#ifndef ACTUATOR_EXEC_Z_VEL
+#define ACTUATOR_EXEC_Z_VEL      50
+#endif
+#ifndef ACTUATOR_EXEC_Z_ACC
+#define ACTUATOR_EXEC_Z_ACC      100
+#endif
+
+/** 运动到位等待裕量 (ms): 无到位反馈, 在计算运动时间上叠加的安全余量 */
+#ifndef ACTUATOR_SETTLE_MARGIN_MS
+#define ACTUATOR_SETTLE_MARGIN_MS  400
+#endif
+
+/** 舵机动作 / 磁铁吸合后的等待时间 (ms) */
+#ifndef ACTUATOR_GRIP_SETTLE_MS
+#define ACTUATOR_GRIP_SETTLE_MS    300
 #endif
 
 /**
@@ -137,10 +158,10 @@ extern "C" {
  * 待机械测量后填入。
  * ================================================================ */
 #ifndef ACTUATOR_OFFSET_X_MM
-#define ACTUATOR_OFFSET_X_MM     8.0f    /* angle=0° 时工具末端偏右 ~8mm (实测) */
+#define ACTUATOR_OFFSET_X_MM     6.6f    /* angle=0° 时工具末端偏右 ~8mm (实测) */
 #endif
 #ifndef ACTUATOR_OFFSET_Y_MM
-#define ACTUATOR_OFFSET_Y_MM     -30.0f  /* angle=0° 时工具末端偏上 ~30mm (实测) */
+#define ACTUATOR_OFFSET_Y_MM     -35.75f  /* angle=0° 时工具末端偏上 ~30mm (实测) */
 #endif
 
 /**
@@ -171,6 +192,39 @@ void actuator_init(void);
  */
 void actuator_set_scale(float x_scale, float y_scale, float angle_scale);
 
+/**
+ * @brief  按 Emm_V5 曲线加减速公式估算纯运动时间 (ms)
+ * @param  pulses  脉冲数 (行程)
+ * @param  vel     速度 RPM
+ * @param  acc     加速度档位 0~255 (0=直接启动, 无曲线)
+ * @note   曲线公式: 每提升 1 RPM 耗时 (256-acc)*50us, 按梯形/三角形加减速估算
+ */
+uint32_t actuator_motion_time_ms(uint32_t pulses, uint16_t vel, uint8_t acc);
+
+/**
+ * @brief  运动到位等待 = 估算运动时间 + 安全裕量
+ * @param  pulses  脉冲数 (行程)
+ * @param  vel     速度 RPM
+ * @param  acc     加速度档位 0~255
+ */
+uint32_t actuator_settle_ms(uint32_t pulses, uint16_t vel, uint8_t acc);
+
+/**
+ * @brief  直线轴运动到位等待 (按实际 mm 距离换算脉冲)
+ * @param  distance_mm  直线距离 (mm, 取绝对值)
+ * @param  vel          速度 RPM
+ * @param  acc          加速度档位 0~255
+ */
+uint32_t actuator_mm_settle_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_settle_ms(float angle_deg, uint16_t vel, uint8_t acc);
+
 /**
  * @brief  设置 Z 旋转轴心 ↔ 工具末端 XY 偏移量 (mm)
  * @param  offset_x_mm  工具末端相对于旋转中心的 X 偏移 (mm)

main/main.c

@@ -38,9 +38,8 @@ 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 (低速);
-#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,15 +47,28 @@ static void button_recognize_task(void *pvParam)
         if (key_is_pressed()) {
             ESP_LOGI(TAG, "按钮按下, 归零后再发送 RECOGNIZE ...");
 
-            /* ---- 归零: 舵机抬起 + Z 回 0° ---- */
+            /* ---- 归零: 舵机抬起 + Z 回 0° (低速, 最坏行程 270°) ---- */
             actuator_servo_up();
-            actuator_motor_angle(ACTUATOR_ADDR_ANGLE, 0.0f, 50, 100, true);
+            actuator_motor_angle(ACTUATOR_ADDR_ANGLE, 0.0f,
-            vTaskDelay(pdMS_TO_TICKS(HOME_Z_SETTLE_MS));
+                                 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 (vel=1500 acc=200) ---- */
+            /* ---- XY 回零: X=0, Y=330 (按最坏行程实时计算等待) ---- */
-            actuator_motor_mm(ACTUATOR_ADDR_X, 0.0f, 1500, 200, true);
+            uint32_t xy_settle = actuator_mm_settle_ms(ACTUATOR_LIMIT_X_MAX,
-            actuator_motor_mm(ACTUATOR_ADDR_Y1, 330.0f, 1500, 200, true);
+                                                        ACTUATOR_EXEC_VEL,
-            vTaskDelay(pdMS_TO_TICKS(HOME_XY_SETTLE_MS));
+                                                        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));
 
             ESP_LOGI(TAG, "归零完成, 发送 RECOGNIZE ...");