完整链路可以理解成一句话命令行里的10°→ 程序读成amplitude10.0→ 传给每个轴的运动模块 → 换算成编码器脉冲 → 每 1ms 写入 EtherCAT 的0x607A Target Position→ 伺服驱动器执行位置环 → 电机转动 → 机械臂关节真的运动。1. 命令行输入 10°比如你运行./dual_arm_aging --f0 m0.xml --f1 m1.xml --amplitude 10 --period 2或者简写./dual_arm_aging --f0 m0.xml --f1 m1.xml -a 10 -p 2这里的10会被程序读到double amplitude 10.0, period 2.0;2.amplitude10传给两个机械臂控制对象主函数里创建两个 EtherCAT 控制任务ArmProgram master0(0); master0.init(..., amplitude, period); ArmProgram master1(1); master1.init(..., amplitude, period);也就是说命令行读到的10°被传进了ArmProgram::init(..., double amplitude_deg, double period_sec)所以此时amplitude_deg 10.0 period_sec 2.0M0 和 M1 两组机械臂都会收到这个运动参数。3. EtherCAT 初始化找到每个伺服轴进入ArmProgram::init()后程序会加载 ENI 文件task.load_eni(file_name, cycle_time);然后在配置回调里扫描 EtherCAT 从站if (task.profile_no(sp) ! 402) continue;这句话的意思是程序只处理CiA402 伺服驱动器。然后给每个轴创建一个axis_data里面记录ax-slave_pos sp; ax-axis_id axis_count; ax-master_id mid;也就是说每一个 EtherCAT 伺服轴都会被程序抽象成一个软件里的axis_data对象。4. 注册 PDO把 C 指针和伺服对象字典绑定这一步非常关键。程序会把几个 CiA402 对象注册到 PDO0x6040 control_word 0x6041 status_word 0x6060 mode_of_operation 0x6064 position_actual_value 0x607A target_position代码里是这样task.try_register_pdo_entry(ax-control_word, sp, {0x6040 off, 0}); task.try_register_pdo_entry(ax-mode_of_operation, sp, {0x6060 off, 0}); task.try_register_pdo_entry(ax-target_position, sp, {0x607a off, 0});这一步完成之后后面你在 C 里写*axis-target_position 某个数;就等价于在周期通信中给伺服驱动器写0x607A Target Position这就是从软件变量到 EtherCAT PDO 的绑定关系。5. 10°不是直接发给电机要先换算成脉冲伺服驱动器不认识“10°”这个概念它真正接收的是编码器脉冲位置。所以程序要先算10° 多少个 position counts你代码里每个轴都有double counts_per_deg() const { return static_castdouble((1LL motorBit) * gearRatio / 360.0); }也就是每度脉冲数 2^motorBit × 减速比 / 360普通轴motorBit 24 gearRatio 101 counts_per_deg 2^24 × 101 / 360 ≈ 4,706,941 counts/deg所以普通轴的 10°10° ≈ 47,069,411 countsti5 轴motorBit 18 gearRatio 1 counts_per_deg 2^18 / 360 ≈ 728.18 counts/deg所以 ti5 轴的 10°10° ≈ 7,281 counts程序会根据从站位置判断普通轴还是 ti5 轴然后设置不同的motorBit和gearRatio。6. 把 10°参数放进每个轴的运动模块初始化每个轴的时候程序会执行prog.motion.set_params(amplitude_deg, period_sec, cycle_dt, ax-counts_per_deg());这一步就把四个东西传给运动模块amplitude_deg 10.0 // 角度幅值 period_sec 2.0 // 周期 cycle_dt 0.001 // 每周期时间默认 1ms counts_per_deg 每个轴自己的脉冲/度所以到这里运动模块已经知道这个轴要做 ±10° 正弦运动 这个轴 1° 等于多少脉冲 每 1ms 更新一次目标位置。7. 伺服先上电使能进入 CSP 模式在真正运动之前程序不是直接写目标位置而是先让伺服进入可运行状态。主函数会等待while (g_running (!master0.is_all_enabled() || !master1.is_all_enabled()))而is_all_enabled()里判断的是(*ax-status_word 0x006f) 0x0027也就是每个轴都进入operation enabled只有进入这个状态电机才真正可以响应目标位置。同时在power::on_cycle()里当状态机到switched_on时程序会设置*axis-mode_of_operation 8; // CSP8就是CSP周期同步位置模式。也就是说你这个项目最终是靠周期性写目标位置控制机械臂。8. 回零先把关节拉到 0 附近程序启动后会让你确认安全然后执行master0.begin_homing(); master1.begin_homing();在周期回调里收到 homing 命令后for (auto p : progs) p.motion.start_homing();运动模块进入HOMING模式后会逐步让cur_target靠近 0int32_t err 0 - cur_target;也就是说程序先尝试让所有关节目标位置回到 0。回零完成后后面的 10° 周期运动才是围绕当前起始位置进行。9. 按 Enter 开始 aging记录每个轴的起始位置回零完成后你按 Entermaster0.begin_aging(); master1.begin_aging();在 EtherCAT 周期回调里会执行for (auto p : progs) p.motion.start_aging(*p.axis-position_actual_value);这一步非常重要。它会把每个轴当前实际位置记录为start_pos actual_pos;所以 10°周期运动不是绝对从 0 开始而是从当前实际位置 start_pos 开始做 ±10° 正弦摆动如果回零后实际位置接近 0那么就是围绕 0 做 ±10°。10. 每 1ms 计算一次新的目标位置真正的 10°运动发生在SineMotion::update()里。代码是elapsed cycle_dt; double amp_counts amplitude_deg * counts_per_deg; int32_t offset static_castint32_t( amp_counts * std::sin(2.0 * M_PI * elapsed / period_sec)); cur_target start_pos offset;也就是说目标位置 起始位置 10°对应脉冲数 × sin(2πt / period)如果周期是 2 秒那么关节目标角度就是t0s 0° t0.5s 10° t1.0s 0° t1.5s -10° t2.0s 0°所以这里的10°是正弦运动的幅值不是“一直转到 10°不动”。11.program()把计算结果写到target_position周期回调最后会执行for (auto p : progs) p();每个program的operator()里会先执行上电状态机power_.on_cycle();如果伺服还没使能就先保持当前位置*axis-target_position *axis-position_actual_value;如果已经使能成功就执行int32_t actual *axis-position_actual_value; *axis-target_position motion.update(actual);这句就是整个链路里最关键的一句*axis-target_position motion.update(actual);左边的target_position已经绑定到了 EtherCAT 的0x607A Target Position右边的motion.update()根据命令行的10°算出了当前周期的目标脉冲。12. EtherCAT 把0x607A发给伺服驱动器因为前面已经完成了 PDO 映射axis-target_position 绑定到 0x607A所以当程序写*axis-target_position cur_target;EtherCAT 主站在下一个通信周期会把这个值打包进 RxPDO通过网线发给伺服驱动器。这时数据流是C变量 target_position → EtherCAT RxPDO → 0x607A Target Position → 伺服驱动器伺服驱动器收到目标位置后会由驱动器内部完成位置环 → 速度环 → 电流环 → 电机转动上位机不直接控制电流也不直接控制 PWM。你这层程序控制的是目标位置。13. 电机转了机械臂关节才真的动伺服驱动器执行目标位置后电机会带动减速器和机械臂关节转动。如果换算关系正确target_position 变化 47,069,411 counts ≈ 普通轴机械侧变化 10°如果是 ti5 轴target_position 变化 7,281 counts ≈ ti5 轴变化 10°所以真实运动链路是0x607A目标位置变化 → 伺服驱动器控制电机 → 电机轴转动 → 减速器输出 → 关节转动 → 机械臂运动14. 实际位置反馈回来程序显示角度伺服驱动器还会把实际位置通过 TxPDO 返回给主站task.try_register_pdo_entry(ax-position_actual_value, sp, {0x6064 off, 0});也就是0x6064 Position Actual Value显示线程里会调用m0-get_joint_angles(a0, 7); m1-get_joint_angles(a1, 7);get_joint_angles()里面会做反向换算angles[i] position_actual_value / counts_per_deg();所以你屏幕上看到的M0: [...] M1: [...]就是把驱动器反馈的实际脉冲重新换算成角度后的结果。整个链路画成一条线命令行输入 --amplitude 10 ↓ boost 解析成 amplitude 10.0 ↓ master0.init(..., amplitude, period) master1.init(..., amplitude, period) ↓ ArmProgram::init() ↓ 扫描 EtherCAT 从站找到 CiA402 伺服轴 ↓ 注册 PDO 0x6040 控制字 0x6041 状态字 0x6060 模式 0x6064 实际位置 0x607A 目标位置 ↓ 根据 motorBit 和 gearRatio 计算 counts_per_deg ↓ motion.set_params(10°, period, cycle_dt, counts_per_deg) ↓ CiA402 状态机上电 ↓ 设置 0x6060 8也就是 CSP 模式 ↓ 按 Enter 开始 aging ↓ 记录当前实际位置 start_pos ↓ 每 1ms 执行一次 offset 10° × counts_per_deg × sin(2πt / period) target start_pos offset ↓ 写入 *axis-target_position ↓ 等价于写入 EtherCAT 0x607A Target Position ↓ EtherCAT 周期帧发送给伺服驱动器 ↓ 伺服驱动器内部位置环控制电机 ↓ 电机通过减速器带动机械臂关节运动 ↓ 驱动器通过 0x6064 返回实际位置 ↓ 程序换算成角度并显示最核心的三句话第一命令行的10°只是一个上层运动参数。第二程序会把10°换算成每个轴自己的编码器脉冲数。第三真正发给伺服驱动器的是0x607A Target Position伺服驱动器收到目标位置后自己闭环控制电机运动。