作者：煲仔卤煮
MPS 用户名：Zealerlu
1. 作品简介
这个作品是一套面向小型机器人关节的双电机闭环驱动系统。每个节点由 AT32M412、MP4583、MP6540H 和同轴 MA600A 构成，可独立完成有感 FOC、电流环、速度环和位置环控制。系统通过 1 Mbps CAN 接收 Z-Trace(X-Track复刻改造项目) 下发的轨迹，并采用“节点预装轨迹点 + 广播 SYNC”的方式驱动五连杆机构的两个主动关节。
目前两套驱动和五连杆机构已经完成联调。Z-Trace根据预先生成的 MPS 末端轨迹，换算并下发两个关节的目标位置，最终由五连杆末端夹笔在纸面留下了实际的 MPS 字样。
这个项目最初其实来自两个在箱子里吃灰的小云台电机。看到 EEWORLD 和 MPS 联合举办机器人模块创新大赛以后，我正好也想试一下：AI 在这种软硬件混合、需要不断实测的项目里，究竟能帮到什么程度。
为了把这碟醋用起来，我最后画了两版驱动板、写了一套控制固件，又打印了一套五连杆机构。资料梳理、代码、测试、排障记录和帖子整理里，能交给 AI 协助的部分就尽量让它参与；但板子能不能转、两个节点能不能同步、最后能不能真的在纸上写出 MPS，还是要靠实物来回答。
饺子包得比一开始预计的大了一点，不过至少把最初设想的完整链路跑通了。下面是最终作品和参赛视频。
2. 系统总体方案
整个系统分成轨迹控制、CAN 总线、两个电机节点和五连杆机构四层：
两个驱动节点使用相同的硬件和固件，节点差异通过关节配置保存，包括 node ID、机械已知角、方向和软限位。这样不用为左右两侧维护两套工程，装配后只需要根据机构坐标分别配置。
CAN 也没有做成“收到角度就立刻执行”。Z-Trace 先把同一个 sequence 对应的关节轨迹点分别预装到 Node 1 和 Node 2，再广播 SYNC。两个节点收到匹配的 session 和 sequence 后，才会在各自的本地闭环里应用目标，避免谁先收到谁先走。
3. 作品中的 MPS 器件
这次使用了三颗 MPS 器件，刚好把电源、三相驱动和角度反馈串成一条完整链路。
3.1 MP4583：板上电源
MP4583 用于输入侧同步降压，为 MCU、传感器和驱动板低压部分供电。
电机项目里，电源看起来不像控制算法那么显眼，但它是后面所有测试的基础。两版板卡都围绕“上电默认不让电机乱动”来处理：先确认低压电源和控制状态，再逐步接入 PWM、ADC、磁编和电机负载。
3.2 MP6540H：三相电机驱动
MP6540H 负责驱动三相无刷电机。AT32M412 输出 16 kHz 中心对齐 PWM，控制侧完成 Clarke/Park、Id/Iq PI、反 Park 和 SVPWM。
项目同时使用 MP6540H 的 SOA/SOB/SOC 电流镜做三相电流反馈。调试中确认 SOx 反映的是低边器件电流，因此软件不仅要选择有效 PWM 采样窗口、重构缺失相，还要把低边器件电流方向统一转换成 FOC 的相电流正方向。
3.3 MA600A：同轴角度反馈
MA600A 用于读取电机导出轴的机械角度。
V1 采用旁轴布局，优点是节省轴向空间，但对磁场非线性、气隙、偏心和装配一致性更敏感。V2 将 MA600A 改为同轴正对，让磁铁在传感器敏感区域中心上方旋转。
当前编码器由独立定时器以 4 kHz 采样，16 kHz FOC ISR 只消费已经发布的角度快照，不在最高优先级控制中断里直接执行 SPI 事务。两套 V2 板均完成 256 点自动标定和重启恢复，标定采集期间 spike=0。
4. 从 V1 旁轴到 V2 同轴
V1 并不是完全不能用。MA600A 的 SPI 读取、旁轴仿真、BCT/correction table 和自动标定都实际跑过，完整一圈标定也能把内部质量指标压进门槛。
问题是，这个项目后面还要做三级闭环和双电机机构。旁轴布局把机械空间省下来了，却增加了磁场、装配和标定的不确定性。为了让后面的控制问题少背一个“会不会又是磁编”的锅，我重新 Layout 了 V2：
| 项目 | V1 旁轴 | V2 同轴 |
|---|---|---|
| 轴向空间 | 更节省 | 需要给磁铁和传感器留中心空间 |
| 磁场对称性 | 较弱 | 更接近理想同轴旋转 |
| 装配敏感度 | 对偏心、气隙和倾角更敏感 | 仍需控制装配，但关系更直观 |
| 标定压力 | 更依赖补偿 | 原始角度可用性更好，标定更接近小误差修整 |
| 闭环调试 | 磁编容易成为额外变量 | 更适合作为三环控制基线 |
V1 留下的波形不是和 V2 完全相同的测试条件，所以这里不做硬凑出来的精度横评。更准确的说法是：同轴方案降低了安装和标定的不确定性，让后面的三环和双电机调试有了更干净的角度基线。
旁轴方案:
同轴方案:
5. 控制系统
5.1 实时路径
当前控制频率和数据路径为：
MA600A 采样：4 kHz
FOC ISR：16 kHz
速度环：1 kHz
CAN 运动服务：1 kHz
轨迹目标：100 Hz
FOC ISR 内完成电流采样消费、Clarke/Park、Id/Iq PI、反 Park 和 SVPWM。SPI 编码器读取由 TMR7 独立完成；CAN 的轨迹老化、SYNC lease、反馈调度和安全状态由独立的 1 kHz 服务节拍维护。
5.2 三级级联
位置模式采用三级级联：
目标位置
-> 位置误差 + 速度前馈
-> 速度目标
-> 速度 PI + 摩擦前馈
-> Iq_ref
-> Id/Iq 电流 PI
-> Vd/Vq
-> SVPWM
位置环输出受最大速度约束，速度环输出受 Iq_ref 限流约束。运动和静止分别使用小幅摩擦前馈，帮助低速位置越过摩擦死区，但不会改动独立速度模式的参数。
5.3 核心代码模块
控制固件按“采集—控制—通信—安全”拆成几个相对独立的模块：
| 模块 | 主要职责 |
|---|---|
encoder_service |
唯一访问 MA600A 的采集服务，发布 raw、展开角、校正角和诊断快照 |
current_reconstruction |
选择有效低边采样窗口，按 KCL 重构缺失相并统一 FOC 电流极性 |
foc_core / motor_control_isr |
16 kHz Clarke/Park、控制调度、反 Park 与 SVPWM |
current_loop / speed_loop / position_loop |
从内到外完成 Id/Iq、速度和连续机械位置级联 |
motor_tuning |
集中保存 RAM 在线参数与各环目标、反馈、误差、输出和饱和状态 |
can_protocol / can_motion_service |
帧编解码、预装轨迹、广播 SYNC、1 kHz 状态机和超时 |
joint_config |
持久化 node ID、机械已知角、方向、软限位和 CRC |
fault_manager |
故障锁存、停机和清除边界 |
Z-Trace侧还承担预生成轨迹的缩放、离散、五连杆逆运动学和双节点 CAN 下发。由于本项目主要是电机驱动模块,所以Z-Trace这个项目的内容就不做过多赘述了.
以下是核心代码块简介:
控制代码没有全部堆在一个 ISR 里，而是按“采集—控制—通信—安全”拆开。各模块之间主要传递四类数据：
MA600A 原始角度
→ encoder_service 角度快照
→ 位置环 / 速度估算
ADC 三相原始值
→ 有效窗口判断与电流重构
→ Clarke / Park
→ Id / Iq 电流环
CAN 轨迹帧
→ pending 轨迹点
→ 广播 SYNC
→ position_setpoint
传感器 / 跟踪 / 通信异常
→ fault_manager 故障位
→ MOTOR_CONTROL_STATE_FAULT
→ PWM 安全停机
5.3.1. 编码器服务：把原始角度整理成控制快照
MA600A 的原始角度不会直接散落到各个控制模块中。采样通过检查后，由 encoder_service 统一发布展开角度、校正角度、机械位置和速度。
next_accept_count = s_snapshot.accept_count + 1u;
encoder_update_window_speed(s_snapshot.corrected_unwrapped,
next_accept_count);
s_snapshot.raw_delta = delta;
s_snapshot.raw16 = raw;
s_snapshot.corrected_raw16 = corrected;
s_snapshot.corrected_delta = corrected_delta;
s_snapshot.control_position_mdeg =
encoder_control_position_mdeg(s_snapshot.corrected_unwrapped);
s_snapshot.speed_raw = speed;
s_snapshot.mech_mdeg = encoder_mech_mdeg_from_raw(raw);
s_snapshot.raw_elec_mrad =
encoder_elec_mrad_from_position(raw);
s_snapshot.elec_mrad =
encoder_elec_mrad_from_position(corrected);
s_snapshot.valid = 1u;
s_snapshot.fresh = 1u;
s_snapshot.spike_rejected = 0u;
s_snapshot.accept_count++;
控制模块通过快照接口消费数据：
bool encoder_service_get_snapshot(encoder_snapshot_t *out)
{
if (out == 0) {
return false;
}
*out = s_snapshot;
if (s_snapshot.fresh == 0u) {
s_snapshot.stale_count++;
}
s_snapshot.fresh = 0u;
return s_snapshot.valid != 0u;
}
5.3.2. 电流采样：先判断窗口，再重构三相电流
ADC 原始值先减零偏并换算成安培，然后根据当前 PWM 占空比判断哪些低边电流样本有效。
current_sense_at32m412_get_offset(&ofs_a, &ofs_b, &ofs_c);
gain = CURRENT_GAIN_DEFAULT_A_PER_LSB;
ia = current_sense_calc(ia_raw, (float)ofs_a, gain);
ib = current_sense_calc(ib_raw, (float)ofs_b, gain);
ic = current_sense_calc(ic_raw, (float)ofs_c, gain);
motor_pwm_at32m412_get_sample_plan(&plan);
current_reconstruction_run(
&plan,
ia, ib, ic,
g_motor_tuning.protection.sample_blanking_ticks,
&sample);
重构模块选择采样窗口，并利用三相电流和为零的关系补回缺失相：
out->ia = -raw_ia;
out->ib = -raw_ib;
out->ic = -raw_ic;
switch (out->reconstructed_phase) {
case CURRENT_RECON_PHASE_A:
out->ia = -(out->ib + out->ic);
break;
case CURRENT_RECON_PHASE_B:
out->ib = -(out->ia + out->ic);
break;
case CURRENT_RECON_PHASE_C:
out->ic = -(out->ia + out->ib);
break;
default:
return;
}
out->frame_valid = true;
这里的负号是低边器件电流到 FOC 相电流方向的统一。这个符号如果漏掉，电流 PI 就可能从负反馈直接变成正反馈。
5.3.3. 三级级联：位置目标逐层变成电压输出
位置模式中，位置环先产生速度目标，速度环再生成 Iq_ref，最后由电流环计算 Vd/Vq。
position_speed_ref = position_loop_run(
encoder_service_get_control_position_mdeg());
speed_loop_set_target_rad_s(position_speed_ref);
if (position_loop_get_snapshot(&position_snapshot) &&
position_snapshot.tracking_fault != 0u) {
current_fault_latch(mc, FAULT_POSITION_TRACKING);
}
if (sample.frame_valid) {
iq_ref =
speed_loop_run(encoder_tracker_get_speed_rad_s()) +
position_loop_get_iq_feedforward_A();
if (iq_ref > g_motor_tuning.speed.output_limit_A) {
iq_ref = g_motor_tuning.speed.output_limit_A;
} else if (iq_ref <
-g_motor_tuning.speed.output_limit_A) {
iq_ref = -g_motor_tuning.speed.output_limit_A;
}
current_loop_set_targets(0.0f, iq_ref);
foc_clarke(sample.ia, sample.ib, sample.ic,
&i_alpha, &i_beta);
foc_park(i_alpha, i_beta, theta, &id, &iq);
current_loop_run(id, iq, &vd_ref, &vq_ref);
}
对应的数据关系就是：
连续机械位置
→ position_loop
→ speed target
→ speed_loop
→ Iq_ref
→ current_loop
→ Vd / Vq
5.3.4. CAN 轨迹：先预装，收到 SYNC 才应用
轨迹帧到达后不会立即改变电机目标，而是先保存为 pending。广播控制帧中的 SYNC 才会触发应用。
static void process_control(const can_broadcast_t *control)
{
switch (control->opcode) {
case CAN_OPCODE_ARM:
process_arm(control);
break;
case CAN_OPCODE_SYNC:
apply_pending(control->sequence,
control->session);
break;
case CAN_OPCODE_CLEAR_FAULT:
process_clear_fault();
break;
default:
break;
}
}
apply_pending() 将 CAN 轨迹转换为位置环使用的统一目标结构：
setpoint.position_mdeg =
s_service.pending.position_mdeg;
setpoint.velocity_mdeg_s =
s_service.pending.velocity_mdeg_s;
setpoint.sequence =
s_service.pending.sequence;
setpoint.lease_ms =
CAN_MOTION_POSITION_LEASE_MS;
if (!s_service.applied_valid) {
result = s_service.ops.position_start(&setpoint);
} else {
result = s_service.ops.position_submit(&setpoint);
}
if (result != 0) {
return;
}
s_service.applied_point = s_service.pending;
s_service.applied_sequence = setpoint.sequence;
s_service.applied_valid = true;
s_service.position_active = true;
s_service.sync_age_ms = 0u;
clear_pending();
set_state(CAN_NODE_STATE_RUNNING);
这样 Node 1 和 Node 2 可以先分别收到同一个 sequence 的关节目标，再通过广播 SYNC 同时交给本地位置环。
5.3.5. 通信状态：短时丢包 HOLD，长时超时停机
CAN 服务以 1 ms 节拍维护轨迹年龄和同步超时。
static void expire_pending_and_watchdog(void)
{
if (s_service.pending_valid &&
s_service.pending_age_ms >=
CAN_MOTION_PENDING_TIMEOUT_MS) {
clear_pending();
}
if (!s_service.applied_valid ||
(s_service.state != CAN_NODE_STATE_RUNNING &&
s_service.state != CAN_NODE_STATE_HOLD)) {
return;
}
if (s_service.sync_age_ms >=
CAN_MOTION_FAULT_TIMEOUT_MS) {
latch_fault_and_stop(FAULT_CAN_TIMEOUT);
return;
}
if (s_service.state == CAN_NODE_STATE_RUNNING &&
s_service.sync_age_ms >=
CAN_MOTION_HOLD_TIMEOUT_MS) {
set_state(CAN_NODE_STATE_HOLD);
}
}
这部分对应两个不同层级：
短时间没有新 SYNC
→ RUNNING 切到 HOLD
→ 保持当前目标
长时间没有有效 SYNC
→ FAULT_CAN_TIMEOUT
→ 停止位置控制并进入安全状态
5.3.6. 故障管理：故障位与控制状态分开处理
各控制模块发现异常时，只需要锁存相应故障位，并把电机控制状态切换到 FAULT。
static void current_fault_latch(motor_control_t *mc,
uint32_t fault)
{
fault_manager_set_bits(fault);
mc->state = MOTOR_CONTROL_STATE_FAULT;
}
故障位的修改使用临界区保护，避免线程和中断同时更新时互相覆盖：
void fault_manager_set_bits(uint32_t bits)
{
#if defined(__CC_ARM) || defined(__arm__) || \
defined(__thumb__)
uint32_t primask;
primask = fault_manager_irq_lock();
s_fault_flags |= bits;
fault_manager_irq_unlock(primask);
#else
(void)atomic_fetch_or_explicit(
&s_fault_flags,
bits,
memory_order_seq_cst);
#endif
}
控制路径判断是否存在致命故障：
bool fault_manager_any_fatal(void)
{
return (fault_manager_get() &
FAULT_FATAL_MASK) != 0u;
}
6. 五连杆 Demo 设计
电机驱动模块完成以后，我想了挺久该用什么方式展示。论坛里有坛友做力反馈旋钮，也有人做交互机器人；我最后选五连杆，是因为它能比较直接地展示两个独立关节是否真的在协同工作，同时也是轮足机器人里比较常见的一类腿部模块。
单电机位置环跑得再顺，也只能证明一个节点。五连杆把两个主动关节通过机械结构耦合起来：两个电机的角度、方向、零点或时序只要有一项不一致，末端运动就会立刻表现出来。
6.1 正运动学：两个电机转到这里，末端会去哪
6.2 逆运动学：想让笔尖到这里，两个电机该转多少
真正书写 MPS 时，输入不是两个电机角度，而是一串末端平面坐标。Z-Trace 需要先把每个末端点换算成两个主动关节角，再分别交给 Node 1 和 Node 2。
这一步还要处理：
1. MPS 图形在机构工作空间内的平移和缩放
2. 轨迹点的离散与顺序
3. 与实物装配一致的运动学支路
4. 两个关节的机械零位和正方向
5. 关节软限位与不可达点检查
示教功能这次没有实现，最终展示使用的是预先生成的 MPS 轨迹。AI帮我根据MPS的logo:
生成了一笔画轨迹:
错了,不是这张(AI还需要调教调教),下面这张看着才顺眼些
轨迹最终在了Z-Trace中:
7. 3D 打印与后处理
五连杆结构采用 3D 打印，主要是为了在项目周期内快速迭代：
打印件不是从机器上拿下来就直接进入最终装配。实际还要检查孔径、转动间隙、连杆平面度和紧固位置。对写字机构来说，某个孔紧一点、某根连杆翘一点，不只会变成电机额外负载，还会直接反映到笔迹上。
这次最终效果一般，也不能只把问题归到控制参数。打印件间隙、关节回差、笔尖与纸面摩擦、笔的固定刚度和纸面是否平整，都会与两个关节的跟踪误差一起叠加。
8. 机构装配
装配按从机械到电气的顺序推进：
1. 安装底座、电机和两侧主动连杆
2. 装入从动连杆，手动检查完整活动范围
3. 调整孔位、紧固和连杆层级，避免干涉
4. 安装两套驱动板并整理供电、CAN 和调试线
5. 将机构回到约定机械零位
6. 配置 Node 1 / Node 2 的 ID、方向和软限位
7. 安装末端夹笔并调整笔尖与纸面接触
8. 先跑小范围轨迹，再逐步放大到完整 MPS 字样
两套节点已经按整机坐标完成配置，并实际进入双节点轨迹运行。装配阶段最关键的并不是“螺丝都拧上了”，而是机械零位能否重复、两个关节正方向是否一致，以及线束在完整行程内会不会拉扯机构。
末端夹笔又增加了一个新的变量：笔尖必须接触纸面才能留下笔迹，但压力太大时摩擦会明显增加，太小时又容易断线。这部分没有单独的抬笔自由度或力控，只能通过机械安装和轨迹范围尽量折中，也是当前字形质量与重复性一般的原因之一。
9. 从 MPS 轨迹到两个电机
最终运行的数据链路如下：
预生成 MPS 末端坐标
-> 按五连杆工作空间进行平移和缩放
-> 离散为连续轨迹点
-> 五连杆逆运动学
-> Node 1 / Node 2 目标角度
-> Z-Trace 以 10 ms 节拍预装双节点轨迹
-> 广播 SYNC
-> 两个 MotorDriver 本地三级闭环
-> 五连杆末端夹笔在纸面运动
这里没有使用示教记录。MPS 字样在运行前已经确定，Z-Trace 负责把末端轨迹转换为关节目标，并按同一个 sequence 发送给两个节点。MotorDriver 不需要知道笔尖要写什么，只负责在本地让关节角跟上各自的目标。
调试仍然按“先拆开，后组合”的顺序推进：
单块驱动板
-> 单电机磁编标定
-> 电流环
-> 速度环
-> 位置环
-> Node 1 单节点 CAN
-> 第二套板本地闭环
-> Node 2 整机坐标配置
-> 双节点小范围联动
-> 完整 MPS 轨迹
最终联调时，五连杆末端夹持一支笔，Z-Trace 下发预先生成的 MPS 轨迹，两块驱动板分别控制两个主动关节。机构能够连续跑完整段轨迹，并在纸面留下实际的 MPS 字样。
从演示效果看，整段动作是流畅的，没有把三个字母拆成互不相干的点动。但当前字样还谈不上精细：局部线条和比例不够理想，多次运行的一致性也一般。
更准确的完成度可以概括为：
完成了：
五连杆末端夹笔
纸面留下连续 MPS 实体笔迹
整段预生成轨迹能够流畅跑完
还需要优化：
字形比例与局部线条
多次运行的一致性
机械间隙和零位重复性
笔尖摩擦、固定刚度与纸面接触
轨迹缩放、离散和关节跟踪参数
这次没有额外给字形误差、成功率或重复次数编一个漂亮数字。就现有结果而言，它已经完成了最初的展示目标：让两套独立电机驱动经过 CAN 和五连杆机构，真正把预设图形写到了纸上；同时，实物也把机械精度和重复性的问题很直接地暴露了出来。
11. 作品源码、编译与使用说明
代码托管在GITHUB
11.1 工程结构
MotorDriver 侧主要代码分为：
application/motor_control/ FOC、三环、标定、故障和 CAN 运动服务
platform/at32m412/ PWM、ADC、编码器、CAN、Flash 和板级外设
communication/ CAN 帧、协议编解码和驱动边界
middlewares/msp/ma600/ MA600A 协议与 AT32 SPI2 适配
tests/ 主机数学测试、静态契约和台架脚本
project/MDK_V5/ Keil ARMCC5 工程与烧录脚本
Z-Trace 侧与最终演示直接相关的功能包括：
预设 MPS 末端轨迹
轨迹平移、缩放与离散
五连杆逆运动学
双关节目标生成
10 ms CAN 轨迹调度
Node 1 / Node 2 预装与广播 SYNC
反馈和故障状态显示
这里暂时只列功能模块，由于该部分代码无关比赛 暂未整理提交.
11.2 构建与烧录
MotorDriver 同时维护 CMake/GCC 和 Keil ARMCC5 构建路径。当前硬件主要使用 Keil clean build 生成固件，再通过 J-Link 擦除、烧录和 Verify。
基本流程：
1. Keil ARMCC5 clean build，确认 0 Error / 0 Warning
2. J-Link 连接 SWD，烧录并 Verify
3. 12 V 限流供电
4. 新板完成电流零偏和 MA600A 标定
5. 分别验证单电机三环和单节点 CAN
6. 在机构机械零位配置两个节点
7. 先执行小范围双节点轨迹
8. 安装夹笔并运行完整 MPS 轨迹
CAN 使用 1 Mbps Classic CAN。首次联调需要确认终端、电气连接和 ACK 对端；机构运行前还要检查两个节点状态、关节方向、软限位和末端活动范围。
12. 过程帖与 AI 协同开发
我从项目立项开始就全程让 AI 参与，能让他干的我就只下达指令, 主要包括：
比赛规则和任务拆分
芯片资料与代码路径梳理
上电检查表和调试记录模板
异常现象的排查树
控制与 CAN 代码实现、复查
主机测试和台架脚本
测试数据整理、过程帖和视频脚本
这个项目也正好能说明 AI 在硬件开发里的边界(当然AI能延展到多少也取决于使用者的能力,故仅代表我个人的使用情况…)。它很擅长快速阅读大量资料、拆分任务、补测试、整理日志，并且能在软件问题里提供不少排查方向。但它不能替我判断磁铁有没有偏、SOx 电流方向是不是符合物理约定、打印件是不是卡住，也不能仅凭代码宣布五连杆已经跑通。
中间一些看起来合理的建议，例如测速窗口、主动制动和补偿方向，经过硬件 A/B 测试以后并没有保留。最终采用什么，仍然由波形、日志和实物结果决定。
这次 AI 最终参与了从立项、代码、测试到内容整理的多个环节，但作品的验收标准很朴素：两块真实的板、两个真实的电机，最后能不能让笔尖把 MPS 写在纸上。
开发过程记录贴：
13. 项目总结
这个项目最终做出来的不只是一块三相驱动板，而是一条从电源、功率级、传感器、实时控制到双节点机构和纸面书写的完整链路：
V1 / V2 电机驱动板
-> V2 同轴 MA600A
-> 电流、速度、位置三级闭环
-> 双节点 CAN 轨迹与广播 SYNC
-> 五连杆运动学和 3D 打印机构
-> 末端夹笔
-> 纸面 MPS 实体字样
过程中比较有代表性的工程取舍包括：
V1 旁轴节省空间，V2 同轴降低传感不确定性
SPI 编码器从 16 kHz FOC ISR 移到 4 kHz 独立采集
低边电流镜在重构边界统一转换极性
速度环保留低速纹波边界，不用平均值冒充瞬时性能
双节点轨迹使用预装 + SYNC，不让谁先收到谁先跑
关节零点、方向和软限位写入配置，不为两台板维护两份固件
预生成末端轨迹通过逆运动学转换为两个关节目标
现在五连杆已经能够把完整 MPS 轨迹流畅跑下来，并留下实际笔迹，这说明从轨迹、运动学、CAN、双节点闭环到机械机构的主链路已经接通。
它离“高精度写字机”还有明显距离：字形效果一般，多次运行的一致性也一般。后面如果继续优化，重点会放在机械间隙、零位重复性、笔尖接触、轨迹离散和双关节跟踪上，而不是简单把 PI 增益继续往上拧。
对我来说，这个结果已经足够回答项目最开始的问题：AI 可以明显加快资料整理、软件实现、测试和记录，但硬件项目最后仍然需要靠实物闭环。好在这次饺子虽然包大了，最后确实在纸上留下了 MPS