TI5 SDK核心开发指南

1. 概述

本指南面向 TI5 机器人 SDK 开发者，整合以下两类能力：

关节与机器人本体控制（robot_model_menu、motor_menu）
外设控制（PeripheralSDK，包含灵巧手、六维力、底盘等）

目标是提供一份统一入口文档，帮助你快速完成环境搭建、接口调用、示例运行与问题定位。

1.1 支持的平台

Linux x86（推荐 Ubuntu 22.04）
Linux ARM64（推荐 Ubuntu 22.04）

2. 快速开始

2.1 Ti5_SDK 安装与部署

硬件准备

CAN 设备（如 PEAK CAN、SocketCAN 适配器）
目标关节/电机模组
机器人本体（如 T140A / T170A / T170C / T170D，按测试需求）
外设（灵巧手、底盘、六维力传感器）

软件依赖

编译器：gcc/g++（建议支持 C++17）
构建工具：cmake、make
CAN 相关：libsocketcan-dev
其他依赖：libengin3-dev（按平台与工程要求安装）

一键安装示例（Debian/Ubuntu）：

sudo apt update
sudo apt install -y build-essential cmake libsocketcan-dev libengin3-dev

CAN 驱动安装说明（Linux）

SocketCAN：确认内核已启用 CAN（或对应模块可加载）；接口就绪后可用 ip link、candump 等检查。
PEAK：通常需安装厂商驱动包；就绪后结合板载指示灯、lsmod、ip link 综合判断。

下面以 PEAK Linux 驱动 为例（包名与版本以实际下载为准，如 peak-linux-driver-8.20.0）。部分平台（例如与 mttcan 等片上 CAN 控制器共存时）可能出现资源冲突，可按需先屏蔽冲突模块再安装 PEAK 驱动。

禁用与 PEAK 冲突的内核模块

若安装后模块无法加载，可尝试将 mttcan 加入黑名单（配置文件不存在时自行创建即可）：

sudo vim /etc/modprobe.d/denylist-mttcan.conf

在文件末尾追加：

blacklist mttcan

更新 initramfs，重启后生效：

sudo update-initramfs -u

编译并安装 PEAK 驱动

tar -xvf peak-linux-driver-8.20.0.tar
cd peak-linux-driver-8.20.0
make netdev
sudo make install
sudo reboot

验证

完全重启后：红色常亮 多见于驱动已正确加载；快闪多表示驱动未装好。可再执行：

lsmod | grep peak

若能看到 peak_* 等相关模块，通常表明驱动已加载。

故障排除与卸载

现象	建议
模块加载失败	确认已按需屏蔽冲突模块并重启；核对内核与驱动版本是否匹配
权限不足	安装与卸载步骤在需要处使用 `sudo`
编译报错	安装与当前内核一致的 `linux-headers`（或发行版提供的对应头文件包）

在驱动源码目录卸载：

sudo make uninstall

参考：PEAK System 官方站点 · Linux Kernel CAN 文档

2.2 编译与运行

交付物分两类，本节先说明如何区分与查阅：

本体控制标准 SDK：路径形如 Ti5SdkDemo/linux_x64、Ti5SdkDemo/linux_arm64.
外设独立 SDK：路径为 Ti5SdkDemo/PeripheralSDK/，按设备拆分多个子工程（如 hand_sdk、chassis_sdk、sri_can_sdk），各自独立 CMakeLists.txt 与示例.

项目结构

以下 本体控制标准包 以 Linux x64 预编译 SDK 为例，linux_arm64 等目录布局原则相同，差异主要在 runtime 内库文件后缀与 3rdparts 中部分平台二进制。

目录树概览

Ti5SdkDemo/linux_arm64/
├── CMakeLists.txt          # 本包统一 CMake 入口：导入 Ti5RobotControl、编译示例/小工具
├── include/                # SDK 对外头文件（集成时主要依赖此目录）
│   ├── api/                # C API：关节/机器人等（如 MotorCtrlApi、RobotCtrlInstMng、灵巧手接口等）
│   └── basic/              # 错误码、连接参数、日志、JSON、定时器、平台类型等基础能力
├── runtime/                # 运行时二进制：Ti5RobotControl 动态库（.so）及辅助脚本
├── bin/                     # 默认生成 motor_menu、robot_model_menu（构建后）
├── test/                   # 随包示例与自测源码（非公开 API 的封装，仅演示用法）
│   ├── include/            # 示例内部头文件（cmd / comm / cpp）
│   ├── src/
│   │   ├── cpp/            # 交互菜单主程序及各机型 *\_Test 实现
│   │   └── comm/           # HumanoidBody、关节组、电机辅助、臂 IK 等封装
│   └── json/               # 示例 JSON（机型配置、轨迹等）及说明
├── 3rdparts/               # 示例链接的第三方依赖（头文件或源码）
│   ├── spdlog/             # 日志库
│   ├── json/               # nlohmann/json
│   └── tcan/               # CAN 接口头文件及各平台库路径（按目标平台选用）
└── build/                  # 本地 CMake 构建目录（可选；通常不随发行包提交）

