/** * @file actuator.h * @brief 执行器应用层封装 — 4 路步进电机 + 1 路舵机 * * 轴映射: * 1 号电机 (addr=1) ── Angle/Z 旋转轴 * 2 号电机 (addr=2) ── X 轴 * 3 号电机 (addr=3) ── Y 轴 ─┐ 完全同步 * 4 号电机 (addr=4) ── Y 轴 ─┘ * 舵机 (PWM) ── Z 轴抬升 (仅抬起/放下两态) * * 运动模式: 位置模式, 绝对定位 (raF=true), 暂不标定 */ #ifndef ACTUATOR_H #define ACTUATOR_H #include #include #include "driver/gpio.h" #include "protocol.h" #include "emm_v5.h" #ifdef __cplusplus extern "C" { #endif /* ================================================================ * 可配置宏 (可在项目级或 -D 覆盖) * ================================================================ */ /** 丝杆参数 (X / Y 轴直线运动) */ #define ACTUATOR_SCREW_LEAD_MM 8.0f /* 导程: 一圈 = 8mm 直线位移 */ #define ACTUATOR_SCREW_PITCH_MM 2.0f /* 螺距 */ /** 电机 Emm_V5 地址 */ #ifndef ACTUATOR_ADDR_ANGLE #define ACTUATOR_ADDR_ANGLE 1 /* Angle / Z 旋转轴 */ #endif #ifndef ACTUATOR_ADDR_X #define ACTUATOR_ADDR_X 2 /* X 轴 */ #endif #ifndef ACTUATOR_ADDR_Y1 #define ACTUATOR_ADDR_Y1 3 /* Y 轴 #1 (同步) */ #endif #ifndef ACTUATOR_ADDR_Y2 #define ACTUATOR_ADDR_Y2 4 /* Y 轴 #2 (同步) */ #endif /** 舵机 PWM GPIO */ #ifndef ACTUATOR_SERVO_GPIO #define ACTUATOR_SERVO_GPIO GPIO_NUM_4 #endif /** 电机 UART 引脚 */ #ifndef ACTUATOR_UART_PORT #define ACTUATOR_UART_PORT UART_NUM_1 #endif #ifndef ACTUATOR_UART_TX #define ACTUATOR_UART_TX GPIO_NUM_6 #endif #ifndef ACTUATOR_UART_RX #define ACTUATOR_UART_RX GPIO_NUM_7 #endif /** 默认速度 (RPM, 0~5000) */ #ifndef ACTUATOR_DEFAULT_VEL #define ACTUATOR_DEFAULT_VEL 500 #endif /** 默认加速度 (0~255, 0=直接启动) */ #ifndef ACTUATOR_DEFAULT_ACC #define ACTUATOR_DEFAULT_ACC 50 #endif /** 脉冲换算系数 (坐标值 × scale = 脉冲数) * * X/Y 轴: 丝杆导程 8mm → 一圈 8mm = 3200 脉冲 * scale = 3200 / 8 = 400 脉冲/mm * * Z 旋转: angle_x10 单位为 0.1° * 一圈 360° = 3600 (0.1°) = 3200 脉冲 * scale = 3200 / 3600 ≈ 0.8889 脉冲/(0.1°) */ #ifndef ACTUATOR_SCALE_X #define ACTUATOR_SCALE_X 400.0f /* mm → 脉冲 */ #endif #ifndef ACTUATOR_SCALE_Y #define ACTUATOR_SCALE_Y 400.0f /* mm → 脉冲 */ #endif #ifndef ACTUATOR_SCALE_ANGLE #define ACTUATOR_SCALE_ANGLE ((float)EMM_V5_PULSES_PER_REV / 3600.0f) /* 0.1° → 脉冲 */ #endif /** 便捷宏: mm 直线距离 → 脉冲数 (四舍五入) */ #define ACTUATOR_MM_TO_PULSES(mm) ((uint32_t)((float)(mm) * ACTUATOR_SCALE_X + 0.5f)) /** 软限位 (左上零点坐标系, 单位 mm) */ #define ACTUATOR_LIMIT_X_MIN 0.0f #define ACTUATOR_LIMIT_X_MAX 210.0f #define ACTUATOR_LIMIT_Y_MIN 0.0f #define ACTUATOR_LIMIT_Y_MAX 335.0f /** Z 旋转轴软限位 (±°, 机械行程) */ #ifndef ACTUATOR_ANGLE_LIMIT_DEG #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 超出文档范围需实测 */ #ifndef ACTUATOR_EXEC_VEL #define ACTUATOR_EXEC_VEL 3500 #endif #ifndef ACTUATOR_EXEC_ACC #define ACTUATOR_EXEC_ACC 250 #endif /** Z 旋转轴执行速度与加速度 (结构刚性有限, 低速为主; 300RPM 已偏快) 2026-08-01: 50→100RPM / acc 100→150 (仍远低于 300 上限, 可再微调) */ #ifndef ACTUATOR_EXEC_Z_VEL #define ACTUATOR_EXEC_Z_VEL 100 #endif #ifndef ACTUATOR_EXEC_Z_ACC #define ACTUATOR_EXEC_Z_ACC 150 #endif /** 到位等待超时兜底裕量 (ms): 闭环到位轮询的硬超时上限 (纯运动时间 + 此裕量); 正常到位会提前返回, 此裕量仅作保底防卡死。 2026-08-03: 400→1000 —— 大行程/高速运动后驱动闭环在 0.1° 窗口内稳定需要 额外时间, 400ms 裕量实测多次在到位帧到达前就超时 (电机3/4 迟到 ~0.5s) */ #ifndef ACTUATOR_SETTLE_MARGIN_MS #define ACTUATOR_SETTLE_MARGIN_MS 1000 #endif /** 到位判定容差 (原始单位, Emm 固件 0~65535 = 360°; 1.5° ≈ 273) (2° 太松会减速途中误判"已到位"提前下一步; 若某轴一直超时可调大) */ #ifndef ACTUATOR_ARRIVAL_TOL_RAW #define ACTUATOR_ARRIVAL_TOL_RAW 273 #endif /** 舵机动作 / 磁铁吸合后的等待时间 (ms) (150 偏短不稳, 300 略保守 → 200) */ #ifndef ACTUATOR_GRIP_SETTLE_MS #define ACTUATOR_GRIP_SETTLE_MS 200 #endif /** * 连续多帧之间的间隔 (ms) * Emm_V5 驱动板无法解析 UART 上零间隔的背靠背连续帧 (会丢字节), * 凡是一次动作需要连发多帧 (如 Y 轴双电机), 帧间必须插入该延时 */ #ifndef ACTUATOR_FRAME_GAP_MS #define ACTUATOR_FRAME_GAP_MS 10 #endif /** 控制模式: 2 = 闭环位置模式 */ #define ACTUATOR_CTRL_MODE 2 /* ================================================================ * Z 旋转轴心 ↔ 工具末端 偏移补偿 * * 终端执行机构 (工具末端) 与 Z 旋转机构不在同一条纵轴上, * 旋转角度 θ 时, 工具末端的 XY 位置会绕旋转中心偏移。 * * ⚠️ 语义 (2026-08-03 修正): MPU 下发的坐标 (source/target) 来自 calibrated_H, * 本身是"旋转中心"坐标 —— θ=0 时中心到坐标点, 工具末端即落在目标物点 * 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)。 * ================================================================ */ #ifndef ACTUATOR_OFFSET_X_MM #define ACTUATOR_OFFSET_X_MM 6.6f /* angle=0° 时工具末端偏右 ~8mm (实测) */ #endif #ifndef ACTUATOR_OFFSET_Y_MM #define ACTUATOR_OFFSET_Y_MM -35.75f /* angle=0° 时工具末端偏上 ~30mm (实测) */ #endif /** * XY 平面左上零点坐标系 运动方向: * X 轴正向 (→): CW (dir=0) ── 电机2 * Y 轴正向 (↓): CCW (dir=1) ── 电机3+4 (双电机同步) * Z 旋转正向 (↻): CW (dir=0) ── 电机1 */ #define ACTUATOR_DIR_X 0 /* X轴正向 = CW */ #define ACTUATOR_DIR_Y 1 /* Y轴正向 = CCW */ #define ACTUATOR_DIR_ANGLE 0 /* Z旋转正向 = CW */ /* ================================================================ * API * ================================================================ */ /** * @brief 初始化所有执行器 * @note 内部调用 Emm_V5_Init + servo_init + 4 路电机使能 + 闭环模式 */ void actuator_init(void); /** * @brief 设置脉冲换算系数 * @param x_scale X 轴: 坐标值 × x_scale = 脉冲数 * @param y_scale Y 轴: 坐标值 × y_scale = 脉冲数 * @param angle_scale Z 旋转: angle_x10 × angle_scale = 脉冲数 */ void actuator_set_scale(float x_scale, float y_scale, float angle_scale); /** * @brief 按 Emm_V5 曲线加减速公式估算纯运动时间 (ms) * @param pulses 脉冲数 (行程) * @param vel 速度 RPM * @param acc 加速度档位 0~255 (0=直接启动, 无曲线) * @note 曲线公式: 每提升 1 RPM 耗时 (256-acc)*50us, 按梯形/三角形加减速估算 */ uint32_t actuator_motion_time_ms(uint32_t pulses, uint16_t vel, uint8_t acc); /** * @brief 运动到位等待 = 估算运动时间 + 安全裕量 * @param pulses 脉冲数 (行程) * @param vel 速度 RPM * @param acc 加速度档位 0~255 */ uint32_t actuator_settle_ms(uint32_t pulses, uint16_t vel, uint8_t acc); /** * @brief 直线轴运动到位等待 (按实际 mm 距离换算脉冲) * @param distance_mm 直线距离 (mm, 取绝对值) * @param vel 速度 RPM * @param acc 加速度档位 0~255 */ uint32_t actuator_mm_settle_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_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 等待指定电机到位 (事件驱动: 收到驱动主动回传的到位帧) * @param base_wait_ms 纯运动时间估算 (兜底超时基准, 超时=纯运动时间+裕量) * @param moved_mask 实际有行程的电机位掩码: * bit0=Z旋转, bit1=X, bit2=Y1, bit3=Y2 * @return true=全部到位, false=超时 * @note 依赖驱动 Response=Reached/Both/Other 回传到位帧 (地址+FD+9F+6B) */ bool actuator_wait_arrival(uint32_t base_wait_ms, uint32_t moved_mask); /** * @brief 设置 Z 旋转轴心 ↔ 工具末端 XY 偏移量 (mm) * @param offset_x_mm 工具末端相对于旋转中心的 X 偏移 (mm) * @param offset_y_mm 工具末端相对于旋转中心的 Y 偏移 (mm) * @note 补偿会在每次 fragment 执行时自动应用; * 未调用时默认使用 ACTUATOR_OFFSET_X_MM / ACTUATOR_OFFSET_Y_MM */ void actuator_set_offset(float offset_x_mm, float offset_y_mm); /** * @brief 获取当前 XY 偏移量 * @param offset_x_mm [out] X 偏移 (mm), 可为 NULL * @param offset_y_mm [out] Y 偏移 (mm), 可为 NULL */ void actuator_get_offset(float *offset_x_mm, float *offset_y_mm); /** * @brief 对目标坐标应用 Z 旋转偏移补偿 * @param target_x [in/out] 目标 X 坐标 (mm) * @param target_y [in/out] 目标 Y 坐标 (mm) * @param angle_deg 当前旋转角度 (度) * @note 内部使用, 也可由上层在手动模式调用 */ void actuator_apply_offset_compensation(uint16_t *target_x, uint16_t *target_y, float angle_deg); /** * @brief 执行一个完整 Cycle (遍历所有 Fragment, 逐段运动) * @param cycle 协议下发的 Cycle 数据 * @return 0 成功, -1 参数无效 */ 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 路电机 */ void actuator_stop_all(void); /** * @brief 舵机: 抬起 (上限位) */ void actuator_servo_up(void); /** * @brief 舵机: 放下 (下限位) */ void actuator_servo_down(void); /** * @brief 舵机: 设置任意角度 * @param angle 角度 0.0 ~ 180.0 */ void actuator_servo_set_angle(float angle); /* ---- 单电机手动控制 (Web 调试用, Y 轴自动同步) ---- */ /** * @brief 单电机使能控制 (Y 轴电机各自独立) * @param addr 电机地址 (1~4) * @param state 使能状态 */ void actuator_motor_enable(uint8_t addr, bool state); /** * @brief 单电机速度模式 (Y 轴地址 3/4 自动同步) * @param addr 电机地址 (1~4) * @param dir 方向, 0=CW, 非0=CCW * @param vel 速度 0~5000 RPM * @param acc 加速度 0~255 */ void actuator_motor_vel(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc); /** * @brief 单电机位置模式 (Y 轴地址 3/4 自动同步) * @param addr 电机地址 (1~4) * @param dir 方向, 0=CW, 非0=CCW * @param vel 速度 0~5000 RPM * @param acc 加速度 0~255 * @param clk 脉冲数 * @param raF 相对/绝对, false=相对, true=绝对 */ void actuator_motor_pos(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF); /** * @brief 单电机角度直控 (Y 轴地址 3/4 自动同步) * @param addr 电机地址 (1~4) * @param angle_deg 目标角度 (°), 内部换算为脉冲 (3200=360°) * @param vel 速度 0~5000 RPM * @param acc 加速度 0~255 * @param raF 相对/绝对, false=相对, true=绝对 */ void actuator_motor_angle(uint8_t addr, float angle_deg, uint16_t vel, uint8_t acc, bool raF); /** * @brief 单电机直线距离控制 (X/Y 轴, Y 轴地址 3/4 自动同步) * @param addr 电机地址 (2=X, 3/4=Y) * @param distance_mm 直线移动距离 (mm), 丝杆导程 8mm/圈 * @param vel 速度 0~5000 RPM * @param acc 加速度 0~255 * @param raF 相对/绝对, false=相对, true=绝对 */ void actuator_motor_mm(uint8_t addr, float distance_mm, uint16_t vel, uint8_t acc, bool raF); /** * @brief 单电机立即停止 (Y 轴地址 3/4 自动同步) * @param addr 电机地址 (1~4) */ void actuator_motor_stop(uint8_t addr); /** * @brief 获取最近一次执行的 Cycle 快照 (供外部读取, 如 Web 面板) * @return 有效时返回指针, 否则 NULL */ const edc_cycle_t *actuator_get_last_cycle(void); /** * @brief 最近 Cycle 是否有效 */ bool actuator_is_last_cycle_valid(void); /** * @brief 清除最近 Cycle 记录 (用于等待新 Cycle) */ void actuator_clear_last_cycle(void); #ifdef __cplusplus } #endif #endif /* ACTUATOR_H */