24. Profile Position 位置运动

学习形式运动实验

硬件要求完整实验平台

配套 Demolab_24_profile_position

本课动作电机会实际运动

这一课完成第一次明确的位置运动。程序使用 CiA 402 Profile Position 模式，先读取当前状态和位置，再由串口命令触发小距离移动。

运动不再是“发送一个神秘数字”。我们会先把圈、角度、counts、绝对位置和相对移动对应起来。

当前验证状态：本课独立工程已编译、烧录并完成 test 往返验证，固件大小 243840 字节。电机从 9622 counts 前往目标 13718，实际到达 13720；随后返回目标 9622，实际到达 9624，两段误差均为 2 counts。记录见第 24 课上板记录。

这一课代码较多，所以先把职责拆开：

串口任务只负责把文字变成一条完整命令；CANopen 主任务负责安全检查和电机通信。两边通过 FreeRTOS 队列交接，串口日志再多，也不应该把半条命令直接当成运动请求。

从第 23 课建立运动工程

新建标准工程 lab_24_profile_position，把第 23 课已经上板通过的 CANopen 初始化、SDO upload/download、OD.c、OD.h 和组件依赖带入新工程。不要从运动函数开始写，先原样构建并运行 status 读取逻辑，确认电机仍能读取且不会运动。

然后把主文件改名为 canopennode_motor.c，新建：

main/
├── canopennode_motor.c
├── motor_commands.c
├── motor_commands.h
├── OD.c
├── OD.h
├── CMakeLists.txt
└── idf_component.yml

main/CMakeLists.txt 写成：

# canopennode_motor.c 负责总线和运动，motor_commands.c 只负责串口解析，OD.c 提供对象字典。
# 分开编译后，输入错误不会直接混入 SDO 状态机，运动逻辑也不依赖半行串口字符。
idf_component_register(
    SRCS "canopennode_motor.c" "motor_commands.c" "OD.c"
    INCLUDE_DIRS "."
    REQUIRES espressif__canopennode esp_driver_twai esp_timer
)

先给两个新文件写最小函数，执行一次 idf.py build。这轮只验证拆文件和 CMake，不写 0x607A，也不运动。

本实验的位置单位

厂家手册写明每转反馈 16384 个脉冲，本项目当前按下面关系换算：

1 圈 = 16384 counts
2 圈 = 32768 counts
90°  = 16384 × 90 / 360 = 4096 counts

这些 counts 描述当前电机配置下的位置数值。若输出端有减速箱或电子齿轮，最终机构角度需要另行核对。

绝对移动和相对移动

假设当前位置 0x6064 = 71339：

命令意图	目标值
绝对移动到 10000	0x607A = 10000
正向相对 4096	0x607A = 71339 + 4096 = 75435
反向相对 32768	0x607A = 71339 - 32768 = 38571

程序中的 jog 命令先读当前位置，再计算新的绝对目标；move 则直接把参数当作绝对目标位置。

串口命令应该怎样读

命令	明确含义
status	读取状态、模式和当前位置，不运动
enable	只执行 CiA 402 使能，不运动
prep	检查 Profile Position 所需对象
jog rev 1	正向转 1 圈
jog rev -1	反向转 1 圈
jog deg 90	正向转 90°
jog 4096	正向移动 4096 counts
move 10000	移动到绝对位置 10000 counts
test	正向 4096 counts，再回到起点
diag on / diag off	开关周期诊断日志

以 jog deg 90 为例，程序不是把字符串直接发给电机：

因此串口解析、单位换算、当前位置读取和运动触发都能分别检查。看到目标位置不符合预期时，应先沿图向前查，不要立刻怀疑 CAN 物理层。

输入完整一行后按回车。若日志太多，先输入：

diag off

命令解析器逐字符缓存，只有收到 \r 或 \n 才提交一整条命令；正常情况下不会输入一个字就执行。

第一步：写命令的数据结构

打开 motor_commands.h，先写位置常量：