各目录说明

路径	作用
`include/`	给集成方包含的头文件；业务代码通常只需 `#include` 其中 `api/` 与必要的 `basic/`.
`runtime/`	运行期动态库：`Ti5RobotControl.so`、`Ti5_Arm_2204.so`（双臂 IK/FK）、`libcontrolcan.so` 等；通过 `scripts/init.sh` 部署到系统库并执行 `ldconfig`。
`bin/`	当前默认编译产物：`motor_menu`（单电机）、`robot_model_menu`（整机/双臂）；输出于此。
`test/`	官方示例：演示如何调用 DLL 中 API.
`3rdparts/`	示例工程依赖，不替代 `include` 中的公开 API；二次开发可替换为自己的依赖管理方式.

集成/SDK 分发时，除业务工程外，一般需要保留 include/ + runtime/；示例与构建缓存可按需裁剪。

构建方式

以下以 本体包 linux_arm64 为例。从包根目录（如 Ti5SdkDemo/linux_arm64）直接使用 CMakeLists.txt 进行构建，所有示例/工具目标一次生成：

cd Ti5SdkDemo/linux_arm64
cmake -B build -S .
cmake --build build -j "$(nproc)"

-j 为并行编译线程数；$(nproc) 自动取当前机器逻辑 CPU 数，也可改为固定值（如 -j 10）。最终生成的可执行文件位于 bin/（motor_menu、robot_model_menu），runtime/ 存放运行所需依赖库。

运行方式

1. 环境初始化（首次部署或重新编译后必做）

运行前执行 scripts/init.sh：将 runtime/ 依赖库部署到系统库路径并执行 ldconfig，同时为 bin/ 下可执行文件授予 CAN 所需的 setcap 权限，便于普通用户运行（无需 root）。

cd <包根目录>/scripts
chmod +x init.sh
./init.sh

若目录布局与默认不一致，可传入包根路径：./init.sh /path/to/linux_arm64。

2. 运行示例程序

初始化完成后，在包根目录下进入 bin/ 启动交互菜单：

cd <包根目录>/bin

# 单电机联调（禁止用于整机）
./motor_menu

# 整机型号测试（T140A / T170A / T170C / T170D）
./robot_model_menu

若未执行 init.sh、或从本地 build/ 目录直接运行，需先指定动态库路径，例如：

export LD_LIBRARY_PATH=<包根目录>/runtime:${LD_LIBRARY_PATH}
cd <包根目录>/bin
./robot_model_menu

菜单项说明见下文 「可执行文件与脚本说明」。

1. 可执行文件与脚本说明

以下可执行文件由本包 CMakeLists.txt 构建，产物位于 bin/。

1.1 单电机测试（motor_menu）

备注

强烈提示： motor_menu 仅用于单电机 CSP/读参联调，禁止用于整机、多关节或人形机器人测试。该程序未实现关节角度限位与整机安全策略，误用于多轴/整机可能导致异常力矩、超限位、碰撞或设备损坏。整机与双臂测试请使用 robot_model_menu。

可执行文件名称: motor_menu
用途：单电机交互菜单（CSP、读参），用于 CAN 总线与单轴快速验证。
安全提示行为：程序启动时打印醒目 WARNING 横幅；进入 CSP/读参流程时再次提示；菜单内亦标注「仅单电机联调」。
菜单编号说明：界面上提供 1（CSP）、3（读全参） 与 0（退出）（PT 测试暂未开放）。

# 交互菜单（示例：省略时间戳/日志前缀）
# 菜单中无 2 号项：2 本应为 PT，功能未完备故暂不开放展示。
./motor_menu

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!! WARNING: motor_menu 仅用于【单电机】联调与参数读写。
!! 禁止用于整机/多关节/人形机器人测试 — 无关节限位与安全策略。
!! 整机测试请使用 robot_model_menu。
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

=============== Single motor test menu ===============
  !! 仅单电机联调 — 禁止用于整机/人形机器人测试 !!
  1) CSP (read-all-params then CSP submenu)
  3) Read all parameters (hardware)
  0) Exit
Input choice (1..3):

1. 机器人测试（robot_model_menu）

备注

强烈提示： 菜单中选择机型与运动项时，必须与实际连接的机器人型号、关节配置一致。若控制指令、运动学或关节组与真实硬件不符（信号/模型误差），可能导致异常力矩、超限位、碰撞或设备损坏；运行前请确认机型与现场实物一致。

与 motor_menu 的区别：robot_model_menu 面向整机/多关节实机演示；不要用 motor_menu 替代本入口。

可执行文件名：robot_model_menu
用途：T140A / T170A / T170C / T170D 整机交互式实机演示。

第一步：选择型号

./robot_model_menu

================ 整机型号测试 ================
  1) T140A
  2) T170A
  3) T170C
  4) T170D
  0) 退出
============================================
请选择型号 (0-4):

第二步：T170C / T170D 测试菜单（T170D 第 5 项为升降）

