API 控制
CAN配置
硬件配置
终端电阻必须使用 Split termination。禁止使用星形连接接法。必须保证同根CAN线上有且仅有两组终端电阻，并且都在线的两个末端上。
如下图:
软件配置
电机使用 CAN-FD。仲裁段固定为 1Mbps，采样点为0.8。 数据段波特率固定为 5Mbps，采样点为 0.75。
推荐的 CAN 配置命令：
ip link set can0 type can bitrate 1000000 sample-point 0.8 dbitrate 5000000 dsample-point 0.75 sjw 5 dsjw 3 fd on
CAN 配置成功后激活 CAN 线：
注意
必须正确设置采样点，否则发送长帧时必定导致通信失败。有些USB转CAN设备不一定正确支持设置这些参数。如果遇到这种情况，请转而使用 RK3576 等硬件 CAN-FD 支持的 SOC。
推荐的初始化与控制流程
总之，一个典型的完整电机初始化与控制流程是这样的：
- NMT → Pre-Operational (允许 SDO 配置)
- 读取 0x1018:01 — 验证工厂 UID (0x4859444C)
- 读取 0x1018:03 — 验证固件版本 (v8以上)
- 读取 0x1018:04 — 记录序列号
- 读取 0x6076:00 — 读取峰值扭矩 (type: REAL32, Nm)
- 写入 0x2040:00 — 失能时短接制动 (按需配置)
- 写入 0x6072:00 — 最大扭矩限幅
- 写入 0x1016:01 — 禁用消费者心跳监控
- 写入 0x6040:00 — CiA402 状态机复位 (0 → 0x80)
- 写入 0x6060:00 — 设置工作模式 (常用: MIT=5, Velocity=3; 更多模式请看对象字典手册里的
0x6060对象) - [ if (压缩 MIT 模式) ] 配置 0x2004 子索引 (范围 + KP/KD 限幅)
- 配置 TPDO 映射 ( 按需配置 )
- 配置 RPDO 映射 ( 按照您需要的控制模式影射为对应的对象字典 )
- CiA402 状态机: Shutdown(6) → SwitchOn(7) → Enable(0xF)
- NMT → Operational (PDO 开始传输)
-
写入 0x1016:01 — 启用消费者心跳监控 (确保上位机程序结束时电机会自动退出使能)
-
上位机开始发送自己的心跳包
- 开始通过PDO发送控制指令。（MIT模式或速度模式的控制数据）
具体实现的代码可参考开源仓库 hex-motor-control-test
解析CAN消息
这里提供解析部份（也就是第6步）的部分代码示例 默认的TPDO映射如下：
// TPDO1: 位置(0x6064, 0, i32(长度0x20)，时间戳(0x1013 0, u32(长度0x20))，力矩(0x6077, 0, i16(长度0x10))，错误码(0x603F, 0, u16(长度0x10))。12字节，1000Hz。
// TPDO2: 状态字(2字节)，驱动器温度(2字节)，电机温度(2字节)，控制字(2字节)，错误码(2字节)。10字节(实际发送12字节)，50Hz。
有几个小细节
- 速度使用dequeue通过硬件时间戳做差在上位机进行计算，消除潜在的时间误差，并且方便用户自行选择滤波算法。
- 由于 6077h 没有量纲，是0-1000，以千分比形式提供。因此需要乘以 6076h 得到国际单位后的力矩值。您应该在电机开始时读取 一次 6076h(对同一电机为常量)。后续用它乘即可。
for frame in frames {
let id = frame.id();
if let socketcan::Id::Standard(id) = id {
let id = id.as_raw();
let canopen_id = (id as u8) & 0x7F;
if canopen_id != self.canopen_id {
continue;
}
if let Ok(communication_object_code) =
canopen::canopen_types::CommunicationObject::try_from(id)
{
let constants = self.runtime_constants.get();
match communication_object_code {
canopen::canopen_types::CommunicationObject::TPDO1 => {
if frame.data().len() != 12 {
continue;
}
// TPDO1: 位置(4字节 F32 单圈 rev [-0.5, 0.5))，时间戳(4字节)，力矩(2字节)，错误码(2字节)。12字节，1000Hz。
let position_f32 = f32::from_le_bytes([
frame.data()[0],
frame.data()[1],
frame.data()[2],
frame.data()[3],
]);
// info!("position_f32: {:.4}", position_f32);
let timestamp = u32::from_le_bytes([
frame.data()[4],
frame.data()[5],
frame.data()[6],
frame.data()[7],
]);
let speed = {
let dq = &mut self.runtime_data.lock().unwrap().raw_position;
let prev = dq.back().map(|(p, _)| *p).unwrap_or_default();
let new_pos = prev.update(position_f32);
// info!("new_pos: {:?}, prev: {:?}", new_pos, prev);
dq.push_back((new_pos, timestamp));
if dq.len() > self.speed_average_window {
let last_data = dq.pop_front().unwrap();
let time_diff_us = timestamp.overflowing_sub(last_data.1).0;
let time_diff = time_diff_us as f64 / 1_000_000.0f64;
let position_diff_rev = new_pos.diff(last_data.0);
position_diff_rev as f64 * std::f64::consts::TAU / time_diff
} else {
0.0f64
}
};
let torque = if let Some(peak_torque) = self.runtime_constants.get() {
let peak_torque = peak_torque.peak_torque;
let torque = i16::from_le_bytes([frame.data()[8], frame.data()[9]]);
// Convert mapped torque to Nm
torque as f64 * peak_torque as f64 / 1000.0f64
} else {
0.0f64
};
let error_code =
u16::from_le_bytes([frame.data()[10], frame.data()[11]]);
{
let runtime_data = &mut self.runtime_data.lock().unwrap();
// Raw position is already updated, so no need to update it here
runtime_data.last_contact_time = std::time::Instant::now();
runtime_data.smoothed_speed = speed;
runtime_data.torque = torque;
runtime_data.raw_error_code = error_code;
}
}
canopen::canopen_types::CommunicationObject::TPDO2 => {
if frame.data().len() != 12 {
continue;
}
// TPDO2: 状态字(2字节)，驱动器温度(2)，电机温度(2)，控制字(2)。8字节，50Hz。
let status_word =
u16::from_le_bytes([frame.data()[0], frame.data()[1]]);
let driver_temperature =
i16::from_le_bytes([frame.data()[2], frame.data()[3]]) as f32
* 0.1f32;
let motor_temperature =
i16::from_le_bytes([frame.data()[4], frame.data()[5]]) as f32
* 0.1f32;
let control_word =
u16::from_le_bytes([frame.data()[6], frame.data()[7]]);
{
let runtime_data = &mut self.runtime_data.lock().unwrap();
runtime_data.status_word = status_word;
runtime_data.driver_temperature = Some(driver_temperature);
runtime_data.motor_temperature = Some(motor_temperature);
runtime_data.read_control_word = control_word;
}
}
canopen::canopen_types::CommunicationObject::Heartbeat => {
if frame.data().len() != 1 {
continue;
}
let data = frame.data()[0];
if let Ok(nmt_state) = NMTState::try_from(data) {
if nmt_state != NMTState::Operational {
{
let mut runtime_data = self.runtime_data.lock().unwrap();
if runtime_data.last_nmt_send_time.elapsed().as_millis()
> 250
{
runtime_data.last_nmt_send_time =
std::time::Instant::now();
}
}
ret.push(canopen::set_nmt_state(
NMTState::Operational,
self.canopen_id,
));
}
self.runtime_data.lock().unwrap().nmt_state = Some(nmt_state);
}
}
_ => {}
}
}
}
}
使用MIT模式时推荐使用压缩型MIT控制参数,详见版本08对象字典2004h，将MIT控制目标值转换为MIT控制字的rust代码示例如下：
impl Default for MitTargetMapping {
fn default() -> Self {
Self {
position_min: -0.5,
position_max: 0.5,
velocity_min: -10.0,
velocity_max: 10.0,
torque_min: -10.0,
torque_max: 10.0,
kp_min: 0.0,
kp_max: 100.0,
kd_min: 0.0,
kd_max: 20.0,
}
}
}
fn float_to_uint(x: f32, x_min: f32, x_max: f32, bits: u32) -> u32 {
let span = x_max - x_min;
let offset = x_min;
let scale = ((1 << bits) - 1) as f32;
((x - offset) * scale / span) as u32
}
pub fn to_le_bytes(self, mapping: &MitTargetMapping) -> [u8; 8] {
// Clamp values to mapping range so overflowing values are wrapped to min/max
let position = self
.position
.clamp(mapping.position_min, mapping.position_max);
let velocity = self
.velocity
.clamp(mapping.velocity_min, mapping.velocity_max);
let torque = self.torque.clamp(mapping.torque_min, mapping.torque_max);
let kp = self.kp.clamp(mapping.kp_min, mapping.kp_max);
let kd = self.kd.clamp(mapping.kd_min, mapping.kd_max);
let pos = Self::float_to_uint(position, mapping.position_min, mapping.position_max, 16);
let vel = Self::float_to_uint(velocity, mapping.velocity_min, mapping.velocity_max, 12);
let torque_u = Self::float_to_uint(torque, mapping.torque_min, mapping.torque_max, 12);
let kp_u = Self::float_to_uint(kp, mapping.kp_min, mapping.kp_max, 12);
let kd_u = Self::float_to_uint(kd, mapping.kd_min, mapping.kd_max, 12);
let lower_u32 = torque_u | (kd_u << 12) | ((kp_u & 0xFF) << 24);
let upper_u32 = (kp_u >> 8) | (vel << 4) | (pos << 16);
let lower = lower_u32.to_le_bytes();
let upper = upper_u32.to_le_bytes();
[
lower[0], lower[1], lower[2], lower[3], upper[0], upper[1], upper[2], upper[3],
]
}
等价的 C 语言示例：
#include <stdint.h>
typedef struct {
float position;
float velocity;
float torque;
float kp;
float kd;
} MitTarget;
typedef struct {
float position_min;
float position_max;
float velocity_min;
float velocity_max;
float torque_min;
float torque_max;
float kp_min;
float kp_max;
float kd_min;
float kd_max;
} MitTargetMapping;
static MitTargetMapping mit_target_mapping_default(void) {
MitTargetMapping m = {
.position_min = -0.5f,
.position_max = 0.5f,
.velocity_min = -10.0f,
.velocity_max = 10.0f,
.torque_min = -10.0f,
.torque_max = 10.0f,
.kp_min = 0.0f,
.kp_max = 100.0f,
.kd_min = 0.0f,
.kd_max = 20.0f,
};
return m;
}
static float clampf(float x, float lo, float hi) {
if (x < lo) {
return lo;
}
if (x > hi) {
return hi;
}
return x;
}
static uint32_t float_to_uint(float x, float x_min, float x_max, uint32_t bits) {
float span = x_max - x_min;
float offset = x_min;
float scale = (float)((1u << bits) - 1u);
return (uint32_t)(((x - offset) * scale) / span);
}
static void store_u32_le(uint8_t dst[4], uint32_t v) {
dst[0] = (uint8_t)(v & 0xFFu);
dst[1] = (uint8_t)((v >> 8) & 0xFFu);
dst[2] = (uint8_t)((v >> 16) & 0xFFu);
dst[3] = (uint8_t)((v >> 24) & 0xFFu);
}
/* 将 MIT 目标压成 8 字节（先 lower_u32，后 upper_u32，均为小端） */
void mit_target_to_le_bytes(const MitTarget *self, const MitTargetMapping *mapping,
uint8_t out[8]) {
float position = clampf(self->position, mapping->position_min, mapping->position_max);
float velocity = clampf(self->velocity, mapping->velocity_min, mapping->velocity_max);
float torque = clampf(self->torque, mapping->torque_min, mapping->torque_max);
float kp = clampf(self->kp, mapping->kp_min, mapping->kp_max);
float kd = clampf(self->kd, mapping->kd_min, mapping->kd_max);
uint32_t pos = float_to_uint(position, mapping->position_min, mapping->position_max, 16);
uint32_t vel = float_to_uint(velocity, mapping->velocity_min, mapping->velocity_max, 12);
uint32_t torque_u = float_to_uint(torque, mapping->torque_min, mapping->torque_max, 12);
uint32_t kp_u = float_to_uint(kp, mapping->kp_min, mapping->kp_max, 12);
uint32_t kd_u = float_to_uint(kd, mapping->kd_min, mapping->kd_max, 12);
uint32_t lower_u32 = torque_u | (kd_u << 12) | ((kp_u & 0xFFu) << 24);
uint32_t upper_u32 = (kp_u >> 8) | (vel << 4) | (pos << 16);
store_u32_le(out, lower_u32);
store_u32_le(out + 4, upper_u32);
}
电机控制
控制指令超时
CANOpen自带超时机制。默认是关闭的，您需要手动启用。具体请查看 1016h 对象的介绍。
电机失能刹车
通过调整 2040h 00h 对象，可以设置电机失能或发生故障后是否短接绕组以维持制动状态。
一拖多控制
在CANOpen中，PDO并非只能使用指定的COB-ID，而是可以自由设置。这意味着您可以通过设置多个机器的RPDO的COB-ID为同一个来实现一拖多的功能。但是，原有的RPDO COB-ID是无法满足需求的。因此不仅需要调整所有被控电机的 Mapping，还需要调整 RPDO COB-ID。
听起来有点抽象？
以带力矩上限的速度控制为例，每个电机都需要使用 6072h(最大力矩/Max Torque) 与 60FFh(目标速度/Target Velocity) 这两个对象。因此每个电机都需要六个字节。一共有4个电机，因此需要24字节。
四个电机的CANOpenID分别为0x01, 0x02, 0x03, 0x04。每个电机都控制他们的 6072h(最大力矩/Max Torque) 与 60FFh(目标速度/Target Velocity)。这里我们需要选一个 COB-ID 做所有电机的 RPDO1 COB-ID。
这里我们选择 0x10 节点的 TPDO1 COB-ID（即0x190）。您可以任意选择一个CAN-ID，只要确保不会与其他节点冲突即可。这里推荐选主站的TPDO1，2，3或4。
对于发送的程序来讲，全部24字节的顺序是这样的：
4字节填充对象为 3000h 03h, 2字节填充对象为 3000h 02h
电机1最大力矩(2 Bytes) | 电机1目标速度(4 Bytes) | 电机2最大力矩(2 Bytes) | 电机2目标速度(4 Bytes) | 电机3最大力矩(2 Bytes) | 电机3目标速度(4 Bytes) | 电机4最大力矩(2 Bytes) | 电机4目标速度(4 Bytes)
那么每个电机有两个有效对象。其他字节使用填充即可。我们假定将6072h排在前面，那么对于一号电机，映射应该是这样的：
最大力矩 6072h | 目标速度 60FFh | 4字节填充 | 4字节填充 | 4字节填充 | 4字节填充 | 2字节填充
那么对2号电机来讲，他的RPDO Mapping应该是这样的：
4字节填充 | 2字节填充 | 最大力矩 6072h | 目标速度 60FFh | 4字节填充 | 4字节填充 | 4字节填充
那么对3号电机来讲，他的RPDO Mapping应该是这样的：
4字节填充 | 4字节填充 | 4字节填充 | 最大力矩 6072h | 目标速度 60FFh | 4字节填充 | 2字节填充
那么对4号电机来讲，他的RPDO Mapping应该是这样的：
4字节填充 | 4字节填充 | 4字节填充 | 4字节填充 | 2字节填充 | 最大力矩 6072h | 目标速度 60FFh
保存PDO Mapping
PDO Mapping 映射是可以保存的，避免每次开机都重新设置。具体请查看对象 1010h。保存后每次开机仅需发送NMT指令即可直接开始控制。
关于填充对象
无需拘泥于此形式，得益于PDO的灵活特性，您可以自由安排。假设实际上目标速度需要更高的变动频率，而最大力矩不需要，那么完全可以将所有的最大力矩放到RPDO2，以高频发送目标速度，低频发送最大力矩。
也无需强制所有的电机都处于同一控制模式，完全可以一部分电机处于速度控制模式，一部分电机使用MIT控制模式。只要 Mapping 正确即可。
关于配置自动化与配置持久化
请记住，本质上 canopend 就是发送了一些 can 消息，您完全可以将配置PDO的过程自动化。关于 SDO 协议的具体格式请直接查看丰富的互联网教程。
另外，这些配置除非通过 1010h 对象进行保存，否则每次开机后都会恢复为默认配置。如果您确认不会改变这些配置，那么只要通过 1010h 对象进行保存，即可实现配置持久化。后续开机无需重新配置，仅需发送NMT指令与选择模式，操作控制字。
具体实现的代码可参考快速上手章节的完整控制演示部分: 具体实现代码
MIT控制与速度控制
对于 动力角轮(Powered Castor Wheel) 来讲，可被人推动基本是必须的。很多情况下都会希望在底盘能在特定情况能被自由推动。此时仅需要将电机的最大输出力矩限制为0即可。到这里都只需要使用速度控制。但如果您希望增加一些力控，例如 动/静摩擦力补偿 等，那么只能使用 MIT 控制模式。
另外还值得一提的是，没有所有电机必须要使用同种模式的限制，因此您可以自由组合速度控制与MIT控制，只要确保您的PDO映射正常即可。
速度模式：
- 数据简短，有效降低can总线占用率。
- 结合 6072h 使用，可以实现力位混合控制，令底盘可以被推动。力位混控情况下单电机需要6字节，64字节可以轻松带动 4个舵轮模组 (8个电机)
- 无法实现力控
MIT控制模式：
- 数据较长，会显著增加can总线占用率。如果can总线信号质量太差，会导致通信失败。
- 无需配合6072h使用，直接Kd给0即可。
- 可以实现力控，例如 动/静摩擦力补偿 等。
- 视情况长度可为 16 字节 或 10 字节。无论何种情况，都需要2个COB-ID带动全部4个舵轮模组。尽管如此，使用10字节长度可以有效降低can总线占用率，因此仍然推荐。
关于MIT控制
单个MIT对象有16字节长，其中4字节的目标位置与2字节的Kp，如果没有使用，建议您直接从映射中去除。这样可以进一步减少CAN总线占用率。
压缩MIT模式：
如果您想降低 CAN 总线带宽占用，可以考虑使用压缩MIT进行控制。
压缩 MIT 控制字总共 64 位，由低 32 位部分和高 32 位部分组成。
在使用时两部分需要映射到同一 帧 PDO 以确保一次性写入或读取，且映射顺序 要低 32 位在前，高 32 位在后。压缩 MIT 控制字的位域定义如下：
注意：以下内容仅针对 hex-43xx 系列电机，不泛指其他的产品的压缩MIT规则，使用其他产品时，请具体分析。
压缩后的位域定义
| Bit范围 | 名称 | 位宽 | 数据范围 |
|---|---|---|---|
| [63:48] | 目标位置 Pdes | 16 bit | 0x0000 ~ 0xFFFF |
| [47:36] | 目标速度 Vdes | 12 bit | 0x000 ~ 0xFFF |
| [35:24] | 比例增益 Kp | 12 bit | 0x000 ~ 0xFFF |
| [23:12] | 微分增益 Kd | 12 bit | 0x000 ~ 0xFFF |
| [11:0] | 前馈力矩 Tff | 12 bit | 0x000 ~ 0xFFF |
本电机的压缩 MIT 协议只规定：
- Position 使用 16bit
- Velocity 使用 12bit
- Kp 使用 12bit
- Kd 使用 12bit
- Torque 使用 12bit
但并不规定：实际的位置范围是多少，实际的速度范围是多少，实际的力矩范围是多少……
因此：(0x0000 - 0xFFFF) 不代表具体的某个物理量范围
真正对应的物理量范围由对应对象字典的相关配置决定 ( 具体且详细的内容请看对象字典手册中的0x2004对象字典中的介绍 )。
例如，假设：
向0x2004h的0x04中写入-5.0，即Position Min = -5.0 Rev ( 位置下限为 -5.0 Rev)
向0x2004h的0x05中写入 5.0，即Position Max = +5.0 Rev( 位置上限为 -5.0 Rev)
则：
| 编码值 | 实际位置 |
|---|---|
| 0x0000 | -5 Rev |
| 0x8000 | 0 Rev |
| 0xFFFF | +5 Rev |
MIT中其他的对象也是以此类推。
hex-43xx电机中将会默认配置MIT各个对象的默认值。具体且详细的内容请看对象字典手册中的0x2004对象字典中的介绍。
本文在此只是简单介绍一下本产品的压缩MIT，希望能够帮助到您，如果您想要更详细地了解MIT控制，请看网上更详细的资料。
压缩MIT数据包的示例C代码
#include <stdint.h>
typedef struct {
float position_min, position_max; /* Rev */
float velocity_min, velocity_max; /* Rev/s */
float torque_min, torque_max; /* Nm */
float kp_min, kp_max; /* Nm/Rev */
float kd_min, kd_max; /* Nm*s/Rev */
} hexfellow_mit_mapping_t;
typedef struct {
float position; /* Rev */
float velocity; /* Rev/s */
float torque; /* Nm (feed-forward) */
float kp; /* Nm/Rev */
float kd; /* Nm*s/Rev */
} hexfellow_mit_target_t;
static inline uint32_t float_to_uint(float x, float xmin, float xmax,
uint32_t bits) {
float span = xmax - xmin;
float scale = (float)((1u << bits) - 1u);
return (uint32_t)(((x - xmin) * scale) / span);
}
static inline void store_u32_le(uint8_t dst[4], uint32_t v) {
dst[0] = (uint8_t)(v & 0xFFu);
dst[1] = (uint8_t)((v >> 8) & 0xFFu);
dst[2] = (uint8_t)((v >> 16) & 0xFFu);
dst[3] = (uint8_t)((v >> 24) & 0xFFu);
}
void hexfellow_mit_target_pack(const hexfellow_mit_target_t *t,
const hexfellow_mit_mapping_t *m,
uint8_t out[8]) {
float position = clampf(t->position, m->position_min, m->position_max);
float velocity = clampf(t->velocity, m->velocity_min, m->velocity_max);
float torque = clampf(t->torque, m->torque_min, m->torque_max);
float kp = clampf(t->kp, m->kp_min, m->kp_max);
float kd = clampf(t->kd, m->kd_min, m->kd_max);
uint32_t pos_u = float_to_uint(position, m->position_min, m->position_max, 16);
uint32_t vel_u = float_to_uint(velocity, m->velocity_min, m->velocity_max, 12);
uint32_t torq_u = float_to_uint(torque, m->torque_min, m->torque_max, 12);
uint32_t kp_u = float_to_uint(kp, m->kp_min, m->kp_max, 12);
uint32_t kd_u = float_to_uint(kd, m->kd_min, m->kd_max, 12);
uint32_t lower_u32 = torq_u | (kd_u << 12) | ((kp_u & 0xFFu) << 24);
uint32_t upper_u32 = (kp_u >> 8) | (vel_u << 4) | (pos_u << 16);
store_u32_le(out, lower_u32);
store_u32_le(out + 4, upper_u32);
}
堵转错误
当电机持续以峰值力矩输出时，会触发堵转错误。如果您想彻底避开堵转错误，您需要将电机的最大输出力矩限制为 80% 峰值力矩。即向6072h写入 800。
对象字典
仅提供与 CiA301 和 CiA402 不同的部分。由于分发问题，我们无法提供 CiA301.pdf 与 CiA402.pdf，请您使用搜索引擎搜索相关内容。
软件版本
通过读取 1018h 03h 我们可以获取电机的固件版本。不同固件版本的对象字典可能有所不同，请注意。
额外阅读
- hex-motor-control-test — 完整 C 语言控制演示（MIT / 轮廓速度）
- Hex Motor Example
版本变更历史
- 0x08
- 修改浮点数格式，由Q21改为Float32
- 增加压缩型MIT控制参数