基于 EtherCAT + CiA402 的双机械臂10°周期运动流程解析
完整链路可以理解成一句话：
命令行里的
10°→ 程序读成amplitude=10.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.amplitude=10传给两个机械臂控制对象
主函数里创建两个 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 对象注册到 PDO：
0x6040 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_cast<double>((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靠近 0：
int32_t err = 0 - cur_target;也就是说，程序先尝试让所有关节目标位置回到 0。
回零完成后，后面的 10° 周期运动才是围绕当前起始位置进行。
9. 按 Enter 开始 aging，记录每个轴的起始位置
回零完成后，你按 Enter：
master0.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_cast<int32_t>( amp_counts * std::sin(2.0 * M_PI * elapsed / period_sec)); cur_target = start_pos + offset;也就是说：
目标位置 = 起始位置 + 10°对应脉冲数 × sin(2πt / period)如果周期是 2 秒，那么关节目标角度就是：
t=0s 0° t=0.5s +10° t=1.0s 0° t=1.5s -10° t=2.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，伺服驱动器收到目标位置后，自己闭环控制电机运动。