编号	含义
1	双臂普通运动
2	双臂末端解算运动
3	头部运动测试
4	腰部运动测试
5	腿部蹲起（T170D 为升降）
0	返回型号选择

==================================================
T170C 测试菜单
--------------------------------------------------
  1  双臂普通运动
  2  双臂末端解算运动
  3  头部运动测试
  4  腰部运动测试
  5  腿部蹲起测试
  0  返回型号选择
==================================================
请选择 (0-5):

T140A / T170A 测试菜单

编号	含义
1	双臂普通运动
2	双臂末端解算运动
6	整机关节空间测试
0	返回型号选择

2. 环境初始化脚本

脚本名称: init.sh

备注

强烈提示： 每次重新编译都会在输出目录生成新的可执行文件（新 inode）；对可执行文件设置的 setcap 等能力不会自动迁移到新文件上。若需在普通用户下访问 CAN 等能力，请在每次编译产物更新后重新执行 init.sh，以便为新二进制重新赋予权限。

推荐用法（理想情况）：将 SDK 包按发布目录结构解压或放置到位后，通常只需赋予脚本可执行权限并无参运行 ./init.sh 即可完成库路径、权限等初始化；脚本会按规则自动查找 bin/ 与 runtime/（见下）。若目录布局特殊或需指定路径，再使用下文带参方式。
作用概要：
- 检查本机是否具备 Eigen3、libsocketcan 等开发/运行前置（不满足时打印安装提示并退出）。
- 定位 bin/ 与 runtime/：
  - 默认无参：优先使用 scripts/init.sh 所在目录的 bin/ 与 runtime/（若缺则再尝试脚本上一级目录的 bin/ 与 runtime/）。
  - 也支持显式传参：传入包根目录（该目录下应有 bin/ 与 runtime/），或传入显式 bin_dir + runtime_dir。
- 使用 sudo 将 runtime/ 中列出的 .so 拷贝到系统库目录并执行 ldconfig，保证动态链接器能找到 Ti5RobotControl 等依赖。
- 使用 sudo setcap 为 motor_menu、robot_model_menu 等可执行文件设置 cap_net_admin,cap_net_raw，使普通用户态运行即可访问 CAN 相关能力（仍需已安装 libcap2-bin 等，脚本会检查 setcap）。
用法示例：

chmod +x init.sh
# 1) 默认无参（最常用）：理想情况下只需执行这一句；脚本会在 init.sh 所在目录的同级，
#    或再往上一级目录中查找 bin/ 与 runtime/
./init.sh

1. 底盘（chassis）安装与部署

内部目录：PeripheralSDK/chassis_sdk

1.1 依赖环境

建议系统：Ubuntu 22.04（ARM/x86 均可）
编译器：gcc/g++（支持 C++17）
构建工具：cmake、make
运行前置：准备好底盘接入端的 host 与 port，确保网络可达（示例运行时需要传入）。

1.2 通用编译步骤

以 chassis_sdk 为例：

cd <你的路径>/PeripheralSDK/chassis_sdk
mkdir -p build
cd build
cmake ..
make -j

1.3 运行方式

编译后可执行：

./build/chassis_sdk_demo <host> <port> <cmd> [value] [duration_ms]

cmd 含义：

forward：前进（value = vx）
lateral：横移（value = vy）
rotate：旋转（value = wz）
stop：停车
query：查询连接与最近一次速度指令

示例：

./build/chassis_sdk_demo 169.254.128.2 5480 forward 0.1 2000
./build/chassis_sdk_demo 169.254.128.2 5480 rotate 0.2 1500
./build/chassis_sdk_demo 169.254.128.2 5480 query

说明：更完整的控制封装与接口用法，请以 PeripheralSDK/外设控制开发指南.md 为准。

2. 灵巧手（hand）安装与部署

内部目录：PeripheralSDK/hand_sdk

2.1 串口与权限准备

准备串口设备路径（如 /dev/ttyACM0，以实际设备为准）
确保当前用户具备串口访问权限（权限配置按你所用系统实际情况调整）

2.2 通用编译步骤

以 hand_sdk 为例：

cd <你的路径>/PeripheralSDK/hand_sdk
mkdir -p build
cd build
cmake ..
make -j

2.3 运行方式

编译后可执行：

./build/hand_sdk_demo <serial_port> [action_id]

action_id 含义：

0：STRAIGHT
1：VICTORY
2：OK
3：HEART
4：QUERY_POS（仅查询当前位置）

示例：

./build/hand_sdk_demo /dev/ttyACM0 1
./build/hand_sdk_demo /dev/ttyACM0 4

说明：更完整的控制封装与接口用法，请以 PeripheralSDK/外设控制开发指南.md 为准。

3. 六维力传感器（sri_can）安装与部署

内部目录：PeripheralSDK/sri_can_sdk

3.1 CAN 前置条件

建议系统：Ubuntu 22.04
sri_can_demo 仅 Linux 构建目标：依赖 SocketCAN
使用 CAN 接口时通常需要 root 或具备 cap_net_admin 权限（与你系统的 CAN 权限策略一致即可）

