From 74010ef611ffc5b2cb06728661e1e6d5347f6507 Mon Sep 17 00:00:00 2001 From: kirl <18827922006@163.com> Date: Mon, 3 Aug 2026 01:01:23 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D:=20=E5=81=8F=E7=A7=BB?= =?UTF-8?q?=E8=A1=A5=E5=81=BF=E5=8F=8C=E9=87=8D=E6=89=A3=E5=87=8F=E8=87=B4?= =?UTF-8?q?=E6=94=BE=E7=BD=AE=20y=20=E6=95=B4=E4=BD=93=E5=81=8F=E5=A4=A7?= =?UTF-8?q?=2035.75mm?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit MPU calibrated_H 下发的是旋转中心坐标 (source 抓取未补偿且正常为证), 旧公式 center=target-R(theta)*offset 把 target 当工具末端坐标双重扣减, 工具末端恒定偏 (6.6,-35.75)mm。改为 center=target+(dx,dy)-R(theta)*offset, 各角度误差归零。 --- components/actuator/actuator.c | 13 ++++++++++--- components/actuator/actuator.h | 14 +++++++++----- 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/components/actuator/actuator.c b/components/actuator/actuator.c index 339aec7..a6fa9bd 100644 --- a/components/actuator/actuator.c +++ b/components/actuator/actuator.c @@ -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; diff --git a/components/actuator/actuator.h b/components/actuator/actuator.h index 2d39aa4..f1fb700 100644 --- a/components/actuator/actuator.h +++ b/components/actuator/actuator.h @@ -160,12 +160,16 @@ extern "C" { * 终端执行机构 (工具末端) 与 Z 旋转机构不在同一条纵轴上, * 旋转角度 θ 时, 工具末端的 XY 位置会绕旋转中心偏移。 * - * 补偿公式 (将工具末端定位到 target): - * X_motor = X_tgt - (dx * cosθ - dy * sinθ) - * Y_motor = Y_tgt - (dx * sinθ + dy * cosθ) + * ⚠️ 语义 (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), + * 再按转角 θ 反算旋转中心)。旧公式少了 "+ (dx,dy)" 项, + * 会把工具末端整体放偏 (dx,dy) = (6.6, -35.75)mm (y 偏大 35.75mm)。 * - * dx, dy 为工具末端相对于 Z 旋转中心的 XY 偏移 (mm), - * 待机械测量后填入。 + * dx, dy 为工具末端相对于 Z 旋转中心的 XY 偏移 (mm)。 * ================================================================ */ #ifndef ACTUATOR_OFFSET_X_MM #define ACTUATOR_OFFSET_X_MM 6.6f /* angle=0° 时工具末端偏右 ~8mm (实测) */