// 16384 counts/圈来自当前电机配置；4096 counts 是四分之一圈的最小测试位移。
// 若电子齿轮或减速比改变，必须重新核对这两个值，不能只修改显示单位。
enum {
    MOTOR_COMMAND_COUNTS_PER_REV = 16384,
    MOTOR_COMMAND_TEST_JOG_COUNTS = 4096,
};

再把可能收到的命令写成枚举：

// 串口文本先被解析成枚举，电机任务只处理明确命令类型，不直接比较用户输入字符串。
// 这样输入解析和真实运动分属两个模块，错误单词不会落入运动分支。
typedef enum {
    MOTOR_COMMAND_HELP,
    MOTOR_COMMAND_STATUS,
    MOTOR_COMMAND_ENABLE,
    MOTOR_COMMAND_PREPARE,
    MOTOR_COMMAND_JOG_RELATIVE,
    MOTOR_COMMAND_MOVE_ABSOLUTE,
    MOTOR_COMMAND_TEST_JOG,
    MOTOR_COMMAND_DIAGNOSTIC_ON,
    MOTOR_COMMAND_DIAGNOSTIC_OFF,
} motor_command_type_t;

最后定义队列中真正传递的数据：

// FreeRTOS 队列复制整个结构体：type 表示动作，value 保存 counts 或开关参数。
// status/help 等无参数命令忽略 value；jog/move 必须使用已经检查过范围的 int32_t。
typedef struct {
    motor_command_type_t type;
    int32_t value;
} motor_command_t;

例如 status 只使用 type；jog -4096 则使用 type=MOTOR_COMMAND_JOG_RELATIVE 和 value=-4096。写完头文件和空实现函数后先编译一次。

第二步：先解析数字，再解析整条命令

在 motor_commands.c 中先写一个只负责 int32_t 的函数。核心使用 strtol()，但必须同时检查：

• 没有读到数字；
• 数字超出 INT32_MIN 到 INT32_MAX；
• 数字后面还有无法识别的字符；
• errno 报错。

解析 counts 成功后，再加入 rev 和 deg 换算。角度换算使用：

// degrees 使用浮点数完成角度换算，避免 90/360 先做整数除法而得到 0。
// 得到 scaled 后还要检查 INT32_MIN～INT32_MAX，再转换成电机对象使用的 counts。
scaled = degrees * MOTOR_COMMAND_COUNTS_PER_REV / 360.0;

换算结果仍要检查是否超出 int32_t。不能让一个非常大的串口参数在转换时溢出成方向相反的小数字。

最后写 parse_motor_command()：先取第一个单词判断命令类型，再把剩余文字交给对应的参数函数。到这里可以在不连接电机的情况下，用 help、status、错误单词和超大数字检查解析结果。

第三步：收到回车才提交命令

串口任务每次只从 getchar() 得到一个字符。字符先进入固定长度数组：

// 数组最多保存 95 个可见字符，最后一个位置留给 C 字符串结束符 \0。
// line_len 只记录当前有效字符数，Backspace 和回车都通过它操作边界。
char line[96] = {0};
size_t line_len = 0;

处理规则是：

输入	程序动作
普通可打印字符	放进 line，但不执行
Backspace	删除最后一个字符
\r 或 \n	给字符串补 \0，解析完整命令
超过 95 个字符	丢弃本行并提示过长

解析成功后才发送到队列：

// 解析器先确认命令名、参数格式和数值范围；失败时不会向电机任务发送任何内容。
motor_command_t command = {0};
if (parse_motor_command(line, &command)) {
    // 队列复制 command，串口任务可以继续接收下一行；电机操作始终由另一个任务串行执行。
    xQueueSend(command_queue, &command, pdMS_TO_TICKS(100));
}

这正是“输入一个字不会立刻执行”的代码保证。完成这一阶段后先烧录，只测试 help 和 status；暂时不要给命令分派器加入运动分支。

运动前程序检查什么

开始移动前，程序依次确认：

1. 0x6041 可读取且没有 Fault；
2. 驱动已到 Operation enabled；
3. 0x6061 显示 Profile Position，也就是值 1；
4. 0x6064 当前实际位置可读取；
5. 0x6081 Profile Velocity 可读取；
6. 0x6083 和 0x6084 加减速度对象可读取。