3.2 通用编译步骤

以 sri_can_sdk 为例：

cd <你的路径>/PeripheralSDK/sri_can_sdk
mkdir -p build
cd build
cmake ..
make -j

说明：非 Linux 下该目录可能只生成头文件接口库，不会产出可执行 demo。

3.3 运行方式（示例）

编译后可执行（默认 CAN 名 vcan0，可用参数覆盖）：

./build/sri_can_demo [can_interface]

示例：

./build/sri_can_demo can0

4. 关节控制 API

本章按能力域介绍核心接口，并补充 Ti5SdkDemo_linux_arm64/README-zh.md 中常用 CAN 指令映射，便于联调抓包时快速对照。详细参数与枚举以 MotorCtrlApi.h 为准。

4.1 日志初始化与输出开关（必填）

接口	功能	备注
`Motor_LogInit(const char* logFile, int async, MotorLogLevelEnum level)`	初始化 SDK 日志系统	建议调用：日志输出依赖初始化。
`Motor_LogSetLevel(MotorLogLevelEnum level)`	设置 SDK 日志级别	用于运行期调整输出等级。
`Motor_LogGetLevel()`	获取当前日志级别	返回当前 `MotorLogLevelEnum`。
`Motor_LogShutdown()`	关闭 SDK 日志系统	建议调用且必须与 `Motor_LogInit` 配对,主程序退出前调用,调用后不应继续使用logger输出：没有 `init + shutdown` 配对时，SDK 可能不会产生任何日志输出。只有init没有shutdown可能导致程序异常

最小示例（对应 @include/api/MotorCtrlApi.h 中的日志接口）：

Motor_LogInit(nullptr, /*async=*/1, MOTOR_LOG_INFO);
Motor_LogSetLevel(MOTOR_LOG_INFO);

// ... 调用 SDK 相关 API ...

Motor_LogShutdown();

如果没有启用日志,那么SDK将不会有任何的输出,建议启用日志,如果不需要输出使用OFF枚举关闭即可 补充说明：

logFile = nullptr 表示仅控制台输出。
async = 1 为异步日志（更符合示例用法）。
日志级别可用 MOTOR_LOG_INFO / MOTOR_LOG_DEBUG / ...。

4.2 关节控制 API

本节按调用阶段梳理 MotorCtrlApi.h 的关节控制接口，主要分为两类：

控制器生命周期与连接：创建控制实例、选择连接方式、必要时查询总线节点信息，最后断开并释放资源。
参数设置与常用码流：设置通信参数、设备阈值、运行模式与各环（电流/速度/位置）相关参数，配合你选择的 motion/运行计划生效。

建议的调用顺序是：Motor_CreateController -> 连接（Motor_ConnectCan 或 Motor_ConnectSocketCan）-> 参数设置 -> 运行/查询 -> Motor_RemoveController（结束时务必调用）。

控制器生命周期与连接

接口	功能	备注
`Motor_CreateController`	创建电机控制实例	调用其他接口前必须先创建
`Motor_ConnectCan`	通过 CAN 分析仪连接	适用于外置 CAN 设备
`Motor_ConnectSocketCan`	通过 SocketCAN 连接	适用于 Linux `can0/can1`
`Motor_RemoveController`	断开并释放实例	结束时务必调用
`Motor_CanGetNodeInfo`	获取总线节点信息	排查总线设备是否在线

参数设置接口（含常用 CAN 码流）

接口	参数类型（示例）	功能	CAN 码流示例
`Motor_SetCommuPara`	`MOTOR_COMMU_PARA_CAN_ID`	修改电机响应 CANID	`0x2E 64 00 00 00`
	`MOTOR_COMMU_PARA_BAUD_RATE`	修改总线波特率	`0x3F 64 00 00 00`
	`MOTOR_COMMU_TIMER`	设置报文上传/掉线保护时间	`0x59 64 00 C8 00`
`Motor_SetDevicePara`	`MOTOR_DEVICE_PARA_MOTOR_MAX_TEMP`	设置线圈最大温度	`0x8D 64 00 00 00`
	`MOTOR_DEVICE_PARA_PCB_MAX_TEMP`	设置驱动板最大温度	`0x91 64 00 00 00`
	`MOTOR_DEVICE_PARA_MAX_VOLTAGE`	设置过压阈值（重启生效）	`0x87 64 00 00 00`
	`MOTOR_DEVICE_PARA_MIN_VOLTAGE`	设置低压阈值（重启生效）	`0x89 64 00 00 00`
	`MOTOR_DEVICE_PARA_NTC_TYPE`	设置 NTC 类型	`0x96 20 64 00 00 00`
`Motor_SetRunMode`	`MOTOR_RUN_MODE_STOP`	停止电机	`0x02`
	`MOTOR_RUN_MODE_CURRENT`	电流模式	`0x1D 64 00 00 00`
	`MOTOR_RUN_MODE_SPEED`	速度模式	`0x1C 64 00 00 00`
	`MOTOR_RUN_MODE_POS`	位置模式	`0x1E 64 00 00 00`
