diff --git a/components/actuator/actuator.c b/components/actuator/actuator.c index a6fa9bd..25b3c64 100644 --- a/components/actuator/actuator.c +++ b/components/actuator/actuator.c @@ -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 快照查询 * ================================================================ */ diff --git a/components/actuator/actuator.h b/components/actuator/actuator.h index f1fb700..311bdef 100644 --- a/components/actuator/actuator.h +++ b/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 路电机 */ diff --git a/components/emm_v5/CMakeLists.txt b/components/emm_v5/CMakeLists.txt index e246c17..5816b7f 100644 --- a/components/emm_v5/CMakeLists.txt +++ b/components/emm_v5/CMakeLists.txt @@ -1,3 +1,3 @@ idf_component_register(SRCS "emm_v5.c" INCLUDE_DIRS "." - REQUIRES esp_driver_uart esp_driver_gpio driver esp_timer) + REQUIRES esp_driver_uart esp_driver_gpio driver esp_timer freertos) diff --git a/components/emm_v5/emm_v5.c b/components/emm_v5/emm_v5.c index c7e04bc..30b8e51 100644 --- a/components/emm_v5/emm_v5.c +++ b/components/emm_v5/emm_v5.c @@ -8,6 +8,9 @@ #include #include "freertos/FreeRTOS.h" #include "freertos/task.h" +#include "freertos/queue.h" +#include "freertos/semphr.h" +#include "freertos/event_groups.h" #include "esp_timer.h" static const char *TAG = "EMM_V5"; @@ -15,6 +18,40 @@ static const char *TAG = "EMM_V5"; /* 运行时配置 (由 Emm_V5_Init 设置) */ static uart_port_t s_uart_port; +/* ============================================================ + * RX 中断/事件驱动状态 + * + * 驱动 Response 菜单设为 Reached/Both/Other 后, 位置模式 (0xFD) 命令 + * 运动到位会主动回传到位帧: 地址 + FD + 9F + 6B。 + * UART RX 由内部任务独占读取, 解析出到位帧后置位事件组位 + * (bit = 电机地址 - 1); 调用方用 Emm_V5_Wait_Arrival 事件等待, + * 全部到位即返回 —— 不再轮询 S_TPOS/S_CPOS, 避免"没到位就下一步"。 + * ============================================================ */ + +#define EMM_V5_ARRIVAL_MAX_MOTORS 8 + +static EventGroupHandle_t s_arrival_evt = NULL; +static QueueHandle_t s_uart_evt_queue = NULL; +static TaskHandle_t s_rx_task = NULL; + +/* 请求-响应 (网页手动读参数等) */ +static SemaphoreHandle_t s_resp_mutex = NULL; +static struct { + bool active; + uint8_t addr; + uint8_t func; + uint8_t *out; + uint8_t max; + uint8_t len; + SemaphoreHandle_t done; +} s_resp; + +/* RX 帧累积缓冲 (0x6B 为帧尾) */ +static uint8_t s_rx_frame[64]; +static uint8_t s_rx_frame_len; + +static void emm_v5_rx_task(void *arg); /* 前置声明 (Emm_V5_Init 中创建) */ + /* ============================================================ * 初始化 * ============================================================ */ @@ -32,10 +69,13 @@ void Emm_V5_Init(uart_port_t port, int tx_pin, int rx_pin) .source_clk = UART_SCLK_DEFAULT, }; + /* 事件队列: 中断驱动 RX (到位帧解析); 64 深度防突发丢事件 */ + s_uart_evt_queue = xQueueCreate(64, sizeof(uart_event_t)); + ESP_ERROR_CHECK(uart_driver_install(s_uart_port, EMM_V5_UART_BUF_SIZE * 2, EMM_V5_UART_BUF_SIZE * 2, - 0, NULL, 0)); + 16, &s_uart_evt_queue, 0)); ESP_ERROR_CHECK(uart_param_config(s_uart_port, &uart_cfg)); ESP_ERROR_CHECK(uart_set_pin(s_uart_port, tx_pin, @@ -43,7 +83,15 @@ void Emm_V5_Init(uart_port_t port, int tx_pin, int rx_pin) UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE)); - ESP_LOGI(TAG, "初始化完成 (UART%d, TX=%d, RX=%d)", + /* 到位事件组 / 请求-响应信号量 / RX 任务 */ + if (s_arrival_evt == NULL) s_arrival_evt = xEventGroupCreate(); + if (s_resp_mutex == NULL) s_resp_mutex = xSemaphoreCreateMutex(); + if (s_resp.done == NULL) s_resp.done = xSemaphoreCreateBinary(); + if (s_rx_task == NULL) { + xTaskCreate(emm_v5_rx_task, "emm_rx", 3072, NULL, 5, &s_rx_task); + } + + ESP_LOGI(TAG, "初始化完成 (UART%d, TX=%d, RX=%d, 到位帧中断已启用)", s_uart_port, tx_pin, rx_pin); } @@ -57,31 +105,178 @@ static inline void Emm_V5_Send(const uint8_t *cmd, uint8_t len) } /* ============================================================ - * 接收数据 + * RX 中断/事件驱动 + * + * 帧格式: 地址 + 功能码 + 数据... + 0x6B (校验字节) + * 到位帧: 地址 + FD + 9F + 6B (位置模式命令运动到位后主动回传) + * ============================================================ */ + +/* 处理一帧完整数据 (以 0x6B 结尾) */ +static void emm_v5_handle_frame(const uint8_t *f, uint8_t len) +{ + if (len < 4) return; + + /* 到位帧: 地址 + FD + 9F + 6B → 置位事件组 (bit = 地址 - 1) */ + if (f[1] == 0xFD && f[2] == 0x9F) { + uint8_t addr = f[0]; + if (addr >= 1 && addr <= EMM_V5_ARRIVAL_MAX_MOTORS && s_arrival_evt) { + xEventGroupSetBits(s_arrival_evt, (EventBits_t)(1u << (addr - 1))); + ESP_LOGI(TAG, "到位帧: 电机%d", addr); + } + } + + /* 请求-响应匹配 (网页手动读参数等) */ + if (s_resp_mutex) { + xSemaphoreTake(s_resp_mutex, portMAX_DELAY); + if (s_resp.active && f[0] == s_resp.addr && f[1] == s_resp.func) { + uint8_t n = (len < s_resp.max) ? len : s_resp.max; + if (s_resp.out && n) memcpy(s_resp.out, f, n); + s_resp.len = n; + s_resp.active = false; + xSemaphoreGive(s_resp.done); + } + xSemaphoreGive(s_resp_mutex); + } +} + +/* UART RX 独占任务: 事件队列驱动, 解析帧并分派 */ +static void emm_v5_rx_task(void *arg) +{ + uart_event_t evt; + uint8_t buf[64]; + + while (1) { + if (xQueueReceive(s_uart_evt_queue, &evt, portMAX_DELAY) != pdTRUE) + continue; + + if (evt.type == UART_DATA || evt.type == UART_FIFO_OVF || + evt.type == UART_BUFFER_FULL) { + int len; + while ((len = uart_read_bytes(s_uart_port, buf, sizeof(buf), 0)) > 0) { + for (int i = 0; i < len; i++) { + s_rx_frame[s_rx_frame_len++] = buf[i]; + if (s_rx_frame_len >= sizeof(s_rx_frame)) { + s_rx_frame_len = 0; /* 缓冲溢出: 重新同步 */ + } + if (buf[i] == 0x6B) { /* 帧尾 */ + emm_v5_handle_frame(s_rx_frame, s_rx_frame_len); + s_rx_frame_len = 0; + } + } + } + if (evt.type == UART_FIFO_OVF || evt.type == UART_BUFFER_FULL) { + /* 溢出只说明有部分字节被丢弃: 重置帧累积重新同步即可。 + 不能 uart_flush_input —— 那会把缓冲里仍有效的到位帧一并冲掉, + 是偶发丢电机2/3/4 到位帧的根因之一 */ + s_rx_frame_len = 0; + } + } + } +} + +/* 内部: 发送命令并等待匹配的响应帧 (地址+功能码), 供读函数复用 */ +static int emm_v5_request_response(const uint8_t *cmd, uint8_t cmd_len, + uint8_t addr, uint8_t func, + uint8_t *out, uint8_t max, + uint32_t timeout_ms) +{ + if (!s_resp_mutex || !s_resp.done) return -1; + + xSemaphoreTake(s_resp_mutex, portMAX_DELAY); + if (s_resp.active) { + xSemaphoreGive(s_resp_mutex); + ESP_LOGW(TAG, "请求-响应忙, 放弃 (addr=%u func=0x%02X)", addr, func); + return -1; + } + s_resp.active = true; + s_resp.addr = addr; + s_resp.func = func; + s_resp.out = out; + s_resp.max = max; + s_resp.len = 0; + xSemaphoreGive(s_resp_mutex); + + xSemaphoreTake(s_resp.done, 0); /* 清残留计数 */ + Emm_V5_Send(cmd, cmd_len); + + if (xSemaphoreTake(s_resp.done, pdMS_TO_TICKS(timeout_ms)) != pdTRUE) { + xSemaphoreTake(s_resp_mutex, portMAX_DELAY); + s_resp.active = false; /* 放弃等待 */ + xSemaphoreGive(s_resp_mutex); + ESP_LOGW(TAG, "响应超时 (addr=%u func=0x%02X)", addr, func); + return -1; + } + + xSemaphoreTake(s_resp_mutex, portMAX_DELAY); + int n = s_resp.len; + xSemaphoreGive(s_resp_mutex); + return n; +} + +/* ============================================================ + * 到位等待 (事件驱动) + * + * 注意: Emm_V5_Wait_Arrival 不消费到位位 (便于调用方分段等待时闪烁 LED); + * 一段 motion 的等待结束后必须调用 Emm_V5_Arrival_Consume 清除到位位, + * 防止残留位污染下一段 motion。 + * ============================================================ */ + +bool Emm_V5_Wait_Arrival(const uint8_t *addrs, uint8_t count, uint32_t timeout_ms) +{ + if (!s_arrival_evt) return false; + + EventBits_t expected = 0; + for (uint8_t i = 0; i < count; i++) { + uint8_t a = addrs[i]; + if (a >= 1 && a <= EMM_V5_ARRIVAL_MAX_MOTORS) + expected |= (EventBits_t)(1u << (a - 1)); + } + if (expected == 0) return true; + + EventBits_t got = xEventGroupWaitBits(s_arrival_evt, expected, + pdFALSE, pdTRUE, + pdMS_TO_TICKS(timeout_ms)); + return ((got & expected) == expected); +} + +void Emm_V5_Arrival_Consume(const uint8_t *addrs, uint8_t count) +{ + if (!s_arrival_evt) return; + + EventBits_t expected = 0; + for (uint8_t i = 0; i < count; i++) { + uint8_t a = addrs[i]; + if (a >= 1 && a <= EMM_V5_ARRIVAL_MAX_MOTORS) + expected |= (EventBits_t)(1u << (a - 1)); + } + if (expected) + xEventGroupClearBits(s_arrival_evt, expected); +} + +void Emm_V5_Arrival_Reset(void) +{ + if (!s_arrival_evt) return; + xEventGroupClearBits(s_arrival_evt, 0xFF); /* 清 addr 1~8 对应位 (bit=地址-1) */ +} + +/* 查询当前已置位的到位位 (调试用): 返回 addr1~8 对应位掩码 */ +uint32_t Emm_V5_Arrival_Get_Bits(void) +{ + if (!s_arrival_evt) return 0; + return xEventGroupGetBits(s_arrival_evt) & 0xFF; +} + +/* ============================================================ + * 接收数据 (已废弃) * ============================================================ */ uint8_t Emm_V5_Receive_Data(uint8_t *rxCmd, uint8_t maxLen) { - uint8_t count = 0; - int64_t last_time, now; - - last_time = esp_timer_get_time(); - - while (1) { - int len = uart_read_bytes(s_uart_port, &rxCmd[count], 1, - pdMS_TO_TICKS(10)); - if (len > 0) { - count++; - if (count >= maxLen) break; - last_time = esp_timer_get_time(); - } else { - now = esp_timer_get_time(); - if ((now - last_time) > (EMM_V5_RX_TIMEOUT_MS * 1000)) { - break; - } - } - } - return count; + /* 已废弃: UART RX 由内部 RX 任务独占 (中断驱动), 此函数不再直接读 UART; + 网页手动读参数请改用 Emm_V5_Read_Sys_Params_Resp */ + (void)rxCmd; + (void)maxLen; + return 0; } /* ============================================================ @@ -100,33 +295,62 @@ void Emm_V5_Reset_Clog_Pro(uint8_t addr) Emm_V5_Send(cmd, 4); } +/* 系统参数 → 功能码 (读取响应帧按 地址+功能码 匹配) */ +static uint8_t emm_v5_sysparam_func(EmmSysParams_t s) +{ + switch (s) { + case S_VER: return 0x1F; + case S_RL: return 0x20; + case S_PID: return 0x21; + case S_VBUS: return 0x24; + case S_CPHA: return 0x27; + case S_ENCL: return 0x31; + case S_TPOS: return 0x33; + case S_VEL: return 0x35; + case S_CPOS: return 0x36; + case S_PERR: return 0x37; + case S_FLAG: return 0x3A; + case S_ORG: return 0x3B; + case S_Conf: return 0x42; + case S_State: return 0x43; + default: return 0x00; + } +} + void Emm_V5_Read_Sys_Params(uint8_t addr, EmmSysParams_t s) { uint8_t cmd[8] = {0}; uint8_t i = 0; + uint8_t func = emm_v5_sysparam_func(s); cmd[i++] = addr; - switch (s) { - case S_VER: cmd[i++] = 0x1F; break; - case S_RL: cmd[i++] = 0x20; break; - case S_PID: cmd[i++] = 0x21; break; - case S_VBUS: cmd[i++] = 0x24; break; - case S_CPHA: cmd[i++] = 0x27; break; - case S_ENCL: cmd[i++] = 0x31; break; - case S_TPOS: cmd[i++] = 0x33; break; - case S_VEL: cmd[i++] = 0x35; break; - case S_CPOS: cmd[i++] = 0x36; break; - case S_PERR: cmd[i++] = 0x37; break; - case S_FLAG: cmd[i++] = 0x3A; break; - case S_ORG: cmd[i++] = 0x3B; break; - case S_Conf: cmd[i++] = 0x42; cmd[i++] = 0x6C; break; - case S_State: cmd[i++] = 0x43; cmd[i++] = 0x7A; break; - default: break; - } + if (func) cmd[i++] = func; + if (s == S_Conf) cmd[i++] = 0x6C; + if (s == S_State) cmd[i++] = 0x7A; cmd[i++] = 0x6B; Emm_V5_Send(cmd, i); } +uint8_t Emm_V5_Read_Sys_Params_Resp(uint8_t addr, EmmSysParams_t s, + uint8_t *rxOut, uint8_t maxLen, + uint32_t timeout_ms) +{ + uint8_t cmd[8] = {0}; + uint8_t i = 0; + uint8_t func = emm_v5_sysparam_func(s); + + cmd[i++] = addr; + if (func) cmd[i++] = func; + if (s == S_Conf) cmd[i++] = 0x6C; + if (s == S_State) cmd[i++] = 0x7A; + cmd[i++] = 0x6B; + + int n = emm_v5_request_response(cmd, i, addr, func, rxOut, maxLen, + (timeout_ms > 0) ? timeout_ms + : EMM_V5_RX_TIMEOUT_MS); + return (n > 0) ? (uint8_t)n : 0; +} + /* ============================================================ * 读取电机状态标志 (S_FLAG 0x3A) 并解析返回 * ============================================================ */ @@ -137,29 +361,10 @@ uint8_t Emm_V5_Read_Status_Flag(uint8_t addr, uint32_t timeout_ms) uint8_t cmd[3] = {addr, 0x3A, 0x6B}; uint8_t read_to = (timeout_ms > 0) ? timeout_ms : EMM_V5_RX_TIMEOUT_MS; - /* 清掉历史残留 (运动命令的 ACK/到位帧等), 防止误判 */ - uart_flush_input(s_uart_port); - - Emm_V5_Send(cmd, 3); - - uint8_t n = 0; - int64_t last = esp_timer_get_time(); - int64_t deadline = last + (int64_t)read_to * 1000; - while (esp_timer_get_time() < deadline) { - int len = uart_read_bytes(s_uart_port, &rx[n], 1, pdMS_TO_TICKS(5)); - if (len > 0) { - if (n < sizeof(rx) - 1) n++; - last = esp_timer_get_time(); - /* 帧收完整 (地址 + 3A + 状态 + 6B) 立即返回, 不再等空闲超时, - 把单次轮询从 ~17ms 压到 ~3ms, 减小到位检测抖动 */ - for (int i = 0; i + 3 < (int)n; i++) { - if (rx[i] == addr && rx[i + 1] == 0x3A && rx[i + 3] == 0x6B) - return rx[i + 2]; - } - } else if ((esp_timer_get_time() - last) > 5000) { - break; /* 5ms 无新字节: 认为帧已收完/无响应 */ - } - } + int n = emm_v5_request_response(cmd, sizeof(cmd), addr, 0x3A, + rx, sizeof(rx), read_to); + if (n >= 4 && rx[0] == addr && rx[1] == 0x3A && rx[3] == 0x6B) + return rx[2]; return 0xFF; } @@ -174,34 +379,15 @@ static int32_t Emm_V5_Read_Pos(uint8_t addr, uint8_t func) { uint8_t rx[16] = {0}; uint8_t cmd[3] = {addr, func, 0x6B}; - uint8_t read_to = EMM_V5_RX_TIMEOUT_MS; - /* 清掉历史残留 (ACK/到位帧等), 防止误判 */ - uart_flush_input(s_uart_port); - - Emm_V5_Send(cmd, 3); - - uint8_t n = 0; - int64_t last = esp_timer_get_time(); - int64_t deadline = last + (int64_t)read_to * 1000; - while (esp_timer_get_time() < deadline) { - int len = uart_read_bytes(s_uart_port, &rx[n], 1, pdMS_TO_TICKS(5)); - if (len > 0) { - if (n < sizeof(rx) - 1) n++; - last = esp_timer_get_time(); - /* 帧收完整 (地址+功能码+符号+位置4B+6B) 立即返回 */ - for (int i = 0; i + 7 < (int)n; i++) { - if (rx[i] == addr && rx[i + 1] == func && - (rx[i + 2] == 0 || rx[i + 2] == 1) && rx[i + 7] == 0x6B) - return (int32_t)(((uint32_t)rx[i + 3] << 24) | - ((uint32_t)rx[i + 4] << 16) | - ((uint32_t)rx[i + 5] << 8) | - (uint32_t)rx[i + 6]); - } - } else if ((esp_timer_get_time() - last) > 5000) { - break; /* 5ms 无新字节: 认为帧已收完/无响应 */ - } - } + int n = emm_v5_request_response(cmd, sizeof(cmd), addr, func, + rx, sizeof(rx), EMM_V5_RX_TIMEOUT_MS); + if (n >= 8 && rx[0] == addr && rx[1] == func && + (rx[2] == 0 || rx[2] == 1) && rx[7] == 0x6B) + return (int32_t)(((uint32_t)rx[3] << 24) | + ((uint32_t)rx[4] << 16) | + ((uint32_t)rx[5] << 8) | + (uint32_t)rx[6]); return -1; } diff --git a/components/emm_v5/emm_v5.h b/components/emm_v5/emm_v5.h index 9b889a3..1ba3866 100644 --- a/components/emm_v5/emm_v5.h +++ b/components/emm_v5/emm_v5.h @@ -25,7 +25,7 @@ extern "C" { * 可配置常量 * ============================================================ */ #define EMM_V5_UART_BAUDRATE 115200 -#define EMM_V5_UART_BUF_SIZE 256 +#define EMM_V5_UART_BUF_SIZE 512 /* 接收超时时间 (毫秒) */ #define EMM_V5_RX_TIMEOUT_MS 100 @@ -220,10 +220,63 @@ void Emm_V5_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF); void Emm_V5_Origin_Interrupt(uint8_t addr); /** - * @brief 接收电机返回数据 (100ms 超时判定帧结束) - * @param rxCmd 接收缓冲区 - * @param maxLen 缓冲区最大长度 - * @return 实际接收到的字节数 + * @brief 等待指定电机到位 (事件驱动, 不消费到位位) + * + * 电机驱动 Response 菜单设为 Reached/Both/Other 后, 位置模式 (0xFD) 命令 + * 运动到位会主动回传到位帧 (地址 + FD + 9F + 6B)。UART RX 由本组件内部 + * 任务中断读取并解析, 此处事件等待全部到位, 不再轮询 S_TPOS/S_CPOS。 + * + * @note 本函数不清除到位位, 便于调用方分段等待 (如等待期间闪烁 LED); + * 一段 motion 等待结束后必须调用 Emm_V5_Arrival_Consume 清除, + * 防止残留位污染下一段 motion。 + * @param addrs 电机地址数组, 如 {ACTUATOR_ADDR_X} + * @param count 数组长度 + * @param timeout_ms 本次等待超时 (ms); 0 = 立即返回当前状态 + * @return true=全部到位, false=超时/未到齐 + */ +bool Emm_V5_Wait_Arrival(const uint8_t *addrs, uint8_t count, uint32_t timeout_ms); + +/** + * @brief 消费 (清除) 指定电机的到位位 + * @param addrs 电机地址数组 + * @param count 数组长度 + * @note 一段 motion 的到位等待结束后调用 (成功/超时都要调) + */ +void Emm_V5_Arrival_Consume(const uint8_t *addrs, uint8_t count); + +/** + * @brief 清除全部电机到位位 (开始新一段 motion 前调用) + * @note 防止上一段 motion 超时后迟到的到位帧 (或 0 脉冲命令仍回传的帧) + * 污染新一段 motion 的到位等待 + */ +void Emm_V5_Arrival_Reset(void); + +/** + * @brief 查询当前已置位的到位位 (调试用) + * @return 位掩码: bit = (电机地址 - 1), 仅低 8 位有效 + */ +uint32_t Emm_V5_Arrival_Get_Bits(void); + +/** + * @brief 发送读取系统参数命令并等待响应帧返回 (请求-响应式) + * @param addr 电机地址 + * @param s 系统参数类型 (@see EmmSysParams_t) + * @param rxOut 接收缓冲区 + * @param maxLen 缓冲区最大长度 + * @param timeout_ms 等待响应超时 (ms) + * @return 实际收到的响应字节数 (含 地址/功能码/.../0x6B), 失败返回 0 + */ +uint8_t Emm_V5_Read_Sys_Params_Resp(uint8_t addr, EmmSysParams_t s, + uint8_t *rxOut, uint8_t maxLen, + uint32_t timeout_ms); + +/** + * @brief 接收电机返回数据 + * @deprecated UART RX 已由内部 RX 任务独占 (中断驱动), 此函数不再读取 UART; + * 网页手动读参数请改用 Emm_V5_Read_Sys_Params_Resp + * @param rxCmd 接收缓冲区 (保留, 未使用) + * @param maxLen 缓冲区最大长度 (保留, 未使用) + * @return 恒为 0 */ uint8_t Emm_V5_Receive_Data(uint8_t *rxCmd, uint8_t maxLen); diff --git a/components/protocol/PROTOCOL.md b/components/protocol/PROTOCOL.md index b0193d6..415a30f 100644 --- a/components/protocol/PROTOCOL.md +++ b/components/protocol/PROTOCOL.md @@ -55,8 +55,10 @@ Cycle = { cycle_id: uint16, frag_count: uint8 (2~4), fragments: Fragment[] | 命令 | 值 | 方向 | 说明 | |------|-----|------|------| | `CYCLE_DATA` | `0x01` | MPU → MCU | 下发 Cycle 数据 | -| `CYCLE_DONE` | `0x02` | MCU → MPU | Cycle 执行完成 | +| `CYCLE_DONE` | `0x02` | MCU → MPU | Cycle 执行完成 (MPU 同时终止任务计时) | | `CYCLE_REQUEST` | `0x03` | MCU → MPU | 请求下一个 Cycle | +| `RECOGNIZE` | `0x04` | MCU → MPU | 触发视觉识别 | +| `TASK_START` | `0x05` | MCU → MPU | 任务开始 (按钮按下即发, 与 RECOGNIZE 区分; MPU 据此开始计时) | | `ACK` | `0x80` | 双向 | 通用应答 (预留) | | `NACK` | `0xFF` | 双向 | 错误 (预留) | diff --git a/components/protocol/protocol.c b/components/protocol/protocol.c index 975b129..7124db2 100644 --- a/components/protocol/protocol.c +++ b/components/protocol/protocol.c @@ -460,6 +460,14 @@ void edc_proto_send_recognize(void) ESP_LOGI(TAG, "RECOGNIZE sent"); } +void edc_proto_send_task_start(void) +{ + if (!s_uart_enabled) return; + uart_send_frame(EDC_CMD_TASK_START, NULL, 0); + s_stat.request_sent++; + ESP_LOGI(TAG, "TASK_START sent"); +} + /* ================================================================ * 统计 * ================================================================ */ diff --git a/components/protocol/protocol.h b/components/protocol/protocol.h index 602b242..7856bf8 100644 --- a/components/protocol/protocol.h +++ b/components/protocol/protocol.h @@ -77,6 +77,7 @@ typedef enum { EDC_CMD_CYCLE_DONE = 0x02, /* MCU → MPU: Cycle 执行完成 */ EDC_CMD_CYCLE_REQUEST = 0x03, /* MCU → MPU: 请求下一个 Cycle */ EDC_CMD_RECOGNIZE = 0x04, /* MCU → MPU: 触发视觉识别 */ + EDC_CMD_TASK_START = 0x05, /* MCU → MPU: 任务开始 (按钮按下即发, 与 RECOGNIZE 区分) */ EDC_CMD_ACK = 0x80, /* 通用应答 */ EDC_CMD_NACK = 0xFF, /* 错误 */ } edc_cmd_t; @@ -215,6 +216,12 @@ void edc_proto_send_request(void); */ void edc_proto_send_recognize(void); +/** + * @brief 发送 TASK_START 通知 MPU 任务开始计时 (通过 UART, 按钮按下即发, + * 与 RECOGNIZE 区分, 不触发识别) + */ +void edc_proto_send_task_start(void); + /** * @brief 获取统计信息 */ diff --git a/components/web_ctrl/web_ctrl.c b/components/web_ctrl/web_ctrl.c index 6d0b845..09abccf 100644 --- a/components/web_ctrl/web_ctrl.c +++ b/components/web_ctrl/web_ctrl.c @@ -685,10 +685,9 @@ static esp_err_t api_read_param_handler(httpd_req_t *req) int addr = get_query_int(req, "addr", 1); int param = get_query_int(req, "param", 0); - Emm_V5_Read_Sys_Params((uint8_t)addr, (EmmSysParams_t)param); - uint8_t rx[128]; - uint8_t len = Emm_V5_Receive_Data(rx, sizeof(rx)); + uint8_t len = Emm_V5_Read_Sys_Params_Resp((uint8_t)addr, (EmmSysParams_t)param, + rx, sizeof(rx), 200); uint32_t val32 = 0; uint16_t val16 = 0; diff --git a/main/main.c b/main/main.c index cf00b7a..63b48ee 100644 --- a/main/main.c +++ b/main/main.c @@ -22,7 +22,6 @@ * WiFi: 连接 ICE (密码 12345678), 获取 IP 后用浏览器打开控制页面 */ -#include "emm_v5.h" #include "magnet.h" #include "servo.h" #include "web_ctrl.h" @@ -47,35 +46,15 @@ static void button_recognize_task(void *pvParam) { while (1) { if (key_is_pressed()) { - ESP_LOGI(TAG, "按钮按下, 归零后再发送 RECOGNIZE ..."); + ESP_LOGI(TAG, "按钮按下, 先发 TASK_START (MPU 开始计时) ..."); buzzer_beep(2000, 100); /* 短音: 识别开始 */ + /* 按下按钮即通知 MPU 任务开始计时 (与 RECOGNIZE 区分, 不触发识别) */ + edc_proto_send_task_start(); + /* ---- 归零: 舵机抬起 + Z/X/Y 三轴并行回零 ---- - Z 旋转与 X/Y 平移互不干扰 (Fragment 里也是三轴并行), - 无需等 Z 到位再动 X/Y —— 三轴同时下发、一次到位等待 */ - actuator_servo_up(); - - uint32_t z_motion = actuator_angle_motion_time_ms(ACTUATOR_ANGLE_LIMIT_DEG, - ACTUATOR_EXEC_Z_VEL, - ACTUATOR_EXEC_Z_ACC); - uint32_t xy_motion = actuator_mm_motion_time_ms(ACTUATOR_LIMIT_X_MAX, - ACTUATOR_EXEC_VEL, - ACTUATOR_EXEC_ACC); - uint32_t y_motion = actuator_mm_motion_time_ms(ACTUATOR_LIMIT_Y_MAX, - ACTUATOR_EXEC_VEL, - ACTUATOR_EXEC_ACC); - uint32_t home_motion = z_motion; - if (xy_motion > home_motion) home_motion = xy_motion; - if (y_motion > home_motion) home_motion = y_motion; - - actuator_motor_angle(ACTUATOR_ADDR_ANGLE, 0.0f, - ACTUATOR_EXEC_Z_VEL, ACTUATOR_EXEC_Z_ACC, true); - actuator_motor_mm(ACTUATOR_ADDR_X, 0.0f, - ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC, true); - actuator_motor_mm(ACTUATOR_ADDR_Y1, 330.0f, - ACTUATOR_EXEC_VEL, ACTUATOR_EXEC_ACC, true); - actuator_wait_arrival(home_motion, - (1u << 0) | (1u << 1) | (1u << 2) | (1u << 3)); + Cycle 执行完成后也会调用 actuator_home_all() 置零 */ + actuator_home_all(); ESP_LOGI(TAG, "归零完成, 发送 RECOGNIZE ...");