运动控制：用 C 算目标位置

运动控制里，最核心的一步是算下一拍该走到哪。目标、当前位置、步距一摆，下一拍的位置就出来了。这一步用 C 算最清楚，算好再交给脉冲指令去执行。

给三个数就行：目标位置 HD0、当前位置 HD1、单拍最大步距 HD2。目标就是要到哪，当前是现在哪，步距是一拍最多走多少。三个寄存器喂进去就动。

算法就一招：先算目标减当前得差值，再决定这拍走多少。差值比步距大，就整整走一步；差值比步距小，就只走剩下的那点，绝不过冲，这就是逼近的精髓。

差值为正就往前，为负就往后，C 里取个正负号就知道方向。走到目标那拍，给个 arrive 位说一声到了。差值为零就停手，稳稳当当停在目标线上。

逼近有个讲究：最后一段哪怕只剩 1 个脉冲，也老老实实只走 1 个，不硬凑一整步。所以速度是匀速加一小段收尾，位置分毫不差，不会过冲回弹。

用 C 算目标位置就四步：算差值，定方向，按步距取小，到位置位。算好的位置交给脉冲或定位指令执行，算和发分开。单轴也好、联动也好，都是这套。