修复: 偏移补偿双重扣减致放置 y 整体偏大 35.75mm
MPU calibrated_H 下发的是旋转中心坐标 (source 抓取未补偿且正常为证), 旧公式 center=target-R(theta)*offset 把 target 当工具末端坐标双重扣减, 工具末端恒定偏 (6.6,-35.75)mm。改为 center=target+(dx,dy)-R(theta)*offset, 各角度误差归零。
This commit is contained in:
1 parent
c3028a9433
commit
74010ef611
2 files changed
+19
-8
No files matched your search
@@ -338,9 +338,16 @@ void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y,
|
||||
float rot_dx = s_offset_x_mm * cos_t - s_offset_y_mm * sin_t;
|
||||
float rot_dy = s_offset_x_mm * sin_t + s_offset_y_mm * cos_t;
|
||||
|
||||
/* center = target - rotated_offset, 四舍五入 */
|
||||
float cx = (float)(*target_x) - rot_dx;
|
||||
float cy = (float)(*target_y) - rot_dy;
|
||||
/* ⚠️ MPU 下发的 target 来自 calibrated_H, 本身是"旋转中心"坐标
|
||||
(θ=0 时中心到 target, 工具末端即落在目标物点 D = target + (dx,dy);
|
||||
抓取 source 同样未补偿且工作正常可佐证)。
|
||||
因此任意转角 θ 下要把工具末端放到 D, 中心需去:
|
||||
center = D - R(θ)·(dx,dy) = target + (dx,dy) - R(θ)·(dx,dy)
|
||||
旧公式 center = target - R(θ)·(dx,dy) 把 target 当成工具末端坐标,
|
||||
双重扣减 offset, 导致工具末端恒定偏移 (dx,dy) = (6.6,-35.75)mm
|
||||
(y 整体偏大 35.75mm)。 */
|
||||
float cx = (float)(*target_x) + s_offset_x_mm - rot_dx;
|
||||
float cy = (float)(*target_y) + s_offset_y_mm - rot_dy;
|
||||
|
||||
/* 钳位到有效范围并写回 */
|
||||
if (cx < 0.0f) cx = 0.0f;
|
||||
|
||||
Reference in new issue
Block a user