`Motor_SetCurrentPara`	`CURR_PARA_MAX_VAL / MIN_VAL`	电流上下限	`0x20/0x21 64 00 00 00`
	`CURR_PARA_PROPORTIONAL / INTEGRAL`	电流环 KP/KI	`0x83/0x84 64 00 00 00`
`Motor_SetSpeedPara`	`SPEED_PARA_MAX_ACC / MIN_ACC`	加速度上下限	`0x22/0x23 64 00 00 00`
	`SPEED_PARA_MAX_SPEED / MIN_SPEED`	速度上下限	`0x24/0x25 64 00 00 00`
	`SPEED_PARA_PROPORTIONAL / INTEGRAL`	速度环 KP/KI	`0x29/0x2A 64 00 00 00`
`Motor_SetPositionPara`	`POS_PARA_MAX_VALUE / MIN_VALUE`	位置软限位	`0x26/0x27 64 00 00 00`
	`POS_PARA_PROPORTIONAL / DIFFERENTIAL`	位置环 KP/KD	`0x2B/0x2D 64 00 00 00`
	`POS_PARA_OFFSET`	位置偏移	`0x53 64 00 00 00`
`Motor_SetSysCmd`	`MOTOR_SYS_CMD_RESET_FAULT`	清错	`0x0B`
	`MOTOR_SYS_CMD_FACTORY_RESET`	恢复出厂	`0x0F`
	`MOTOR_SYS_CMD_STORE_TO_FLASH`	参数写入 Flash	`0x0E`
	`MOTOR_SYS_CMD_RESTORE_FROM_FLASH`	从 Flash 恢复	`0x0D`

参数读取接口

接口	参数类型（示例）	功能	CAN 码流示例
`Motor_GetDevicePara`	`MOTOR_DEVICE_PARA_GET_FAULT`	获取故障状态	`0x0A`
	`MOTOR_DEVICE_PARA_GET_BUS_VOLTAGE`	获取母线电压	`0x14`
	`MOTOR_DEVICE_PARA_GET_TEMP`	获取线圈温度	`0x31`
	`MOTOR_DEVICE_PARA_GET_PCB_TEMP`	获取驱动板温度	`0x32`
	`MOTOR_DEVICE_PARA_GET_SOFTWARE_VERSION`	获取软件版本	`0x65`
`Motor_GetRunMode`	`-`	获取当前运行模式	`0x03`
`Motor_GetCurrentPara`	`CURR_PARA_GET_CUR_VAL`	获取当前电流	`0x04`
	`CURR_PARA_GET_TARGET_VAL`	获取目标电流	`0x05`
	`CURR_PARA_GET_CSP`	获取电流/速度/位置	`0x41`
`Motor_GetSpeedPara`	`SPEED_PARA_GET_CUR_VAL`	获取当前速度	`0x06`
	`SPEED_PARA_GET_TARGET_VAL`	获取目标速度	`0x07`
`Motor_GetPositionPara`	`POS_PARA_GET_CUR_VAL`	获取当前位置	`0x08`
	`POS_PARA_GET_TARGET_VAL`	获取目标位置	`0x09`

扩展控制与调试接口

接口	功能	使用场景
`Motor_Raw_CanWrite`	发送原始 CAN 数据	协议联调、透传验证
`Motor_SetDecodeFailedStrategy`	设置解码失败策略	丢弃/回调/裸上传
`Motor_GetDecodeFailedStrategy`	获取当前策略	排查异常帧处理逻辑

4.3 运动模式与运动控制 API

`Motor_SetRunMode`

参数	含义
`ctrlId`	控制实例 ID（`Motor_CreateController` 返回值）
`mode`	运行模式，见下表 `MotorRunModeEnum`
`value`	与 `mode` 相关的目标或命令字；头文件约定：电流模式为电流（mA）；速度模式为内圈速度（0.01 Hz）；位置模式为位置（cnt）；`MOTOR_RUN_MODE_STOP` 时按实现可不使用或填 0

MotorRunModeEnum（节选，完整见头文件）：

枚举值	说明
`MOTOR_RUN_MODE_STOP`	停止
`MOTOR_RUN_MODE_CURRENT`	电流模式
`MOTOR_RUN_MODE_POS`	位置模式
`MOTOR_RUN_MODE_CYCLIC_SYN_POS` / `CYCLIC_SYN_SPEED` / `CYCLIC_SYN_TORQUE`	周期同步位置/速度/转矩

`Motor_GetRunMode`

参数	含义
`ctrlId`	控制实例 ID

发起查询；成功后通过 Motor_ReadRspMsg 读取 MotorGetParaRsp，其中 paraType = MOTOR_RUN_MODE_PARA。

`Motor_SendCSP`