厂家没有实现的可选标准对象会返回 Abort，程序记录后继续；必要核心对象不可用时，运动步骤停止。

为什么 prep 会出现很多 Abort

实测中这些对象返回过：

abort=0x06020000 (object does not exist)

包括 0x607D、0x6086、0x608F、0x6091 和 0x6092。它表示该设备没有实现这些对象，不表示 CAN 总线坏了。

prep 最终报告成功，是因为本实验真正依赖的状态、模式、位置、速度和加减速对象可用；对可选对象的探测失败被如实保留。

Profile Position 的关键写入顺序

一次运动大致包括：

1. 必要时写 0x6060:00 = 1 选择 Profile Position；
2. 读取 0x6061，确认显示模式确实变成 1；
3. 读取 0x6064 得到当前位置；
4. 计算并写入 0x607A:00 Target position；
5. 通过 0x6040 的 new set-point 变化让目标生效；
6. 周期读取 0x6041 和 0x6064；
7. 看到 target reached 并核对实际位置。

不能只写 0x607A 就假设电机必然开始运动；CiA 402 的模式、状态和 set-point 握手共同决定目标是否执行。

第四步：先写一个绝对位置运动函数

在已经验证的 course_sdo_download_u16() 基础上，再实现一个 int32_t 下载函数，用于 0x607A。它和 16 位版本的主要差别是四字节小端序：

// 先转成 uint32_t 是为了保留负数的 32 位补码位模式，再按小端序拆成 4 字节。
// 例如 -32768 的 raw 是 0xFFFF8000，发送顺序应为 00 80 FF FF。
uint32_t raw = (uint32_t)target_position;
uint8_t data[4] = {
    (uint8_t)(raw & 0xFFU),
    (uint8_t)((raw >> 8) & 0xFFU),
    (uint8_t)((raw >> 16) & 0xFFU),
    (uint8_t)((raw >> 24) & 0xFFU),
};

先只编译这个函数，不调用它。然后实现一次绝对运动：

// 0x000F 先清除上一次 new set-point，写入 0x607A 后再用 0x001F 产生新目标边沿。
// 随后恢复 0x000F，最后通过 Statusword 和实际位置共同确认运动完成。
static bool run_profile_position_single_move(CO_t *co,
                                             int32_t target_position,
                                             const char *move_name)
{
    // 清 bit 4，确保下一次置位形成新的 0→1 边沿，而不是沿用上一次请求。
    if (!write_motor_controlword(co, 0x000F,
                                 "Clear previous set-point")) {
        return false;
    }
    // 目标位置写入 0x607A；此时还没有触发 new set-point。
    if (!write_motor_target_position(co, target_position, move_name)) {
        return false;
    }
    // 0x001F 在保持 Operation enabled 的同时置 bit 4，通知驱动接受新目标。
    if (!write_motor_controlword(co, 0x001F,
                                 "Trigger new set-point")) {
        return false;
    }
    // 再清 bit 4，为下一次运动准备新的触发边沿。
    if (!write_motor_controlword(co, 0x000F,
                                 "Clear new-setpoint bit")) {
        return false;
    }
    return wait_for_profile_position_target(co, target_position, move_name);
}

参考工程在每次 SDO 成功后还留有短暂状态稳定时间，并为每一步打印明确对象和值。自己实现时不要把四次写压缩成一行，否则失败时无法知道停在哪一步。

第五步：等待目标，而不是延时猜测

wait_for_profile_position_target() 反复读取：

对象	用来判断什么
0x6041	Operation enabled、target reached、Fault 等状态位
0x6064	实际位置与目标相差多少 counts

程序计算距离时要使用 64 位中间值，避免两个 int32_t 相减溢出：

// 两个 int32_t 直接相减可能先溢出；先提升到 int64_t 才能得到可靠的有符号误差。
int64_t error = (int64_t)target_position - snapshot.position_actual;
// distance 只表示绝对距离，先处理正负再转 uint64_t，后面才能与无符号容差比较。
uint64_t distance = (uint64_t)((error < 0) ? -error : error);

