/** * @file actuator.c * @brief 执行器应用层封装实现 */ #include "actuator.h" #include "emm_v5.h" #include "servo.h" #include "magnet.h" #include "rgb.h" #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "esp_log.h" #include "esp_timer.h" #include #include static const char *TAG = "ACTUATOR"; /* ================================================================ * 运行时状态 * ================================================================ */ static float s_scale_x = ACTUATOR_SCALE_X; static float s_scale_y = ACTUATOR_SCALE_Y; 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; static bool s_initialized = false; /* 最近一次执行的 Cycle 快照 */ static edc_cycle_t s_last_cycle; static bool s_last_cycle_valid = false; /* ================================================================ * 内部: 帧间间隔 * * Emm_V5 驱动板无法解析 UART 上零间隔的背靠背连续帧 (会丢字节, * 导致本帧与后续帧全部失效), 凡连发多帧必须插入该延时。 * 曾导致故障: Y 轴 3/4 号电机使能 (单帧) 正常, 速度/位置 (连发多帧) 无反应 * ================================================================ */ static inline void _frame_gap(void) { vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS)); } /* 内部: 带 RGB 闪烁的延时 (周期 ms, 亮灭各半; 精确到系统 tick) */ static void _wait_flash(uint32_t ms, uint8_t r, uint8_t g, uint8_t b, uint32_t period_ms) { TickType_t total = pdMS_TO_TICKS(ms); TickType_t half = pdMS_TO_TICKS(period_ms / 2); if (half == 0) half = 1; bool led_on = true; while (total > 0) { 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(); } /* 到位等待 (事件驱动 + 预估时间兜底) * * 电机驱动 Response 菜单已设为 Reached: 位置模式 (0xFD) 命令运动到位后 * 会主动回传到位帧 (地址 + FD + 9F + 6B)。UART RX 由 emm_v5 内部任务 * 中断读取并解析, 置位对应到位位; 这里事件等待全部到位, 不再轮询 * S_TPOS/S_CPOS, 从根本上避免"前一个动作没到位、下一个动作就执行"。 * * 兜底: 同时按曲线加减速估算纯运动时间 (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=超时 (调用方应继续后续动作) */ bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask) { const uint8_t addrs[4] = { ACTUATOR_ADDR_ANGLE, ACTUATOR_ADDR_X, ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2 }; uint8_t wait_addrs[4]; uint8_t wait_count = 0; for (int i = 0; i < 4; i++) { if (moved_mask & (1u << i)) wait_addrs[wait_count++] = addrs[i]; } if (wait_count == 0) return true; /* 无行程: 视为已到位 */ /* 兜底超时 = 预估纯运动时间 + 裕量 */ 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; 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 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; } 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)); } /* 最后再查一次 (可能刚好到位), 并消费本段 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; } /* ================================================================ * 运动到位时间估算 * * 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_motion_time_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_motion_time_ms(pulses, vel, acc); } uint32_t actuator_angle_motion_time_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_motion_time_ms(pulses, vel, acc); } uint32_t actuator_mm_settle_ms(float distance_mm, uint16_t vel, uint8_t acc) { return actuator_mm_motion_time_ms(distance_mm, vel, acc) + ACTUATOR_SETTLE_MARGIN_MS; } uint32_t actuator_angle_settle_ms(float angle_deg, uint16_t vel, uint8_t acc) { return actuator_angle_motion_time_ms(angle_deg, vel, acc) + ACTUATOR_SETTLE_MARGIN_MS; } /* ================================================================ * 初始化 * ================================================================ */ void actuator_init(void) { /* 初始化电机驱动 UART */ Emm_V5_Init(ACTUATOR_UART_PORT, ACTUATOR_UART_TX, ACTUATOR_UART_RX); /* 初始化舵机 PWM */ servo_init(ACTUATOR_SERVO_GPIO); /* 配置舵机限位: 抬起=0°, 放下=103° */ servo_set_limits(0.0f, 102.0f); /* 上电默认归位到抬起状态 */ servo_set_up(); /* 配置 4 路电机: 闭环位置模式 + 使能 */ const uint8_t addrs[] = { ACTUATOR_ADDR_ANGLE, ACTUATOR_ADDR_X, ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2 }; for (int i = 0; i < 4; i++) { uint8_t addr = addrs[i]; Emm_V5_Modify_Ctrl_Mode(addr, false, ACTUATOR_CTRL_MODE); vTaskDelay(pdMS_TO_TICKS(20)); Emm_V5_En_Control(addr, true, false); vTaskDelay(pdMS_TO_TICKS(20)); } s_initialized = true; /* 位置跟踪与驱动零位对齐 (重新初始化时复位) */ 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, ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2, ACTUATOR_SERVO_GPIO); } /* ================================================================ * 脉冲换算 * ================================================================ */ void actuator_set_scale(float x_scale, float y_scale, float angle_scale) { s_scale_x = x_scale; s_scale_y = y_scale; s_scale_angle = angle_scale; ESP_LOGI(TAG, "脉冲比例: X=%.3f Y=%.3f Angle=%.3f", s_scale_x, s_scale_y, s_scale_angle); } /* ================================================================ * Z 旋转轴心 ↔ 工具末端 偏移补偿 * * 终端执行机构与 Z 旋转机构不在同一条纵轴上, * 旋转角度 θ 时, 工具末端 XY 位置 = 旋转中心 + R(θ)·(dx, dy) * * ⚠️ 实际语义 (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) { s_offset_x_mm = offset_x_mm; s_offset_y_mm = offset_y_mm; ESP_LOGI(TAG, "偏移补偿: dx=%.2fmm dy=%.2fmm", (double)offset_x_mm, (double)offset_y_mm); } void actuator_get_offset(float *offset_x_mm, float *offset_y_mm) { if (offset_x_mm) *offset_x_mm = s_offset_x_mm; if (offset_y_mm) *offset_y_mm = s_offset_y_mm; } void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y, float angle_deg) { if (!target_x || !target_y) return; /* 偏移量为零时跳过, 避免浮点运算 */ if (s_offset_x_mm == 0.0f && s_offset_y_mm == 0.0f) return; float theta_rad = angle_deg * (float)(M_PI / 180.0); float cos_t = cosf(theta_rad); float sin_t = sinf(theta_rad); /* R(θ)·(dx, dy): 旋转后的偏移向量 */ 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; /* ⚠️ 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; if (cy < 0.0f) cy = 0.0f; *target_x = (uint16_t)(cx + 0.5f); *target_y = (uint16_t)(cy + 0.5f); ESP_LOGD(TAG, "补偿: (%.1f°) offset(%.2f,%.2f)→rot(%.2f,%.2f) " "tgt→ctr (%u,%u)", (double)angle_deg, (double)s_offset_x_mm, (double)s_offset_y_mm, (double)rot_dx, (double)rot_dy, *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)); } /* ================================================================ * 内部: 单轴绝对位置运动 * ================================================================ */ static void _actuator_move_abs(uint8_t addr, uint8_t dir, uint32_t pulses, uint16_t vel, uint8_t acc, bool snF) { Emm_V5_Pos_Control(addr, dir, vel, acc, pulses, true, /* raF=true: 绝对运动 */ 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); } /* ================================================================ * 内部: Z 旋转轴角度运动 (绝对角, ±行程内选最近可达代表角) * ================================================================ */ /** 在 ±limit 行程内选择与当前位置最近的等价角 (mod 360) */ static float _closest_reachable_angle(float target_deg, float cur_deg, float limit) { target_deg = fmodf(target_deg, 360.0f); if (target_deg < 0.0f) target_deg += 360.0f; float best = target_deg; float best_dist = 1e30f; for (int k = -1; k <= 1; k++) { float rep = target_deg + 360.0f * (float)k; if (rep < -limit || rep > limit) continue; float dist = fabsf(rep - cur_deg); if (dist < best_dist - 1e-4f || (fabsf(dist - best_dist) <= 1e-4f && rep > best)) { best_dist = dist; best = rep; } } return best; } /** 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; float abs_deg = fabsf(delta_deg); uint32_t pulses = (uint32_t)(abs_deg * 10.0f * s_scale_angle + 0.5f); if (delta_deg < 0.0f) dir = (uint8_t)!dir; Emm_V5_Pos_Control(ACTUATOR_ADDR_ANGLE, dir, vel, acc, pulses, false, /* raF=false: 相对运动 */ false); s_cur_angle_deg += delta_deg; ESP_LOGI(TAG, "Z 角度: 相对 %.1f° (%lu pulses, %s)", (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 轴双电机同步运动 * ================================================================ */ static void _actuator_move_y_sync(uint32_t pulses_y, uint16_t vel, uint8_t acc) { /* Y 轴正向 = CCW (dir=1), 双电机同步 */ _actuator_move_abs(ACTUATOR_ADDR_Y1, ACTUATOR_DIR_Y, pulses_y, vel, acc, true); _frame_gap(); _actuator_move_abs(ACTUATOR_ADDR_Y2, ACTUATOR_DIR_Y, pulses_y, vel, acc, true); _frame_gap(); /* 广播地址触发一次, 两台电机同时开始运动 */ Emm_V5_Synchronous_motion(0); ESP_LOGD(TAG, "Y 同步(CCW): addr%d+%d -> %lu pulses (%.2fmm) vel=%d acc=%d", ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2, pulses_y, (double)pulses_y / (double)ACTUATOR_SCALE_Y, vel, acc); } /* ================================================================ * 内部: 执行单个 Fragment * ================================================================ */ static void _actuator_move_fragment(const edc_fragment_t *frag) { /* XY 执行速度/加速度 (3000RPM + 250 档, 实测稳定); Z 旋转轴保持低速 (见 ACTUATOR_EXEC_Z_VEL/Z_ACC) */ uint16_t vel_xy = ACTUATOR_EXEC_VEL; uint8_t acc_xy = ACTUATOR_EXEC_ACC; 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; uint16_t src_y = frag->source.y; uint16_t src_angle = frag->source.angle_x10; uint16_t tgt_x = frag->target.x; uint16_t tgt_y = frag->target.y; uint16_t tgt_angle = frag->target.angle_x10; /* 协议角度: source=0 (保持当前), target=相对转角 [0,360); 执行时自动选行程内与当前位置最近的等价角 */ float src_angle_deg = (float)src_angle * 0.1f; float tgt_angle_deg = (float)tgt_angle * 0.1f; /* 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 坐标 → 脉冲数 */ uint32_t src_pulses_x = (uint32_t)(src_x * s_scale_x); uint32_t src_pulses_y = (uint32_t)(src_y * s_scale_y); /* target 坐标 → 脉冲数 */ uint32_t dst_pulses_x = (uint32_t)(tgt_x * s_scale_x); uint32_t dst_pulses_y = (uint32_t)(tgt_y * s_scale_y); ESP_LOGI(TAG, "Fragment: SRC(X=%lu Y=%lu A=%.1f) -> DST(X=%lu Y=%lu A=%.1f comp x=%u y=%u)", (unsigned long)src_pulses_x, (unsigned long)src_pulses_y, (double)src_angle_deg, (unsigned long)dst_pulses_x, (unsigned long)dst_pulses_y, (double)tgt_angle_deg, tgt_x, tgt_y); /* 到位轮询掩码与超时基准: 一律按“当前实际位置 → 目标”的真实行程计算, 而不是按绝对坐标 (原点) 估算。绝对坐标会把已在目标位/离原点很远的轴 算成大行程 → 无行程也等满超时 (初始==目标时整段空等), 且超时被放大 */ float src_target = _closest_reachable_angle(src_angle_deg, s_cur_angle_deg, ACTUATOR_ANGLE_LIMIT_DEG); /* 各轴真实行程 (脉冲): 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_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_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 位置 ---- */ 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); _frame_gap(); _actuator_move_y_sync(src_pulses_y, vel_xy, acc_xy); /* ---- 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); /* ---- 5. 磁铁吸合 ---- */ magnet_set(true); _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200); /* ---- 6. 舵机抬起 ---- */ servo_set_up(); _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); /* 真实行程: 当前 (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_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_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_dy, vel_xy, acc_xy); if (t_wait > dst_wait_ms) dst_wait_ms = t_wait; 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); _frame_gap(); _actuator_move_y_sync(dst_pulses_y, vel_xy, acc_xy); /* ---- 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); /* ---- 10. 磁铁掉电 ---- */ magnet_set(false); _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200); /* ---- 11. 舵机抬起 ---- */ servo_set_up(); _wait_flash(ACTUATOR_GRIP_SETTLE_MS, 128, 0, 0, 200); } /* ================================================================ * 执行完整 Cycle * ================================================================ */ int actuator_execute_cycle(const edc_cycle_t *cycle) { if (!s_initialized) { ESP_LOGE(TAG, "未初始化, 无法执行 Cycle"); return -1; } if (!cycle) { ESP_LOGE(TAG, "Cycle 指针为空"); return -1; } if (cycle->frag_count < EDC_FRAG_MIN || cycle->frag_count > EDC_FRAG_MAX) { ESP_LOGE(TAG, "Fragment 数量无效: %u (需 %d~%d)", cycle->frag_count, EDC_FRAG_MIN, EDC_FRAG_MAX); return -1; } /* 保存快照 (执行前先存, 即使执行中也可查询) */ memcpy(&s_last_cycle, cycle, sizeof(edc_cycle_t)); s_last_cycle_valid = true; ESP_LOGI(TAG, "=== Cycle #%u 开始 (%u fragments) ===", cycle->cycle_id, cycle->frag_count); for (int i = 0; i < cycle->frag_count; i++) { ESP_LOGI(TAG, "-- Fragment %d/%d --", i + 1, cycle->frag_count); _actuator_move_fragment(&cycle->fragments[i]); } ESP_LOGI(TAG, "=== Cycle #%u 完成 ===", cycle->cycle_id); return 0; } /* ================================================================ * 协议回调: 收到 Cycle → 执行 → DONE * ================================================================ */ void edc_proto_on_cycle(const edc_cycle_t *cycle, edc_channel_t ch) { static uint16_t s_last_exec_id = 0xFFFF; ESP_LOGI(TAG, "Cycle #%u via %s, %u frags", cycle->cycle_id, ch == EDC_CH_I2C ? "I2C" : "UART", cycle->frag_count); /* I2C / UART 冗余下发, 同一 cycle_id 只执行一次 */ if (cycle->cycle_id == s_last_exec_id) { ESP_LOGD(TAG, "Cycle #%u 重复, 跳过执行", cycle->cycle_id); return; } s_last_exec_id = cycle->cycle_id; /* 新一轮执行开始: 复位 RGB 状态 (执行期间 _wait_flash 显示红灯) */ rgb_set_state(RGB_STATE_IDLE); /* 执行电机运动 (阻塞, 含拾放全流程) */ ESP_LOGI(TAG, "开始执行 Cycle #%u ...", cycle->cycle_id); actuator_execute_cycle(cycle); /* 完成后执行机构置零 (归位): 舵机抬起 + Z/X/Y 三轴回零点 */ actuator_home_all(); /* 全部执行完毕 (含置零) 后回复 DONE */ edc_proto_send_done(cycle->cycle_id); ESP_LOGI(TAG, "Cycle #%u 执行完毕, DONE 已发送", cycle->cycle_id); /* 设置 RGB 状态为 DONE, 主循环将持续绿灯慢闪 */ rgb_set_state(RGB_STATE_DONE); } /* ================================================================ * 急停 * ================================================================ */ void actuator_stop_all(void) { const uint8_t addrs[] = { ACTUATOR_ADDR_ANGLE, ACTUATOR_ADDR_X, ACTUATOR_ADDR_Y1, ACTUATOR_ADDR_Y2 }; _frame_gap(); /* 防止与上一帧 UART 冲突 */ for (int i = 0; i < 4; i++) { Emm_V5_Stop_Now(addrs[i], false); _frame_gap(); } ESP_LOGW(TAG, "全部电机急停"); } /* ================================================================ * 舵机控制 * ================================================================ */ void actuator_servo_up(void) { servo_set_up(); ESP_LOGD(TAG, "舵机: 抬起"); } void actuator_servo_down(void) { servo_set_down(); ESP_LOGD(TAG, "舵机: 放下"); } void actuator_servo_set_angle(float angle) { servo_set_angle(angle); ESP_LOGD(TAG, "舵机: %.1f°", (double)angle); } /* ================================================================ * 单电机手动控制 (Y 轴自动同步) * ================================================================ */ static inline bool _is_y_motor(uint8_t addr) { return (addr == ACTUATOR_ADDR_Y1 || addr == ACTUATOR_ADDR_Y2); } void actuator_motor_enable(uint8_t addr, bool state) { _frame_gap(); /* 防止与上一帧 UART 冲突 */ Emm_V5_En_Control(addr, state, false); ESP_LOGI(TAG, "电机 %d %s", addr, state ? "使能" : "关闭"); } void actuator_motor_vel(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc) { _frame_gap(); /* 防止与上一帧 UART 冲突 */ Emm_V5_Vel_Control(addr, dir, vel, acc, false); ESP_LOGI(TAG, "速度: 电机%d %s %dRPM acc=%d", addr, dir ? "CCW" : "CW", vel, acc); /* Y 轴同步: 对端电机同样命令 (帧间留间隔) */ if (_is_y_motor(addr)) { uint8_t peer = (addr == ACTUATOR_ADDR_Y1) ? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1; _frame_gap(); Emm_V5_Vel_Control(peer, dir, vel, acc, false); ESP_LOGD(TAG, "Y 同步: 电机%d 同速", peer); } } void actuator_motor_pos(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF) { _frame_gap(); /* 防止与上一帧 UART 冲突 */ if (_is_y_motor(addr)) { /* Y 轴双电机同步位置 (帧间留间隔, 广播地址触发一次) */ uint8_t peer = (addr == ACTUATOR_ADDR_Y1) ? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1; Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, true); _frame_gap(); 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 ? "绝对" : "相对"); } } void actuator_motor_angle(uint8_t addr, float angle_deg, uint16_t vel, uint8_t acc, bool raF) { _frame_gap(); /* 防止与上一帧 UART 冲突 */ /* 方向由 Emm_V5_Pos_Control_Angle 根据角度正负自动翻转 */ uint8_t dir = ACTUATOR_DIR_ANGLE; if (_is_y_motor(addr)) { uint8_t peer = (addr == ACTUATOR_ADDR_Y1) ? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1; Emm_V5_Pos_Control_Angle(addr, dir, vel, acc, angle_deg, raF, true); _frame_gap(); Emm_V5_Pos_Control_Angle(peer, dir, vel, acc, angle_deg, raF, true); _frame_gap(); Emm_V5_Synchronous_motion(0); ESP_LOGI(TAG, "角度(Y同步): 电机%d+%d %.1f° %s vel=%dRPM", addr, peer, (double)angle_deg, raF ? "绝对" : "相对", vel); } else { /* 跟踪命令位置 (供 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, raF ? "绝对" : "相对", vel); } } void actuator_motor_mm(uint8_t addr, float distance_mm, uint16_t vel, uint8_t acc, bool raF) { /* 软限位钳位 */ if (_is_y_motor(addr)) { if (distance_mm < ACTUATOR_LIMIT_Y_MIN) distance_mm = ACTUATOR_LIMIT_Y_MIN; if (distance_mm > ACTUATOR_LIMIT_Y_MAX) distance_mm = ACTUATOR_LIMIT_Y_MAX; } else { if (distance_mm < ACTUATOR_LIMIT_X_MIN) distance_mm = ACTUATOR_LIMIT_X_MIN; if (distance_mm > ACTUATOR_LIMIT_X_MAX) distance_mm = ACTUATOR_LIMIT_X_MAX; } _frame_gap(); /* 防止与上一帧 UART 冲突 */ uint32_t clk = ACTUATOR_MM_TO_PULSES(distance_mm); /* Y 轴正向 = CCW, X 轴正向 = CW */ uint8_t dir = _is_y_motor(addr) ? ACTUATOR_DIR_Y : ACTUATOR_DIR_X; if (_is_y_motor(addr)) { uint8_t peer = (addr == ACTUATOR_ADDR_Y1) ? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1; Emm_V5_Pos_Control(addr, dir, vel, acc, clk, raF, true); _frame_gap(); 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); } } void actuator_motor_stop(uint8_t addr) { _frame_gap(); /* 防止与上一帧 UART 冲突 */ Emm_V5_Stop_Now(addr, false); ESP_LOGI(TAG, "停止: 电机%d", addr); /* Y 轴同步停止 (帧间留间隔) */ if (_is_y_motor(addr)) { uint8_t peer = (addr == ACTUATOR_ADDR_Y1) ? ACTUATOR_ADDR_Y2 : ACTUATOR_ADDR_Y1; _frame_gap(); Emm_V5_Stop_Now(peer, false); ESP_LOGD(TAG, "Y 同步: 电机%d 同停", peer); } } /* ================================================================ * 执行机构置零 (归位) * * 舵机抬起 + 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 快照查询 * ================================================================ */ const edc_cycle_t *actuator_get_last_cycle(void) { return s_last_cycle_valid ? &s_last_cycle : NULL; } bool actuator_is_last_cycle_valid(void) { return s_last_cycle_valid; } void actuator_clear_last_cycle(void) { s_last_cycle_valid = false; }