参数	含义
`ctrlId`	控制实例 ID
`type`	`CSP_TYPE_POSITION` / `CSP_TYPE_CURRENT` / `CSP_TYPE_SPEED`
`value`	目标：位置（cnt）/ 电流（mA）/ 速度（0.01 Hz）

周期性调用以设置目标并获取电流/速度/位置等反馈（与 CSP 运行方式一致）。

`Motor_AngleToPositionCnt`

参数	含义
`encoderNum`	`MOTOR_ENCODER_NUM_SINGLE`（单编）或 `MOTOR_ENCODER_NUM_DUAL`（双编）
`gearRatio`	减速比；双编时换算公式中不使用 `gearRatio`（见头文件注释）
`angleUnit`	`MOTOR_ANGLE_UNIT_DEG`（度）或 `MOTOR_ANGLE_UNIT_RAD`（弧度）
`angle`	外圈角度
`posCnt`	输出：电机端位置计数（`int *`）

换算关系（与 MotorCtrlApi.h 注释一致）：单编时 posCnt 与 angle * gearRatio * 65536 及 360° 或 2π 相关；双编时与 262144 及 360° 或 2π 相关。

`Motor_PositionCntToAngle`

参数	含义
`encoderNum`	单编 / 双编，同上
`gearRatio`	减速比；双编公式中不乘 `gearRatio`
`posCnt`	电机端位置计数
`angleUnit`	输出角度单位：度或弧度
`angle`	输出：外圈角度（`double *`）

`Motor_MoveTargetAngle`

参数	含义
`ctrlId`	控制实例 ID
`encoderNum`	单编 / 双编
`gearRatio`	减速比
`angleUnit`	`targetAngle` 的单位（度或弧度）
`targetAngle`	目标外圈角度

`Motor_MoveDeltaAngle`

参数	含义
`ctrlId`	控制实例 ID
`encoderNum`	单编 / 双编
`gearRatio`	减速比
`angleUnit`	`curAngle`、`deltaAngle` 的单位（度或弧度）
`curAngle`	当前外圈角度
`deltaAngle`	相对当前角度的增量

`Motor_CntSpeedToOutputSpeed`

参数	含义
`cntSpeed`	电机端转速，单位 0.01 Hz（与 `Motor_SetRunMode` 速度模式一致）
`gearRatio`	减速比
`speedUnit`	输出物理量单位：`MOTOR_SPEED_UNIT_RPM`、`MOTOR_SPEED_UNIT_DEG_PER_SEC`、`MOTOR_SPEED_UNIT_RAD_PER_SEC`
`outputSpeed`	输出端转速（`double *`）

头文件给定换算：例如输出为 RPM 时 outputRpm = cntSpeed * 0.6 / gearRatio；度/s、弧度/s 见 MotorCtrlApi.h 注释。

`Motor_OutputSpeedToCntSpeed`

参数	含义
`outputSpeed`	输出端转速（数值，单位由 `speedUnit` 指定）
`gearRatio`	减速比
`speedUnit`	与 `Motor_CntSpeedToOutputSpeed` 相同三选一
`cntSpeed`	输出：电机端转速（0.01 Hz，`int *`）

示例（目标角）

// 与 MotorCtrlApi.h 声明一致；ctrlId / gearRatio / encoderNum 按实机填写
int ctrlId = 0;
int gearRatio = 101;
int posCnt = 0;

Motor_AngleToPositionCnt(MOTOR_ENCODER_NUM_DUAL, gearRatio,
                         MOTOR_ANGLE_UNIT_DEG, 15.0, &posCnt);

Motor_MoveTargetAngle(ctrlId, MOTOR_ENCODER_NUM_DUAL, gearRatio,
                      MOTOR_ANGLE_UNIT_DEG, 15.0);

// 当前位置 cnt 需先 Motor_GetPositionLoopPara 发起查询，再通过 Motor_ReadRspMsg 取回后再调用 PositionCntToAngle

4.4 PT 模式控制

PT 实时控制使用 Motor_ConfigPTMode（初始化一次）+ Motor_SetPT（周期调用）。

`Motor_ConfigPTMode`

在运行前配置静态 PT 参数（通常初始化时调用一次）；之后可用 Motor_SetPT 做周期控制。

参数	含义
`ctrlId`	控制实例 ID
`maxCurrent` / `minCurrent`	电流上下限（A）
`maxTorque` / `minTorque`	扭矩上下限（Nm）
`defRatio`	减速比
`defKT`	扭矩常数（Nm/A）
`maxKP` / `maxKD`	KP/KD 最大值（量化用）

`Motor_SetPT`

周期调用；必须先 Motor_ConfigPTMode。

参数	含义
`ctrlId`	控制实例 ID
`kp`	位置刚度系数
`kd`	阻尼系数
`targetPos`	目标位置（rad）
`targetSpeed`	目标速度（rad/s）
`targetTorque`	前馈扭矩（Nm）

`Motor_GetForceKPDPara` / `Motor_SetForceKPDPara`

接口	参数	含义
`Motor_GetForceKPDPara`	`ctrlId`	控制实例 ID
	`paraType`	`ForceKPDTypeEnum`，见下表
