EDC/edc-mcu
3
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

releasev1.2

Browse Source
This commit is contained in:
kirl committed 2026-08-03 04:09:26 +08:00
1 parent 74010ef611
commit c296caa328
10 files changed
+693 -267
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

components/actuator/actuator.c
+289 -130
View File
@@ -28,6 +28,11 @@ static float s_scale_angle = ACTUATOR_SCALE_ANGLE;
/* Z 轴当前命令位置 (度, 绝对), 初始与驱动零位一致; 用于行程内选近路 */
static float s_cur_angle_deg = 0.0f;
/* X/Y 当前绝对位置跟踪 (脉冲, 原点=0): 用于按“当前→目标”真实行程
计算到位掩码与超时基准, 而不是按绝对坐标 (原点) 估算 */
static uint32_t s_cur_x_pulses = 0;
static uint32_t s_cur_y_pulses = 0;
/* Z 旋转轴心 → 工具末端 偏移补偿 (mm) */
static float s_offset_x_mm = ACTUATOR_OFFSET_X_MM;
static float s_offset_y_mm = ACTUATOR_OFFSET_Y_MM;
@@ -70,23 +75,22 @@ static void _wait_flash(uint32_t ms, uint8_t r, uint8_t g, uint8_t b, uint32_t p
rgb_off();
}
/* 到位等待 (闭环反馈)
/* 到位等待 (事件驱动 + 预估时间兜底)
*
* 实测发现这台 X42S 的 S_FLAG Prf_TF 到位标志只有 X 轴会置位 (0x03),
* Z/Y1/Y2 始终为 0x01 (仅使能位), 轮询 Prf_TF 无法判定到位。
* 因此改为直接比对电机实时位置 (S_CPOS) 与目标位置 (S_TPOS):
* 偏差 < ACTUATOR_ARRIVAL_TOL_RAW 即认为到位 —— 不依赖 Prf_TF。
* 每轮重新读 S_TPOS (电机处理命令后目标才更新, 首轮可能是旧目标)。
* 需连续 2 次在容差内才确认: 确保电机已稳定停下 (减速途中/振荡时
* 位置可能短暂进入容差, 单独一次会"没到位就进行下一步")。
* 电机驱动 Response 菜单已设为 Reached: 位置模式 (0xFD) 命令运动到位后
* 会主动回传到位帧 (地址 + FD + 9F + 6B)。UART RX 由 emm_v5 内部任务
* 中断读取并解析, 置位对应到位位; 这里事件等待全部到位, 不再轮询
* S_TPOS/S_CPOS, 从根本上避免"前一个动作没到位、下一个动作就执行"。
*
* ⚠️ 防坑: 轮询第一帧前必须留帧间隔 —— 运动命令最后一条是 Y 同步广播
* 触发 (00 FF 66 6B), 若紧跟发轮询帧会"零间隔背靠背", 驱动板丢字节
* 吞掉触发帧, 电机不启动。
* 兜底: 同时按曲线加减速估算纯运动时间 (base_wait_ms), 超时 = 估算
* + ACTUATOR_SETTLE_MARGIN_MS; 若超过预估过多 (到位帧丢失/电机异常)
* 仍未到位, 打印告警并返回 false, 上层继续执行后续动作, 不卡死。
*
* 等待期间红灯快闪反馈 "运动中/未到位"。
*
* @param base_wait_ms 纯运动时间估算 (兜底超时基准)
* @param moved_mask 实际有行程的电机位掩码 (bit0=Z, bit1=X, bit2/3=Y1/Y2)
* @return true=全部到位, false=超时
* @return true=全部到位, false=超时 (调用方应继续后续动作)
*/
bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask)
{
@@ -94,85 +98,65 @@ bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask)
ACTUATOR_ADDR_ANGLE, ACTUATOR_ADDR_X,
ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2
};
bool arrived[4] = {false, false, false, false};
uint8_t set_cnt[4] = {0, 0, 0, 0}; /* 连续读到在容差内的次数 (≥2 确认已停稳) */
int32_t last_cpos[4] = {-1, -1, -1, -1}; /* 诊断 */
int32_t last_tpos[4] = {-1, -1, -1, -1}; /* 诊断 */
uint32_t polls = 0; /* 诊断: 轮询轮数 */
uint32_t timeout_ms = base_wait_ms + ACTUATOR_SETTLE_MARGIN_MS;
int64_t deadline = esp_timer_get_time() + (int64_t)timeout_ms * 1000;
uint8_t wait_addrs[4];
uint8_t wait_count = 0;
for (int i = 0; i < 4; i++) {
if (!(moved_mask & (1u << i)))
arrived[i] = true; /* 无行程: 视为已到位 */
if (moved_mask & (1u << i))
wait_addrs[wait_count++] = addrs[i];
}
if (wait_count == 0)
return true; /* 无行程: 视为已到位 */
/* 与上一条运动命令 (尤其 Y 同步触发) 之间留帧间隔, 防丢字节 */
vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS));
/* 兜底超时 = 预估纯运动时间 + 裕量 */
uint32_t timeout_ms = base_wait_ms + ACTUATOR_SETTLE_MARGIN_MS;
int64_t start = esp_timer_get_time();
int64_t deadline = start + (int64_t)timeout_ms * 1000;
while (esp_timer_get_time() < deadline) {
bool all = true;
for (int i = 0; i < 4; i++) {
if (arrived[i])
continue;
ESP_LOGI(TAG, "等待到位: mask=0x%02X (%u 轴: %u/%u/%u/%u), 超时=%u ms (预估 %u + 裕量 %u)",
moved_mask, wait_count,
wait_addrs[0], wait_count > 1 ? wait_addrs[1] : 0,
wait_count > 2 ? wait_addrs[2] : 0, wait_count > 3 ? wait_addrs[3] : 0,
timeout_ms, base_wait_ms, ACTUATOR_SETTLE_MARGIN_MS);
bool at_target = false;
int32_t tpos = Emm_V5_Read_TPOS(addrs[i]);
last_tpos[i] = tpos;
if (tpos >= 0) {
int32_t cpos = Emm_V5_Read_CPOS(addrs[i]);
last_cpos[i] = cpos;
if (cpos >= 0) {
int32_t diff = (int32_t)((uint16_t)cpos - (uint16_t)tpos);
if (diff < 0) diff = -diff;
if (diff > 32768) diff = 65536 - diff; /* 环形距离 */
at_target = (diff <= (int32_t)ACTUATOR_ARRIVAL_TOL_RAW);
}
} else {
/* 目标位置读失败: 退回 Prf_TF 到位标志 (仅 X 可靠) */
uint8_t flags = Emm_V5_Read_Status_Flag(addrs[i], 50);
at_target = (flags != 0xFF && (flags & 0x02));
}
if (at_target) {
/* 连续 2 次在容差内才确认: 电机已稳定停下,
防止减速途中/振荡时误判"到位"提前进入下一步 */
if (++set_cnt[i] >= 2) {
arrived[i] = true;
} else {
all = false;
}
} else {
set_cnt[i] = 0;
all = false;
}
vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS)); /* 帧间隔防粘包 */
bool ok = false;
while (1) {
int64_t left_us = deadline - esp_timer_get_time();
if (left_us <= 0)
break; /* 超过预估过多: 停止等待 */
uint32_t left_ms = (uint32_t)(left_us / 1000);
/* ⚠️ left_us 单位是微秒, 必须先转毫秒再用 (曾把 µs 当 ms → 每次等 40 秒) */
uint32_t chunk = (left_ms > 40000) ? 40000 : left_ms;
if (chunk == 0) chunk = 1;
if (Emm_V5_Wait_Arrival(wait_addrs, wait_count, chunk)) {
ok = true;
break;
}
polls++;
if (all)
return true;
/* 诊断: 前 3 轮打印各电机位置 (调试用, 定位后删除) */
if (polls <= 3) {
ESP_LOGI(TAG, "到位轮询#%u: cpos Z=%ld X=%ld Y1=%ld Y2=%ld "
"tpos Z=%ld X=%ld Y1=%ld Y2=%ld",
polls,
(long)last_cpos[0], (long)last_cpos[1], (long)last_cpos[2], (long)last_cpos[3],
(long)last_tpos[0], (long)last_tpos[1], (long)last_tpos[2], (long)last_tpos[3]);
}
/* 红灯快闪反馈 (与旧 _wait_flash 一致) */
ESP_LOGD(TAG, "到位等待中: 剩余 %u ms, 已到位位=0x%02X",
left_ms, Emm_V5_Arrival_Get_Bits());
/* 红灯快闪: 等待中 */
rgb_set(128, 0, 0);
vTaskDelay(pdMS_TO_TICKS(10));
rgb_off();
vTaskDelay(pdMS_TO_TICKS(10));
}
ESP_LOGW(TAG, "到位超时 (%u ms, %u 轮): cpos Z=%ld X=%ld Y1=%ld Y2=%ld "
"tpos Z=%ld X=%ld Y1=%ld Y2=%ld",
timeout_ms, polls,
(long)last_cpos[0], (long)last_cpos[1], (long)last_cpos[2], (long)last_cpos[3],
(long)last_tpos[0], (long)last_tpos[1], (long)last_tpos[2], (long)last_tpos[3]);
/* 最后再查一次 (可能刚好到位), 并消费本段 motion 的到位位 */
if (!ok)
ok = Emm_V5_Wait_Arrival(wait_addrs, wait_count, 0);
int64_t elapsed_ms = (esp_timer_get_time() - start) / 1000;
uint32_t got_bits = Emm_V5_Arrival_Get_Bits();
Emm_V5_Arrival_Consume(wait_addrs, wait_count);
if (ok) {
ESP_LOGI(TAG, "到位 (mask=0x%02X, %u 轴, 实际等待 %lld ms)",
moved_mask, wait_count, elapsed_ms);
return true;
}
ESP_LOGW(TAG, "到位超时 (%u ms = 预估 %u + 裕量 %u), mask=0x%02X, 实际已等 %lld ms, 已到位位=0x%02X, 继续后续动作",
timeout_ms, base_wait_ms, ACTUATOR_SETTLE_MARGIN_MS, moved_mask,
elapsed_ms, got_bits);
return false;
}
@@ -253,8 +237,8 @@ void actuator_init(void)
/* 初始化舵机 PWM */
servo_init(ACTUATOR_SERVO_GPIO);
/* 配置舵机限位: 抬起=80°, 放下=160° */
servo_set_limits(0.0f, 95.0f);
/* 配置舵机限位: 抬起=0°, 放下=103° */
servo_set_limits(0.0f, 102.0f);
/* 上电默认归位到抬起状态 */
servo_set_up();
@@ -273,8 +257,10 @@ void actuator_init(void)
}
s_initialized = true;
/* Z 轴命令位置跟踪与驱动零位对齐 (重新初始化时复位) */
/* 位置跟踪与驱动零位对齐 (重新初始化时复位) */
s_cur_angle_deg = 0.0f;
s_cur_x_pulses = 0;
s_cur_y_pulses = 0;
ESP_LOGI(TAG, "初始化完成 (Angle=addr%d X=addr%d Y=addr%d/%d, servo=GPIO%d)",
ACTUATOR_ADDR_ANGLE,
ACTUATOR_ADDR_X,
@@ -301,12 +287,18 @@ void actuator_set_scale(float x_scale, float y_scale, float angle_scale)
* 终端执行机构与 Z 旋转机构不在同一条纵轴上,
* 旋转角度 θ 时, 工具末端 XY 位置 = 旋转中心 + R(θ)·(dx, dy)
*
* 要将工具末端放到 target 位置, 旋转中心需要去:
* center = target - R(θ)·(dx, dy)
*
* 即:
* 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 时中心到坐标点, 工具末端即落在
* D = 坐标点 + (dx,dy) (即视觉目标物点; 放置修复后实测验证)。
* 因此要把工具末端放到 D, 中心需去:
* center = D - R(θ)·(dx,dy) = 坐标 + (dx,dy) - R(θ)·(dx,dy)
* 即:
* X_motor = X + dx - (dx·cosθ - dy·sinθ)
* Y_motor = Y + dy - (dx·sinθ + dy·cosθ)
* source 与 target 都必须补偿 (source 不补偿时抓取点会随 Z 角偏移,
* 见 _actuator_move_fragment 调用处注释)。
* 旧公式 center = 坐标 - R(θ)·(dx,dy) 少了 "+ (dx,dy)" 项, 会把工具
* 末端整体放偏 (dx,dy) = (6.6,-35.75)mm (y 偏大 35.75mm)。
* ================================================================ */
void actuator_set_offset(float offset_x_mm, float offset_y_mm)
@@ -363,6 +355,31 @@ void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y,
*target_x, *target_y);
}
/* ================================================================
* 内部: 位置跟踪 (到位掩码 / 超时按真实行程计算)
* ================================================================ */
/* X/Y 轴: 绝对运动直接置为 pulses, 相对运动按 dir 增减 (钳位 ≥0) */
static void _track_xy_axis(bool is_y, uint8_t dir, uint32_t pulses, bool raF)
{
uint32_t *cur = is_y ? &s_cur_y_pulses : &s_cur_x_pulses;
uint8_t pos_dir = is_y ? ACTUATOR_DIR_Y : ACTUATOR_DIR_X;
if (raF) {
*cur = pulses;
} else {
*cur = (dir == pos_dir) ? (*cur + pulses)
: (*cur > pulses ? *cur - pulses : 0);
}
}
/* Z 旋转轴: 脉冲数 → 角度跟踪 (绝对置位 / 相对按 dir 增减) */
static void _track_angle_pulses(uint8_t dir, uint32_t pulses, bool raF)
{
float deg = (float)pulses / (10.0f * s_scale_angle);
s_cur_angle_deg = raF ? deg
: (s_cur_angle_deg + (dir == ACTUATOR_DIR_ANGLE ? deg : -deg));
}
/* ================================================================
* 内部: 单轴绝对位置运动
* ================================================================ */
@@ -373,7 +390,13 @@ static void _actuator_move_abs(uint8_t addr, uint8_t dir, uint32_t pulses,
Emm_V5_Pos_Control(addr, dir, vel, acc,
pulses,
true, /* raF=true: 绝对运动 */
snF); /* 多机同步标志 */
snF);
/* 更新 XY 位置跟踪 (Z 角度由 _actuator_move_angle_rel 单独跟踪) */
if (addr == ACTUATOR_ADDR_X)
_track_xy_axis(false, dir, pulses, true);
else if (addr == ACTUATOR_ADDR_Y1 || addr == ACTUATOR_ADDR_Y2)
_track_xy_axis(true, dir, pulses, true);
}
/* ================================================================
@@ -403,25 +426,8 @@ static float _closest_reachable_angle(float target_deg, float cur_deg, float lim
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 (带符号度): 正负自动选方向, 语义明确, 用于片间旋转 */
/** Z 轴相对运动 delta (带符号度): 正负自动选方向, 语义明确, 用于片间旋转;
⚠️ 一律用相对运动 —— 实测电机1 对“绝对 clk=0”命令不回传到位帧 (会超时) */
static void _actuator_move_angle_rel(float delta_deg, uint16_t vel, uint8_t acc)
{
uint8_t dir = ACTUATOR_DIR_ANGLE;
@@ -438,6 +444,34 @@ static void _actuator_move_angle_rel(float delta_deg, uint16_t vel, uint8_t acc)
(double)delta_deg, (unsigned long)pulses, dir ? "CCW" : "CW");
}
/** Z 轴回 0° 到位确认: 电机1 (Z 轴) 对“终点靠近 0°”的移动不回传到位帧,
改用 Emm_V5_Read_CPOS 轮询实际位置确认 (0~65535 对应 0~360°, 0° 可能读
0 或 65535 负向回绕)。Z 回零移动小, 通常已被 X/Y 事件等待覆盖, 首次即返回。
@return true=已到 0° 附近 (容差 tol_deg) */
static bool _wait_z_at_zero(uint32_t timeout_ms, float tol_deg)
{
uint32_t tol_pulses = (uint32_t)(tol_deg / 360.0f * 65535.0f);
int64_t deadline = esp_timer_get_time() + (int64_t)timeout_ms * 1000;
int32_t last = -1;
while (1) {
int32_t cpos = Emm_V5_Read_CPOS(ACTUATOR_ADDR_ANGLE);
if (cpos >= 0) {
last = cpos;
uint32_t p = (uint32_t)cpos;
if (p <= tol_pulses || p >= 65535u - tol_pulses) {
ESP_LOGI(TAG, "Z 回 0 确认 (CPOS=%ld)", (long)cpos);
return true;
}
}
if (esp_timer_get_time() >= deadline)
break;
vTaskDelay(pdMS_TO_TICKS(20));
}
ESP_LOGW(TAG, "Z 回 0 轮询超时 (%u ms), 最后 CPOS=%ld, 继续",
timeout_ms, (long)last);
return false;
}
/* ================================================================
* 内部: Y 轴双电机同步运动
* ================================================================ */
@@ -484,7 +518,14 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
float src_angle_deg = (float)src_angle * 0.1f;
float tgt_angle_deg = (float)tgt_angle * 0.1f;
/* Z 旋转轴心偏移补偿 (仅对 target 做, 与旧协议行为保持一致) */
/* Z 旋转轴心偏移补偿 —— source 与 target 都做 (2026-08-03):
MPU 下发 source/target 均为"旋转中心"坐标 (θ=0 时吸盘在坐标+(dx,dy))。
若 source 不补偿, 只要抓取瞬间 Z 角 ≠ 0° (前一片放置转角/归零不到位/
超行程卡位等), 吸盘落点就会偏移 (R(θ)-I)·(dx,dy), 其中 y 分量
(dy=35.75mm) 主导 → 表现为"设置的抓取点 vs 实际抓取点偏 y"。
补偿后吸盘恒 = source+(dx,dy) = 视觉目标点, 与抓取时 Z 角无关;
θ=0° 时补偿为恒等, 对原有行为无影响。 */
actuator_apply_offset_compensation(&src_x, &src_y, src_angle_deg);
actuator_apply_offset_compensation(&tgt_x, &tgt_y, tgt_angle_deg);
/* source 坐标 → 脉冲数 */
@@ -502,27 +543,43 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
(double)tgt_angle_deg,
tgt_x, tgt_y);
/* 到位轮询掩码: 只等待实际有行程的电机 (脉冲>0), 无行程直接视为已到位 */
/* 到位轮询掩码与超时基准: 一律按“当前实际位置 → 目标”的真实行程计算,
而不是按绝对坐标 (原点) 估算。绝对坐标会把已在目标位/离原点很远的轴
算成大行程 → 无行程也等满超时 (初始==目标时整段空等), 且超时被放大 */
float src_target = _closest_reachable_angle(src_angle_deg, s_cur_angle_deg,
ACTUATOR_ANGLE_LIMIT_DEG);
uint32_t src_pulses_z = (uint32_t)(fabsf(src_target) * 10.0f * s_scale_angle + 0.5f);
uint32_t src_mask = 0;
if (src_pulses_z > 0) src_mask |= (1u << 0); /* Z 旋转 */
if (src_pulses_x > 0) src_mask |= (1u << 1); /* X */
if (src_pulses_y > 0) src_mask |= (1u << 2) | (1u << 3); /* Y 双电机 */
/* 各轴真实行程 (脉冲): Z 终点=0° 时电机1 不回传到位帧,
不纳入事件等待, 改为之后用 CPOS 轮询确认 */
float src_z_tgt = src_target;
uint32_t src_dz = (uint32_t)(fabsf(src_z_tgt - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f);
uint32_t src_dx = (src_pulses_x > s_cur_x_pulses) ? (src_pulses_x - s_cur_x_pulses)
: (s_cur_x_pulses - src_pulses_x);
uint32_t src_dy = (src_pulses_y > s_cur_y_pulses) ? (src_pulses_y - s_cur_y_pulses)
: (s_cur_y_pulses - src_pulses_y);
/* 纯运动时间 (无裕量), 仅作到位等待的兜底超时基准 */
uint32_t src_wait_ms = actuator_motion_time_ms(src_pulses_z, vel_angle, acc_angle);
uint32_t t_wait = actuator_motion_time_ms(src_pulses_x, vel_xy, acc_xy);
uint32_t src_mask = 0;
if (src_dz > 0 && src_z_tgt != 0.0f) src_mask |= (1u << 0); /* Z 旋转 */
if (src_dx > 0) src_mask |= (1u << 1); /* X */
if (src_dy > 0) src_mask |= (1u << 2) | (1u << 3); /* Y 双电机 */
/* 纯运动时间 (按真实行程, 无裕量), 仅作到位等待的兜底超时基准 */
uint32_t src_wait_ms = actuator_motion_time_ms(src_dz, vel_angle, acc_angle);
uint32_t t_wait = actuator_motion_time_ms(src_dx, vel_xy, acc_xy);
if (t_wait > src_wait_ms) src_wait_ms = t_wait;
t_wait = actuator_motion_time_ms(src_pulses_y, vel_xy, acc_xy);
t_wait = actuator_motion_time_ms(src_dy, vel_xy, acc_xy);
if (t_wait > src_wait_ms) src_wait_ms = t_wait;
ESP_LOGI(TAG, "source 段行程: dZ=%lu dX=%lu dY=%lu 脉冲, mask=0x%02X, 等待超时=%u ms",
(unsigned long)src_dz, (unsigned long)src_dx, (unsigned long)src_dy,
src_mask, src_wait_ms + ACTUATOR_SETTLE_MARGIN_MS);
/* ---- 1. 磁铁掉电 ---- */
magnet_set(false);
/* ---- 2. 运动到 source 位置 ---- */
_actuator_move_angle_abs(src_angle_deg, vel_angle, acc_angle);
Emm_V5_Arrival_Reset(); /* 清掉上一段迟到/多余的到位帧, 防污染本段等待 */
/* source Z 用相对运动转到目标朝向 (0° 基准位; 到位用 CPOS 轮询确认) */
_actuator_move_angle_rel(src_z_tgt - s_cur_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);
@@ -532,6 +589,10 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
/* ---- 3. 等待运动到位 (轮询闭环到位标志, 红灯快闪) ---- */
actuator_wait_arrival(src_wait_ms, src_mask);
/* Z 终点=0°: 不回传到位帧 → 已排除出事件等待, 用 CPOS 轮询确认到位 */
if (src_dz > 0 && src_z_tgt == 0.0f)
_wait_z_at_zero(ACTUATOR_Z_CONFIRM_MS, ACTUATOR_Z_CONFIRM_TOL_DEG);
/* ---- 4. 舵机放下 ---- */
servo_set_down();
_wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
@@ -545,23 +606,36 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
_wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
/* ---- 7. 运动到 target 位置 ---- */
Emm_V5_Arrival_Reset(); /* 清掉上一段迟到/多余的到位帧, 防污染本段等待 */
/* 目标角度取行程内与当前位置最近的等价角, 再用相对运动执行:
终点朝向与旧协议 (绝对转到 rotate mod 360) 完全一致, 且不会绕远路 */
float dst_rep = _closest_reachable_angle(tgt_angle_deg, s_cur_angle_deg,
ACTUATOR_ANGLE_LIMIT_DEG);
uint32_t dst_pulses_z = (uint32_t)(fabsf(dst_rep - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f);
/* 真实行程: 当前 (source 终点) → target; 无行程的轴不等待
Z 终点=0° (相对转角恰为 0) 时不纳入事件等待, 用 CPOS 轮询确认 */
float dst_z_tgt = dst_rep;
uint32_t dst_dz = (uint32_t)(fabsf(dst_z_tgt - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f);
uint32_t dst_dx = (dst_pulses_x > s_cur_x_pulses) ? (dst_pulses_x - s_cur_x_pulses)
: (s_cur_x_pulses - dst_pulses_x);
uint32_t dst_dy = (dst_pulses_y > s_cur_y_pulses) ? (dst_pulses_y - s_cur_y_pulses)
: (s_cur_y_pulses - dst_pulses_y);
uint32_t dst_mask = 0;
if (dst_pulses_z > 0) dst_mask |= (1u << 0);
if (dst_pulses_x > 0) dst_mask |= (1u << 1);
if (dst_pulses_y > 0) dst_mask |= (1u << 2) | (1u << 3);
if (dst_dz > 0 && dst_z_tgt != 0.0f) dst_mask |= (1u << 0);
if (dst_dx > 0) dst_mask |= (1u << 1);
if (dst_dy > 0) dst_mask |= (1u << 2) | (1u << 3);
uint32_t dst_wait_ms = actuator_motion_time_ms(dst_pulses_z, vel_angle, acc_angle);
t_wait = actuator_motion_time_ms(dst_pulses_x, vel_xy, acc_xy);
uint32_t dst_wait_ms = actuator_motion_time_ms(dst_dz, vel_angle, acc_angle);
t_wait = actuator_motion_time_ms(dst_dx, vel_xy, acc_xy);
if (t_wait > dst_wait_ms) dst_wait_ms = t_wait;
t_wait = actuator_motion_time_ms(dst_pulses_y, vel_xy, acc_xy);
t_wait = actuator_motion_time_ms(dst_dy, 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);
ESP_LOGI(TAG, "target 段行程: dZ=%lu dX=%lu dY=%lu 脉冲, mask=0x%02X, 等待超时=%u ms",
(unsigned long)dst_dz, (unsigned long)dst_dx, (unsigned long)dst_dy,
dst_mask, dst_wait_ms + ACTUATOR_SETTLE_MARGIN_MS);
_actuator_move_angle_rel(dst_z_tgt - 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);
@@ -571,6 +645,10 @@ static void _actuator_move_fragment(const edc_fragment_t *frag)
/* ---- 8. 等待运动到位 (轮询闭环到位标志, 红灯快闪) ---- */
actuator_wait_arrival(dst_wait_ms, dst_mask);
/* Z 终点=0°: 用 CPOS 轮询确认到位 */
if (dst_dz > 0 && dst_z_tgt == 0.0f)
_wait_z_at_zero(ACTUATOR_Z_CONFIRM_MS, ACTUATOR_Z_CONFIRM_TOL_DEG);
/* ---- 9. 舵机放下 ---- */
servo_set_down();
_wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200);
@@ -646,7 +724,10 @@ void edc_proto_on_cycle(const edc_cycle_t *cycle, edc_channel_t ch)
ESP_LOGI(TAG, "开始执行 Cycle #%u ...", cycle->cycle_id);
actuator_execute_cycle(cycle);
/* 全部 Fragment 执行完成后回复 DONE */
/* 完成后执行机构置零 (归位): 舵机抬起 + Z/X/Y 三轴回零点 */
actuator_home_all();
/* 全部执行完毕 (含置零) 后回复 DONE */
edc_proto_send_done(cycle->cycle_id);
ESP_LOGI(TAG, "Cycle #%u 执行完毕, DONE 已发送", cycle->cycle_id);
@@ -740,11 +821,16 @@ void actuator_motor_pos(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc,
Emm_V5_Pos_Control(peer, dir, vel, acc, clk, raF, true);
_frame_gap();
Emm_V5_Synchronous_motion(0);
_track_xy_axis(true, dir, clk, raF); /* 更新位置跟踪 */
ESP_LOGI(TAG, "位置(Y同步): 电机%d+%d %s %dRPM %lu脉冲 %s",
addr, peer, dir ? "CCW" : "CW", vel, clk,
raF ? "绝对" : "相对");
} else {
Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, false);
if (addr == ACTUATOR_ADDR_X)
_track_xy_axis(false, dir, clk, raF);
else if (addr == ACTUATOR_ADDR_ANGLE)
_track_angle_pulses(dir, clk, raF);
ESP_LOGI(TAG, "位置: 电机%d %s %dRPM %lu脉冲 %s",
addr, dir ? "CCW" : "CW", vel, clk,
raF ? "绝对" : "相对");
@@ -770,9 +856,14 @@ 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)
/* 跟踪命令位置 (供 fragment 真实行程计算) */
if (addr == ACTUATOR_ADDR_ANGLE) {
s_cur_angle_deg = raF ? angle_deg : (s_cur_angle_deg + angle_deg);
} else if (addr == ACTUATOR_ADDR_X) {
/* X 轴按角度手动控制: 换算脉冲后更新跟踪 (负角内部翻转方向) */
uint8_t eff_dir = (angle_deg < 0.0f) ? (uint8_t)!dir : dir;
_track_xy_axis(false, eff_dir, EMM_V5_DEG_TO_CLK(fabsf(angle_deg)), raF);
}
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,
@@ -805,11 +896,14 @@ void actuator_motor_mm(uint8_t addr, float distance_mm, uint16_t vel,
Emm_V5_Pos_Control(peer, dir, vel, acc, clk, raF, true);
_frame_gap();
Emm_V5_Synchronous_motion(0);
_track_xy_axis(true, dir, clk, raF); /* 更新位置跟踪 */
ESP_LOGI(TAG, "直线(Y同步 %s): 电机%d+%d %.2fmm (%lu脉冲) %s vel=%dRPM",
dir ? "CCW" : "CW", addr, peer, (double)distance_mm,
(unsigned long)clk, raF ? "绝对" : "相对", vel);
} else {
Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, false);
if (addr == ACTUATOR_ADDR_X)
_track_xy_axis(false, dir, clk, raF);
ESP_LOGI(TAG, "直线(%s): 电机%d %.2fmm (%lu脉冲) %s vel=%dRPM",
dir ? "CCW" : "CW", addr, (double)distance_mm,
(unsigned long)clk, raF ? "绝对" : "相对", vel);
@@ -832,6 +926,71 @@ void actuator_motor_stop(uint8_t addr)
}
}
/* ================================================================
* 执行机构置零 (归位)
*
* 舵机抬起 + Z/X/Y 三轴并行回零点。Z 旋转与 X/Y 平移互不干扰
* (Fragment 里也是三轴并行), 无需等 Z 到位再动 X/Y —— 三轴同时
* 下发、一次到位等待。Cycle 执行完成后与按钮识别前都会调用。
* ================================================================ */
void actuator_home_all(void)
{
/* 舵机抬起 */
servo_set_up();
/* 各轴回零真实行程 (当前 → 零点): 已在零位的轴不等待、不算超时 */
uint32_t x_home_pulses = ACTUATOR_MM_TO_PULSES(ACTUATOR_HOME_X_MM);
uint32_t y_home_pulses = ACTUATOR_MM_TO_PULSES(ACTUATOR_HOME_Y_MM);
/* 置零 Z 终点=0°: 电机1 不回传到位帧 → 不纳入事件等待, 用 CPOS 轮询确认 */
float home_z_tgt = ACTUATOR_HOME_ANGLE_DEG;
uint32_t home_dz = (uint32_t)(fabsf(s_cur_angle_deg - home_z_tgt)
* 10.0f * s_scale_angle + 0.5f);
uint32_t home_dx = (s_cur_x_pulses > x_home_pulses) ? (s_cur_x_pulses - x_home_pulses)
: (x_home_pulses - s_cur_x_pulses);
uint32_t home_dy = (s_cur_y_pulses > y_home_pulses) ? (s_cur_y_pulses - y_home_pulses)
: (y_home_pulses - s_cur_y_pulses);
uint32_t home_mask = 0;
if (home_dz > 0 && home_z_tgt != 0.0f) home_mask |= (1u << 0);
if (home_dx > 0) home_mask |= (1u << 1); /* X */
if (home_dy > 0) home_mask |= (1u << 2) | (1u << 3); /* Y 双电机 */
/* 到位等待兜底超时基准 = 各轴真实回零行程的纯运动时间 */
uint32_t home_motion = actuator_motion_time_ms(home_dz, ACTUATOR_EXEC_Z_VEL,
ACTUATOR_EXEC_Z_ACC);
uint32_t t = actuator_motion_time_ms(home_dx, ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC);
if (t > home_motion) home_motion = t;
t = actuator_motion_time_ms(home_dy, ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC);
if (t > home_motion) home_motion = t;
ESP_LOGI(TAG, "置零行程: dZ=%lu dX=%lu dY=%lu 脉冲, mask=0x%02X, 等待超时=%u ms",
(unsigned long)home_dz, (unsigned long)home_dx, (unsigned long)home_dy,
home_mask, home_motion + ACTUATOR_SETTLE_MARGIN_MS);
/* 清掉可能残留的到位帧, 防止污染本次归零等待 (超时后迟到帧边界) */
Emm_V5_Arrival_Reset();
/* Z/X/Y 三轴并行回零 (Z 到位用 CPOS 轮询确认) */
actuator_motor_angle(ACTUATOR_ADDR_ANGLE, home_z_tgt - s_cur_angle_deg,
ACTUATOR_EXEC_Z_VEL, ACTUATOR_EXEC_Z_ACC, false);
actuator_motor_mm(ACTUATOR_ADDR_X, ACTUATOR_HOME_X_MM,
ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC, true);
actuator_motor_mm(ACTUATOR_ADDR_Y1, ACTUATOR_HOME_Y_MM,
ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC, true);
actuator_wait_arrival(home_motion, home_mask);
/* Z 终点=0°: 用 CPOS 轮询确认到位 */
if (home_dz > 0 && home_z_tgt == 0.0f)
_wait_z_at_zero(ACTUATOR_Z_CONFIRM_MS, ACTUATOR_Z_CONFIRM_TOL_DEG);
ESP_LOGI(TAG, "置零完成 (Z=%.1f° X=%.0fmm Y=%.0fmm)",
(double)home_z_tgt,
(double)ACTUATOR_HOME_X_MM,
(double)ACTUATOR_HOME_Y_MM);
}
/* ================================================================
* 最近 Cycle 快照查询
* ================================================================ */
components/actuator/actuator.h
+43 -10
View File
@@ -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 路电机
*/