只有 target reached 置位并且距离不超过容差，才返回成功。循环还必须有 5000 ms 超时；通信失败、Fault 或超时都返回 false，不能用固定延时后直接打印“到达”。

第六步：在命令分派器中接通运动

前三次上板只允许 help、status、enable 和 prep。这些都通过后，再把命令队列中的 JOG_RELATIVE、MOVE_ABSOLUTE 和 TEST_JOG 接到运动函数。

相对 jog 不能直接把 4096 写入 0x607A。它应先读 0x6064，使用 64 位计算：

// jog 的 value 是相对位移，必须先加到刚读回的 0x6064 实际位置上。
// 使用 int64_t 保存中间结果，确认仍在 int32_t 范围后才允许写入 0x607A。
int64_t target = (int64_t)current_position + command.value;

确认目标仍在 int32_t 范围内后，才调用绝对位置运动函数。这样 jog 4096 的含义始终是“从当前位置增加 4096”，而 move 4096 才是“去绝对位置 4096”。

为什么目标位置是 INTEGER32

位置可能在零点两侧，所以 0x607A 和 0x6064 使用有符号 32 位数。

例如目标 -32768 的 32 位补码是 0xFFFF8000，发送到 CANopen SDO DATA 时仍按小端序排列：

00 80 FF FF

这是负数的字节表示，不是四个独立参数。

从最小动作开始

推荐首次操作顺序：

status
enable
prep
jog deg 10
status
jog deg -10
status

先用 10°，观察方向、声音、LED 和实际位置变化。确认机械方向和单位后，再尝试：

jog rev 1
jog rev -1

不要一开始输入很大的绝对 move 数值。绝对位置与当前零点有关，目标离当前位置可能远超肉眼预期。

test 命令做什么

test 的设计目标是容易观察且能够回到起点：

1. 读取并保存起始 0x6064；
2. 移动 +4096 counts，也就是当前换算下约四分之一圈；
3. 等待目标完成并读取实际位置；
4. 把起始位置重新写为目标；
5. 再次等待并核对返回位置。

它仍然是真实运动，不是自检模拟。执行前同样要清空机构周围空间。

写完后按文件核对职责

文件	负责内容
canopennode_motor.c	CANopen 初始化、SDO、Heartbeat、CiA 402 和运动流程
motor_commands.c	串口逐字符输入、参数解析、命令帮助
motor_commands.h	命令类型、位置单位和公开接口
OD.c / OD.h	本地 CANopen 对象字典

串口解析与协议流程分开后，修改命令输入不会扰乱 SDO 状态机，协议日志也不会把半条命令当作输入。

完整工程位于 examples/lab_24_profile_position/。

建议按下面五个检查点构建和上板，不要第一次就执行运动：

检查点	新增内容	只做什么验证
1	拆分源文件和 CMake	idf.py build 成功
2	命令结构、逐字符输入、队列	输入 help 不会逐字执行
3	status、enable、prep	只读状态并完成准备
4	单次绝对运动和目标等待	首次执行 jog deg 10
5	单位换算和往返逻辑	执行 test 并回到起点

每个检查点都通过后再继续。这样即使电机没有按预期动作，也能判断问题在串口解析、单位换算、SDO 写入还是目标等待。

构建、烧录和监视

# 在当前终端加载 ESP-IDF 环境；新开终端后需要重新执行。
cd "$HOME/esp/can-canopen-course/examples/lab_24_profile_position"
source "$HOME/esp/esp-idf-v5.5.4/export.sh"
idf.py build
idf.py flash monitor

ESP-IDF 通常会自动寻找唯一串口；自动识别失败或存在多个设备时才添加 -p /dev/ttyUSB0。

上板验证记录必须包含什么

本课完成后，验证文件至少保存：

• 固件烧录成功和启动配置；
• 电机 Heartbeat 0x701；
• 运动前后的 0x6041、0x6061、0x6064；
• 写 0x607A 和 0x6040 的成功响应；
• 目标 reached 的判断；
• 实物观察到的方向和大致角度；
• 是否出现报警、复位或非预期持续运动。

只有串口数字变化而没有观察实物，不足以确认单位和方向；只有电机转了而没有读回状态，也不足以证明协议流程正确。