`Motor_SetForceKPDPara`	`ctrlId`	控制实例 ID
	`paraType`	`FORCE_KP_MAX` / `FORCE_KD_MAX` 等（设置项）
	`value`	参数值

ForceKPDTypeEnum：

枚举	说明
`FORCE_KP`	KP
`FORCE_KD`	KD
`FORCE_KT`	KT
`FORCE_KI`	KI
`FORCE_KILIMIT`	KI 限幅
`FORCE_KP_MAX`	KP 最大值（设置接口常用）
`FORCE_KD_MAX`	KD 最大值（设置接口常用）

读取结果通过 Motor_ReadRspMsg 获取。

`Motor_GetPTTIPara` / `Motor_SetPTTIPara`

接口	参数	含义
`Motor_GetPTTIPara`	`ctrlId`	控制实例 ID
	`paraType`	`PT_TITypeEnum`
`Motor_SetPTTIPara`	`ctrlId`	控制实例 ID
	`paraType`	`PT_TMAX` / `PT_TMIN` / `PT_IMAX` / `PT_IMIN`
	`value`	参数值

PT_TITypeEnum：

枚举	说明
`PT_TMAX`	扭矩最大值
`PT_TMIN`	扭矩最小值
`PT_IMAX`	电流最大值
`PT_IMIN`	电流最小值

读取结果通过 Motor_ReadRspMsg 获取。

5. 外设控制 API

5.1 灵巧手

主要封装类：hand_sdk::HandController

接口说明

connectRs485(port, baud)
setAllFingerAnglesDeg(angles, speed)
start()
stop()
disconnect()
getFingerAnglesDeg(out_angles_deg)

示例

编译：

cd PeripheralSDK/hand_sdk
mkdir -p build && cd build
cmake ..
make -j

运行动作示例（比耶）：

cd PeripheralSDK/hand_sdk/build
./hand_sdk_demo /dev/ttyACM0 1

查询当前位置：

cd PeripheralSDK/hand_sdk/build
./hand_sdk_demo /dev/ttyACM0 4

action_id 建议说明：

0：STRAIGHT
1：VICTORY
2：OK
3：HEART
4：QUERY_POS（仅查询）

5.2 六维力

六维力建议直接基于 PeripheralSDK/sri_can_sdk 使用。该 SDK 为纯 C++（非 ROS2 依赖），基于 Linux SocketCAN。

接口说明

set_can(iface, bitrate)：配置 CAN 接口与波特率（如 can0, 1000000）
set_channel(cmd_id, data_ids)：配置命令 ID 和 3 个数据帧 ID
connect() / disconnect()：连接/断开 CAN
send(cmd)：发送命令（0x01 单次、0x02 连续、0x00 停止）
read(timeout)：单次采样（内部发送 0x01 并接收解析 3 帧）
recv(timeout)：连续模式下接收并解析 3 帧
connected()：连接状态
data()：读取最新六维力数据

示例

sri_can_sdk::SriSensor s("id");
s.set_can("can0", 1000000);
s.set_channel(0x80, {0x291, 0x292, 0x293});

if (s.connect()) {
  if (s.read(1.0)) {
    auto& d = s.data();
    printf("FX=%.2f FY=%.2f FZ=%.2f\n", d.fx, d.fy, d.fz);
  }

  s.send(sri_can_sdk::SriProtocol::CMD_START_CONTINUOUS);
  while (s.recv(0.1)) {
    auto& d = s.data();
    // 使用 d.fx d.fy d.fz d.mx d.my d.mz
  }
  s.send(sri_can_sdk::SriProtocol::CMD_STOP_CONTINUOUS);
  s.disconnect();
}

编译与可执行文件测试：

sri_can_demo 的参数是 CAN 接口名，例如 can4，不是 /sys/class/net/can4 这类路径。

CAN 接口配置（以 can4、1Mbps 为例）：

sudo ip link set can4 down
sudo ip link set can4 type can bitrate 1000000
sudo ip link set can4 up
ip -details link show can4

运行示例：

cd PeripheralSDK/sri_can_sdk/build
./sri_can_demo can4

菜单测试顺序：

输入 1：连接
输入 3：单次采样，检查 FX/FY/FZ/MX/MY/MZ
输入 4：流式采集，输入采集帧数（如 200）
输入 2：断开
输入 0：退出

联调排查：

若提示“连接失败”，先检查 can4 是否已 up
若提示“单次采样失败”，检查波特率、命令 ID、数据 ID 是否和设备一致
可使用 candump can4 抓包确认是否有数据帧

5.3 底盘

主要封装类：ti5_chassis_sdk::ChassisController

接口说明

connect(host, port)
sendVelocity(vx, vy, wz)
sendVelocityFor(vx, vy, wz, duration_ms, period_ms)
moveForward(vx) / moveLateral(vy) / rotate(wz)
stop()
isConnected()
host() / port()
getLastVelocity(vx, vy, wz)

示例

编译：

