diff --git a/components/actuator/actuator.c b/components/actuator/actuator.c index 0a7378b..339aec7 100644 --- a/components/actuator/actuator.c +++ b/components/actuator/actuator.c @@ -11,6 +11,7 @@ #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "esp_log.h" +#include "esp_timer.h" #include #include @@ -69,6 +70,112 @@ 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 次在容差内才确认: 确保电机已稳定停下 (减速途中/振荡时 + * 位置可能短暂进入容差, 单独一次会"没到位就进行下一步")。 + * + * ⚠️ 防坑: 轮询第一帧前必须留帧间隔 —— 运动命令最后一条是 Y 同步广播 + * 触发 (00 FF 66 6B), 若紧跟发轮询帧会"零间隔背靠背", 驱动板丢字节 + * 吞掉触发帧, 电机不启动。 + * + * @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 + }; + 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; + + for (int i = 0; i < 4; i++) { + if (!(moved_mask & (1u << i))) + arrived[i] = true; /* 无行程: 视为已到位 */ + } + + /* 与上一条运动命令 (尤其 Y 同步触发) 之间留帧间隔, 防丢字节 */ + vTaskDelay(pdMS_TO_TICKS(ACTUATOR_FRAME_GAP_MS)); + + while (esp_timer_get_time() < deadline) { + bool all = true; + for (int i = 0; i < 4; i++) { + if (arrived[i]) + continue; + + 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)); /* 帧间隔防粘包 */ + } + 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 一致) */ + 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]); + return false; +} + /* ================================================================ * 运动到位时间估算 * @@ -110,18 +217,28 @@ 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_settle_ms(float distance_mm, uint16_t vel, uint8_t acc) +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_settle_ms(pulses, vel, acc); + 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) { - 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_settle_ms(pulses, vel, acc); + return actuator_angle_motion_time_ms(angle_deg, vel, acc) + ACTUATOR_SETTLE_MARGIN_MS; } /* ================================================================ @@ -378,15 +495,20 @@ static void _actuator_move_fragment(const edc_fragment_t *frag) (double)tgt_angle_deg, tgt_x, tgt_y); - /* 按实际行程/速度/加速度计算到位等待 (Z/X/Y 并发, 取最长者, 含安全裕量) */ + /* 到位轮询掩码: 只等待实际有行程的电机 (脉冲>0), 无行程直接视为已到位 */ float src_target = _closest_reachable_angle(src_angle_deg, s_cur_angle_deg, ACTUATOR_ANGLE_LIMIT_DEG); - uint32_t src_wait_ms = actuator_settle_ms( - (uint32_t)(fabsf(src_target) * 10.0f * s_scale_angle + 0.5f), - vel_angle, acc_angle); - uint32_t t_wait = actuator_settle_ms(src_pulses_x, vel_xy, acc_xy); + 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 双电机 */ + + /* 纯运动时间 (无裕量), 仅作到位等待的兜底超时基准 */ + 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); if (t_wait > src_wait_ms) src_wait_ms = t_wait; - t_wait = actuator_settle_ms(src_pulses_y, vel_xy, acc_xy); + t_wait = actuator_motion_time_ms(src_pulses_y, vel_xy, acc_xy); if (t_wait > src_wait_ms) src_wait_ms = t_wait; /* ---- 1. 磁铁掉电 ---- */ @@ -400,8 +522,8 @@ static void _actuator_move_fragment(const edc_fragment_t *frag) _frame_gap(); _actuator_move_y_sync(src_pulses_y, vel_xy, acc_xy); - /* ---- 3. 等待运动到位 (红灯快闪, 按行程实时计算) ---- */ - _wait_flash(src_wait_ms, 128, 0, 0, 200); + /* ---- 3. 等待运动到位 (轮询闭环到位标志, 红灯快闪) ---- */ + actuator_wait_arrival(src_wait_ms, src_mask); /* ---- 4. 舵机放下 ---- */ servo_set_down(); @@ -420,12 +542,16 @@ static void _actuator_move_fragment(const edc_fragment_t *frag) 终点朝向与旧协议 (绝对转到 rotate mod 360) 完全一致, 且不会绕远路 */ float dst_rep = _closest_reachable_angle(tgt_angle_deg, s_cur_angle_deg, ACTUATOR_ANGLE_LIMIT_DEG); - uint32_t dst_wait_ms = actuator_settle_ms( - (uint32_t)(fabsf(dst_rep - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f), - vel_angle, acc_angle); - t_wait = actuator_settle_ms(dst_pulses_x, vel_xy, acc_xy); + uint32_t dst_pulses_z = (uint32_t)(fabsf(dst_rep - s_cur_angle_deg) * 10.0f * s_scale_angle + 0.5f); + 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); + + 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); if (t_wait > dst_wait_ms) dst_wait_ms = t_wait; - t_wait = actuator_settle_ms(dst_pulses_y, vel_xy, acc_xy); + t_wait = actuator_motion_time_ms(dst_pulses_y, 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); @@ -435,8 +561,8 @@ static void _actuator_move_fragment(const edc_fragment_t *frag) _frame_gap(); _actuator_move_y_sync(dst_pulses_y, vel_xy, acc_xy); - /* ---- 8. 等待运动到位 (红灯快闪, 按行程实时计算) ---- */ - _wait_flash(dst_wait_ms, 128, 0, 0, 200); + /* ---- 8. 等待运动到位 (轮询闭环到位标志, 红灯快闪) ---- */ + actuator_wait_arrival(dst_wait_ms, dst_mask); /* ---- 9. 舵机放下 ---- */ servo_set_down(); diff --git a/components/actuator/actuator.h b/components/actuator/actuator.h index 42656df..2d39aa4 100644 --- a/components/actuator/actuator.h +++ b/components/actuator/actuator.h @@ -106,30 +106,40 @@ extern "C" { #define ACTUATOR_ANGLE_LIMIT_DEG 270.0f #endif -/** 执行 Cycle / 归零时 XY 轴的速度与加速度档位 (实测 3000RPM + 250 档稳定) */ +/** 执行 Cycle / 归零时 XY 轴的速度与加速度档位 + 2026-08-01 提速: 3000RPM → 3500RPM (加速度保持 250 档, 实测稳定) + 注: 手册 FD 位置模式速度标称上限 0-3000RPM, 3500 超出文档范围需实测 */ #ifndef ACTUATOR_EXEC_VEL -#define ACTUATOR_EXEC_VEL 3000 +#define ACTUATOR_EXEC_VEL 3500 #endif #ifndef ACTUATOR_EXEC_ACC #define ACTUATOR_EXEC_ACC 250 #endif -/** Z 旋转轴执行速度与加速度 (结构刚性有限, 必须保持低速; 300RPM 已偏快) */ +/** Z 旋转轴执行速度与加速度 (结构刚性有限, 低速为主; 300RPM 已偏快) + 2026-08-01: 50→100RPM / acc 100→150 (仍远低于 300 上限, 可再微调) */ #ifndef ACTUATOR_EXEC_Z_VEL -#define ACTUATOR_EXEC_Z_VEL 50 +#define ACTUATOR_EXEC_Z_VEL 100 #endif #ifndef ACTUATOR_EXEC_Z_ACC -#define ACTUATOR_EXEC_Z_ACC 100 +#define ACTUATOR_EXEC_Z_ACC 150 #endif -/** 运动到位等待裕量 (ms): 无到位反馈, 在计算运动时间上叠加的安全余量 */ +/** 到位等待超时兜底裕量 (ms): 闭环到位轮询的硬超时上限 (纯运动时间 + 此裕量); + 正常到位会提前返回, 此裕量仅作保底防卡死 */ #ifndef ACTUATOR_SETTLE_MARGIN_MS #define ACTUATOR_SETTLE_MARGIN_MS 400 #endif -/** 舵机动作 / 磁铁吸合后的等待时间 (ms) */ +/** 到位判定容差 (原始单位, Emm 固件 0~65535 = 360°; 1.5° ≈ 273) + (2° 太松会减速途中误判"已到位"提前下一步; 若某轴一直超时可调大) */ +#ifndef ACTUATOR_ARRIVAL_TOL_RAW +#define ACTUATOR_ARRIVAL_TOL_RAW 273 +#endif + +/** 舵机动作 / 磁铁吸合后的等待时间 (ms) (实测优化: 300 → 150) */ #ifndef ACTUATOR_GRIP_SETTLE_MS -#define ACTUATOR_GRIP_SETTLE_MS 300 +#define ACTUATOR_GRIP_SETTLE_MS 150 #endif /** @@ -225,6 +235,31 @@ uint32_t actuator_mm_settle_ms(float distance_mm, uint16_t vel, uint8_t acc); */ uint32_t actuator_angle_settle_ms(float angle_deg, uint16_t vel, uint8_t acc); +/** + * @brief 直线轴纯运动时间 (按实际 mm 距离换算脉冲, 不含裕量) + * @param distance_mm 直线距离 (mm, 取绝对值) + * @param vel 速度 RPM + * @param acc 加速度档位 0~255 + */ +uint32_t actuator_mm_motion_time_ms(float distance_mm, uint16_t vel, uint8_t acc); + +/** + * @brief Z 旋转轴纯运动时间 (按实际角度换算脉冲, 不含裕量) + * @param angle_deg 旋转角度 (°, 取绝对值) + * @param vel 速度 RPM + * @param acc 加速度档位 0~255 + */ +uint32_t actuator_angle_motion_time_ms(float angle_deg, uint16_t vel, uint8_t acc); + +/** + * @brief 等待指定电机到位 (轮询闭环 S_FLAG 到位标志) + * @param base_wait_ms 纯运动时间估算 (兜底超时基准, 超时=纯运动时间+裕量) + * @param moved_mask 实际有行程的电机位掩码: + * bit0=Z旋转, bit1=X, bit2=Y1, bit3=Y2 + * @return true=全部到位, false=超时 (已按最坏情况等待) + */ +bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask); + /** * @brief 设置 Z 旋转轴心 ↔ 工具末端 XY 偏移量 (mm) * @param offset_x_mm 工具末端相对于旋转中心的 X 偏移 (mm) diff --git a/components/emm_v5/emm_v5.c b/components/emm_v5/emm_v5.c index d07dac7..c7e04bc 100644 --- a/components/emm_v5/emm_v5.c +++ b/components/emm_v5/emm_v5.c @@ -127,6 +127,94 @@ void Emm_V5_Read_Sys_Params(uint8_t addr, EmmSysParams_t s) Emm_V5_Send(cmd, i); } +/* ============================================================ + * 读取电机状态标志 (S_FLAG 0x3A) 并解析返回 + * ============================================================ */ + +uint8_t Emm_V5_Read_Status_Flag(uint8_t addr, uint32_t timeout_ms) +{ + uint8_t rx[8] = {0}; + 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 无新字节: 认为帧已收完/无响应 */ + } + } + return 0xFF; +} + +/* ============================================================ + * 读取电机位置 (S_TPOS 0x33 / S_CPOS 0x36) 并解析返回 + * + * 返回帧: 地址 + 功能码 + 符号 + 位置(4B) + 6B (共 8 字节) + * Emm 固件: 0~65535 对应 0~360° + * ============================================================ */ + +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 无新字节: 认为帧已收完/无响应 */ + } + } + return -1; +} + +int32_t Emm_V5_Read_TPOS(uint8_t addr) +{ + return Emm_V5_Read_Pos(addr, 0x33); +} + +int32_t Emm_V5_Read_CPOS(uint8_t addr) +{ + return Emm_V5_Read_Pos(addr, 0x36); +} + void Emm_V5_Modify_Ctrl_Mode(uint8_t addr, bool svF, uint8_t ctrl_mode) { uint8_t cmd[6] = {addr, 0x46, 0x69, (uint8_t)svF, ctrl_mode, 0x6B}; diff --git a/components/emm_v5/emm_v5.h b/components/emm_v5/emm_v5.h index 16ccfee..9b889a3 100644 --- a/components/emm_v5/emm_v5.h +++ b/components/emm_v5/emm_v5.h @@ -91,6 +91,30 @@ void Emm_V5_Reset_Clog_Pro(uint8_t addr); */ void Emm_V5_Read_Sys_Params(uint8_t addr, EmmSysParams_t s); +/** + * @brief 读取电机状态标志 (S_FLAG, 功能码 0x3A) 并解析返回 + * @param addr 电机地址 + * @param timeout_ms 等待返回超时 (ms), 0 使用默认 EMM_V5_RX_TIMEOUT_MS + * @return 状态标志字节, 读取/解析失败返回 0xFF + * @note 位定义: bit0=Ens_TF 使能, bit1=Prf_TF 到位, bit2=Cgi_TF 堵转, + * bit3=Cgp_TF 堵转保护, bit4/bit5=左/右限位, bit7=Oac_TF 掉电 + */ +uint8_t Emm_V5_Read_Status_Flag(uint8_t addr, uint32_t timeout_ms); + +/** + * @brief 读取电机目标位置 (S_TPOS, 功能码 0x33) 并解析返回 + * @param addr 电机地址 + * @return 目标位置 (Emm 固件: 0~65535 对应 0~360°), 读取/解析失败返回 -1 + */ +int32_t Emm_V5_Read_TPOS(uint8_t addr); + +/** + * @brief 读取电机实时位置 (S_CPOS, 功能码 0x36) 并解析返回 + * @param addr 电机地址 + * @return 实时位置 (Emm 固件: 0~65535 对应 0~360°), 读取/解析失败返回 -1 + */ +int32_t Emm_V5_Read_CPOS(uint8_t addr); + /** * @brief 修改开环/闭环控制模式 * @param addr 电机地址 diff --git a/main/main.c b/main/main.c index 2296a2e..cf00b7a 100644 --- a/main/main.c +++ b/main/main.c @@ -41,7 +41,7 @@ static const char *TAG = "MAIN"; #define RECOGNIZE_TIMEOUT_MS 20000 #define RECOGNIZE_POLL_MS 100 /* 归零速度: XY 用 ACTUATOR_EXEC_VEL/ACC (3000/250), Z 用 ACTUATOR_EXEC_Z_VEL/Z_ACC (低速); - 等待时间按最大行程 + 速度/加速度实时计算 */ + 到位等待: 轮询闭环到位标志, 超时按最大行程 + 裕量兜底 */ static void button_recognize_task(void *pvParam) { @@ -50,28 +50,32 @@ static void button_recognize_task(void *pvParam) ESP_LOGI(TAG, "按钮按下, 归零后再发送 RECOGNIZE ..."); buzzer_beep(2000, 100); /* 短音: 识别开始 */ - /* ---- 归零: 舵机抬起 + Z 回 0° (低速, 最坏行程 270°) ---- */ + /* ---- 归零: 舵机抬起 + 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); - vTaskDelay(pdMS_TO_TICKS(actuator_angle_settle_ms( - ACTUATOR_ANGLE_LIMIT_DEG, - ACTUATOR_EXEC_Z_VEL, ACTUATOR_EXEC_Z_ACC))); - - /* ---- XY 回零: X=0, Y=330 (按最坏行程实时计算等待) ---- */ - uint32_t xy_settle = actuator_mm_settle_ms(ACTUATOR_LIMIT_X_MAX, - ACTUATOR_EXEC_VEL, - ACTUATOR_EXEC_ACC); - uint32_t y_settle = actuator_mm_settle_ms(ACTUATOR_LIMIT_Y_MAX, - ACTUATOR_EXEC_VEL, - ACTUATOR_EXEC_ACC); - if (y_settle > xy_settle) xy_settle = y_settle; - 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); - vTaskDelay(pdMS_TO_TICKS(xy_settle)); + actuator_wait_arrival(home_motion, + (1u << 0) | (1u << 1) | (1u << 2) | (1u << 3)); ESP_LOGI(TAG, "归零完成, 发送 RECOGNIZE ...");