releasev1.2

components/actuator/actuator.h

@@ -106,6 +106,27 @@ extern "C" {
 #define ACTUATOR_ANGLE_LIMIT_DEG  270.0f
 #endif
 
+/** 置零 (归零) 目标位置 (Cycle 完成后 / 按钮识别前回位) */
+#ifndef ACTUATOR_HOME_X_MM
+#define ACTUATOR_HOME_X_MM       0.0f    /* X 轴回零位置 (mm) */
+#endif
+#ifndef ACTUATOR_HOME_Y_MM
+#define ACTUATOR_HOME_Y_MM       330.0f  /* Y 轴回零位置 (mm, 接近行程上限, 与原按钮归零一致) */
+#endif
+#ifndef ACTUATOR_HOME_ANGLE_DEG
+#define ACTUATOR_HOME_ANGLE_DEG  0.0f    /* Z 旋转轴回零角度 (°) */
+#endif
+
+/** Z 轴回 0° 到位确认参数: 电机1 (Z 轴) 对“终点靠近 0°”的移动不回传到位帧
+    (实测 0.1°/0.5° 垫高都不触发) → 用 Emm_V5_Read_CPOS 轮询确认到位:
+    timeout_ms = 轮询超时 (Z 回零移动小, 通常已被 X/Y 等待覆盖), tol = 容差 (°) */
+#ifndef ACTUATOR_Z_CONFIRM_MS
+#define ACTUATOR_Z_CONFIRM_MS      1000
+#endif
+#ifndef ACTUATOR_Z_CONFIRM_TOL_DEG
+#define ACTUATOR_Z_CONFIRM_TOL_DEG 2.0f
+#endif
+
 /** 执行 Cycle / 归零时 XY 轴的速度与加速度档位
     2026-08-01 提速: 3000RPM → 3500RPM (加速度保持 250 档, 实测稳定)
     注: 手册 FD 位置模式速度标称上限 0-3000RPM, 3500 超出文档范围需实测 */
@@ -126,9 +147,11 @@ extern "C" {
 #endif
 
 /** 到位等待超时兜底裕量 (ms): 闭环到位轮询的硬超时上限 (纯运动时间 + 此裕量);
-    正常到位会提前返回, 此裕量仅作保底防卡死 */
+    正常到位会提前返回, 此裕量仅作保底防卡死。
+    2026-08-03: 400→1000 —— 大行程/高速运动后驱动闭环在 0.1° 窗口内稳定需要
+    额外时间, 400ms 裕量实测多次在到位帧到达前就超时 (电机3/4 迟到 ~0.5s) */
 #ifndef ACTUATOR_SETTLE_MARGIN_MS
-#define ACTUATOR_SETTLE_MARGIN_MS  400
+#define ACTUATOR_SETTLE_MARGIN_MS  1000
 #endif
 
 /** 到位判定容差 (原始单位, Emm 固件 0~65535 = 360°; 1.5° ≈ 273)
@@ -137,9 +160,9 @@ extern "C" {
 #define ACTUATOR_ARRIVAL_TOL_RAW   273
 #endif
 
-/** 舵机动作 / 磁铁吸合后的等待时间 (ms) (实测优化: 300 → 150) */
+/** 舵机动作 / 磁铁吸合后的等待时间 (ms) (150 偏短不稳, 300 略保守 → 200) */
 #ifndef ACTUATOR_GRIP_SETTLE_MS
-#define ACTUATOR_GRIP_SETTLE_MS    150
+#define ACTUATOR_GRIP_SETTLE_MS    200
 #endif
 
 /**
@@ -162,12 +185,14 @@ extern "C" {
  *
  * ⚠️ 语义 (2026-08-03 修正): MPU 下发的坐标 (source/target) 来自 calibrated_H,
  *    本身是"旋转中心"坐标 —— θ=0 时中心到坐标点, 工具末端即落在目标物点
- *    (source 抓取不补偿且工作正常可佐证)。因此 target 的补偿公式为:
- *      X_motor = X_tgt + dx - (dx * cosθ - dy * sinθ)
- *      Y_motor = Y_tgt + dy - (dx * sinθ + dy * cosθ)
- *    (即先把中心坐标换算回目标物点 D = target + (dx,dy),
+ *    D = 坐标 + (dx,dy)。source 与 target 都须按此补偿, 公式为:
+ *      X_motor = X + dx - (dx * cosθ - dy * sinθ)
+ *      Y_motor = Y + dy - (dx * sinθ + dy * cosθ)
+ *    (即先把中心坐标换算回目标物点 D = 坐标 + (dx,dy),
  *     再按转角 θ 反算旋转中心)。旧公式少了 "+ (dx,dy)" 项,
  *     会把工具末端整体放偏 (dx,dy) = (6.6, -35.75)mm (y 偏大 35.75mm)。
+ *    ⚠️ source 不补偿时, 抓取瞬间 Z 角 ≠ 0° 则吸盘偏移 (R(θ)-I)·(dx,dy),
+ *       y 分量 (35.75mm) 主导 → 抓取点偏 y (2026-08-03 已修: source 也补偿)。
  *
  * dx, dy 为工具末端相对于 Z 旋转中心的 XY 偏移 (mm)。
  * ================================================================ */
@@ -256,11 +281,12 @@ uint32_t actuator_mm_motion_time_ms(float distance_mm, uint16_t vel, uint8_t acc
 uint32_t actuator_angle_motion_time_ms(float angle_deg, uint16_t vel, uint8_t acc);
 
 /**
- * @brief  等待指定电机到位 (轮询闭环 S_FLAG 到位标志)
+ * @brief  等待指定电机到位 (事件驱动: 收到驱动主动回传的到位帧)
  * @param  base_wait_ms  纯运动时间估算 (兜底超时基准, 超时=纯运动时间+裕量)
  * @param  moved_mask    实际有行程的电机位掩码:
  *                       bit0=Z旋转, bit1=X, bit2=Y1, bit3=Y2
- * @return true=全部到位, false=超时 (已按最坏情况等待)
+ * @return true=全部到位, false=超时
+ * @note   依赖驱动 Response=Reached/Both/Other 回传到位帧 (地址+FD+9F+6B)
  */
 bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask);
 
@@ -297,6 +323,13 @@ void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y,
  */
 int actuator_execute_cycle(const edc_cycle_t *cycle);
 
+/**
+ * @brief  执行机构置零 (归位): 舵机抬起 + Z/X/Y 三轴并行回零点
+ * @note   Cycle 执行完成 / 按钮识别前调用; 内部含到位等待;
+ *         目标位置由 ACTUATOR_HOME_X_MM / Y_MM / ANGLE_DEG 配置
+ */
+void actuator_home_all(void);
+
 /**
  * @brief  急停全部 4 路电机
  */