439 lines
15 KiB
C
439 lines
15 KiB
C
/**
|
||
* @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 <stdint.h>
|
||
#include <stdbool.h>
|
||
#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
|
||
|
||
/** 放置段: XY 运动启动后, 到 Z 旋转启动的额外延迟 (ms)
|
||
2026-08-04: 碎片在区域边缘时, 先旋转会把碎片甩向机械边缘碰撞;
|
||
延迟让 XY 先把碎片带离边缘再旋转, 防碰撞 */
|
||
#ifndef ACTUATOR_ROTATE_DELAY_MS
|
||
#define ACTUATOR_ROTATE_DELAY_MS 350
|
||
#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 */
|