cd PeripheralSDK/chassis_sdk
mkdir -p build && cd build
cmake ..
make -j

前进 2 秒：

cd PeripheralSDK/chassis_sdk/build
./chassis_sdk_demo <host> <port> <cmd> [value] [duration_ms]
./chassis_sdk_demo 169.254.128.2 5480 forward 0.10 2000

原地旋转 1.5 秒：

cd PeripheralSDK/chassis_sdk/build
./chassis_sdk_demo 169.254.128.2 5480 rotate 0.20 1500

查询连接状态与最近一次速度命令：

cd PeripheralSDK/chassis_sdk/build
./chassis_sdk_demo 169.254.128.2 5480 query

6. 故障排除

6.1 编译失败

检查编译器版本与 C++ 标准是否满足要求
检查三方库是否安装（libsocketcan-dev、libengin3-dev）
清理后重编译：删除 build 目录后执行 cmake -B build -S . && cmake --build build -j "$(nproc)"

6.2 无法连接 CAN

检查 CAN 设备供电、线序与终端电阻
核对 deviceIndex、canIndex、baudRate 是否与硬件一致
检查驱动是否正确加载（PEAK/SocketCAN）

6.3 电机无响应或动作异常

先确认 appId 与目标电机 ID 一致
先执行清错与状态读取，确认无故障码再发控制指令
降低目标速度/电流/力矩，避免触发保护

6.4 整机运动异常

确认所选型号（T140A / T170A / T170C / T170D）与现场机器人一致
双臂末端解算失败时，检查 runtime/ 中运动学库是否与当前机型匹配
双臂普通运动出现剧烈抖动时，勿使用 motor_menu；应通过 robot_model_menu 菜单 1 测试
多关节/整机联调勿使用 motor_menu（无关节限位），应使用 robot_model_menu

6.5 外设通信异常

灵巧手：确认串口路径、权限与波特率
底盘：确认 IP、端口与网络可达
六维力：确认协议帧格式、数据频率与坐标系定义

6.6 测试安全建议

程序选择：单轴/CAN 联调用 motor_menu；整机、双臂、头/腰/腿用 robot_model_menu。切勿用 motor_menu 驱动多关节或整机。
首次联调必须空载或低风险姿态
所有动作测试从低速低增益开始
出现异常立即执行 stop 或断开使能，优先保证人机安全

TI5 SDK核心开发指南

1. 概述

1.1 支持的平台

2. 快速开始

2.1 Ti5_SDK 安装与部署

硬件准备

软件依赖

CAN 驱动安装说明（Linux）

禁用与 PEAK 冲突的内核模块

编译并安装 PEAK 驱动

验证

故障排除与卸载

2.2 编译与运行

项目结构

构建方式

运行方式

1. 可执行文件与脚本说明

1.1 单电机测试（motor_menu）

1. 机器人测试（robot_model_menu）

2. 环境初始化脚本

1. 底盘（chassis）安装与部署

1.1 依赖环境

1.2 通用编译步骤

1.3 运行方式

2. 灵巧手（hand）安装与部署

2.1 串口与权限准备

2.2 通用编译步骤

2.3 运行方式

3. 六维力传感器（sri_can）安装与部署

3.1 CAN 前置条件

3.2 通用编译步骤

3.3 运行方式（示例）

4. 关节控制 API

4.1 日志初始化与输出开关（必填）

4.2 关节控制 API

控制器生命周期与连接

参数设置接口（含常用 CAN 码流）

参数读取接口

扩展控制与调试接口

4.3 运动模式与运动控制 API

Motor_SetRunMode

Motor_GetRunMode

Motor_SendCSP

Motor_AngleToPositionCnt

Motor_PositionCntToAngle

Motor_MoveTargetAngle

Motor_MoveDeltaAngle

Motor_CntSpeedToOutputSpeed

Motor_OutputSpeedToCntSpeed

示例（目标角）

4.4 PT 模式控制

Motor_ConfigPTMode

Motor_SetPT

Motor_GetForceKPDPara / Motor_SetForceKPDPara

Motor_GetPTTIPara / Motor_SetPTTIPara

5. 外设控制 API

5.1 灵巧手

接口说明

示例

5.2 六维力

接口说明

示例

5.3 底盘

接口说明

示例

6. 故障排除

6.1 编译失败

6.2 无法连接 CAN

6.3 电机无响应或动作异常

6.4 整机运动异常

6.5 外设通信异常

6.6 测试安全建议

`Motor_SetRunMode`

`Motor_GetRunMode`

`Motor_SendCSP`

`Motor_AngleToPositionCnt`

`Motor_PositionCntToAngle`

`Motor_MoveTargetAngle`

`Motor_MoveDeltaAngle`

`Motor_CntSpeedToOutputSpeed`

`Motor_OutputSpeedToCntSpeed`

`Motor_ConfigPTMode`

`Motor_SetPT`

`Motor_GetForceKPDPara` / `Motor_SetForceKPDPara`

`Motor_GetPTTIPara` / `Motor_SetPTTIPara`