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 sample-point 0.75 sjw 5 dsjw 3 fd on
注意
必须正确设置采样点，否则发送长帧时必定导致通信失败。有些USB转CAN设备不一定正确支持设置这些参数。如果遇到这种情况，请转而使用 RK3576 等硬件 CAN-FD 支持的 SOC。
配置电机ID 与 解析CAN消息
我们的电机使用 CANOpen(CiA301) 协议。CANOpen是工业自动化领域广泛使用的协议，网上拥有诸多教程，在此不做赘述。其中针对PDO做了一些小修改，允许PDO最大长度为 64 字节，以此充分利用CAN-FD的带宽。
CAN-ID的设置是通过对象字典 2001h, 1h进行设置的。
例如使用 canopend 工具进行配置，将原本的1号电机改为3号:
关于如何使用 canopend 工具，请参考其 Github 介绍，此处不做解释说明
新ID断电生效
新ID设置后，断电才会生效。请您务必小心，不要设置了错误的ID。
默认的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。
总之，一个典型的完整电机初始化与控制流程是这样的：
- 上位机程序读取 一次 1018h 03h 对象获取电机固件版本(必须为版本08)，以确保上位机程序与电机固件版本兼容。
- 上位机程序读取 一次 6076h 00h 对象获取电机的峰值力矩，方便后续计算转换为国际单位力矩。
- 上位机程序读取 一次 2003h 07h 对象获取 MIT KP，KD的尺度因子，方便转换为国际单位量纲的KP，KD。（仅限程序版本08，且仅限需要使用MIT模式）
- 上位机程序配置心跳包检测 1016h 00h 对象，确保上位机程序结束时电机会自动退出使能。（可选，从第一次接收到目标节点心跳包开始计时）
- 上位机配置TPDO与RPDO映射（如果已经通过 1010h 对象完成过参数保存，则不需要进行这一步）
- 上位机程序生成CAN接收处理进程，针对电机的TPDO与HEARTBEAT以及可选的EMCY进行解析并处理
- 上位机发送NMT指令将电机设置为 Operational 状态。
- 上位机正确配置 CiA402 状态机，选择进入的模式并使能电机。
- 上位机开始发送自己的心跳包（可选，如果需要心跳包超时保护。自己的心跳包建议低频发送。），并开始通过PDO发送控制指令。
这里提供解析部份（也就是第6步）的部分代码示例
有几个小细节
- 速度使用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为同一个来实现一拖多的功能。
听起来有点抽象？我们以四个电机，使用力位混合控制为例。
两个电机的CANOpenID分别为0x01,0x02。每个电机都控制他们的 6072h(最大力矩/Max Torque) 与 60FFh(目标速度/Target Velocity)。将这两个电机的RPDO1的COB-ID均设置为0x10节点的TPDO1 COB-ID上。
那么每个电机有两个有效对象。其他字节使用填充即可。我们假定将6072h排在前面，那么对于一号电机，映射应该是这样的：60FFh(4bytes)|6072h(2bytes)|fill(4bytes)|fill(4bytes)|fill(4bytes)|fill(4bytes)|fill(2bytes)。因为一共有4个电机，所以需要 4 * ( 4 + 2 ) = 24 字节来一帧控制所有电机，因此需要在后方放18字节长的填充对象。
那么二号电机同理，就应该是：fill(4bytes)|fill(2bytes)|60FFh(4bytes)|6072h(2bytes)|fill(4bytes)|fill(4bytes)|fill(4bytes)。以此类推。
保存PDO Mapping
PDO Mapping 映射是可以保存的，避免每次开机都重新设置。具体请查看对象 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总线占用率。
堵转错误
当电机持续以峰值力矩输出时，会触发堵转错误。如果您想彻底避开堵转错误，您需要将电机的最大输出力矩限制为 80% 峰值力矩。即向6072h写入 800。
控制测试
请谨慎操作
开始电机控制前，请务必确保周围环境安全。由于在终端中反复发送某个节点的心跳包不是特别方便，这里提供的两个案例都没有使用心跳超时检测。
SDO控制测试
关于SDO控制测试
事实上你应该使用PDO写例如 60FFh 等对象，但我们这里先使用SDO进行CAN通信演示。
请谨慎操作
开始电机控制前，请务必确保周围环境安全
开始之前，我们重新对电机上电，确保电机处于初始状态。假定需要控制的电机为0x01 与 0x02。我们将使用速度模式进行演示。其中1号电机的力矩将被限制到峰值力矩的5%，2号电机将被限制到 80% 峰值力矩。
1 w 0x6060 0 i8 3
2 w 0x6060 0 i8 3
1 w 0x60FF 0 i32 0
2 w 0x60FF 0 i32 0
1 w 0x6040 0 u16 6
2 w 0x6040 0 u16 6
1 w 0x6072 0 u16 50
2 w 0x6072 0 u16 800
等待上述全部写入后，再将控制字写7
最后再写0x0f
此时电机启动使能，目标为 0Rev/s。写入Float32单位的转速即可。这里准备以0.1Rev/s的速度进行测试。
此时两个电机开始以0.1Rev/s的速度旋转，并且两电机的最大力矩不同，其中一个施加较小阻力就会停止，另一个则需要更大的阻力才会停止。
PDO控制测试
显然，使用SDO通信带宽浪费明显。应该尽量使用PDO进行控制，但正如前文所提到的，PDO映射需要根据实际操作的电机数量进行调整，因此本demo仍然只演示控制4个电机的情况，其余情况请根据实际情况调整。
调整PDO Mapping
为了实现一帧拖多电机进行控制，原有的RPDO COB-ID是无法满足需求的。因此不仅需要调整所有被控电机的 Mapping，还需要调整 RPDO COB-ID。
以带力矩上限的速度控制为例，每个电机都需要使用 6072h(最大力矩/Max Torque) 与 60FFh(目标速度/Target Velocity) 这两个对象。因此每个电机都需要六个字节。一共有4个电机，因此需要24字节。
对于发送的程序来讲，全部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)
那么对1号电机来讲，他的RPDO Mapping应该是这样的：
最大力矩 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的灵活特性，您可以自由安排。假设实际上目标速度需要更高的变动频率，而最大力矩不需要，那么完全可以将所有的最大力矩放到RPDO2，以高频发送目标速度，低频发送最大力矩。
也无需强制所有的电机都处于同一控制模式，完全可以一部分电机处于速度控制模式，一部分电机使用MIT控制模式。只要 Mapping 正确即可。
关于配置自动化与配置持久化
请记住，本质上 canopend 就是发送了一些 can 消息，您完全可以将配置PDO的过程自动化。关于 SDO 协议的具体格式请直接查看丰富的互联网教程。
另外，这些配置除非通过 1010h 对象进行保存，否则每次开机后都会恢复为默认配置。如果您确认不会改变这些配置，那么只要通过 1010h 对象进行保存，即可实现配置持久化。后续开机无需重新配置，仅需发送NMT指令与选择模式，操作控制字。
这里我们需要选一个 COB-ID 做所有电机的 RPDO1 COB-ID。这里我们选择 0x10 节点的 TPDO1 COB-ID（即0x190）。您可以任意选择一个CAN-ID，只要确保不会与其他节点冲突即可。这里推荐选主站的TPDO1，2，3或4。
- 首先给 1400h 01h 写
0x8000_0000 | 0x190失能 RPDO1 - 设置 1400h 02h 传输类型设置为 255
- 设置 1600 00h RPDO 有效映射数为 0
- 设置 1600 01h 应用对象1为 4字节填充 3000h 03h
- 设置 1600 02h 应用对象2为 2字节填充 3000h 02h
- 设置 1600 03h 应用对象3为 最大力矩 6072h 00h
- 设置 1600 04h 应用对象4为 目标速度 60FFh 00h
- 设置 1600 05h 应用对象5为 4字节填充 3000h 03h
- 设置 1600 06h 应用对象6为 4字节填充 3000h 03h
- 设置 1600 07h 应用对象7为 4字节填充 3000h 03h
- 设置 1600 00h 有效映射数为 7
- 最后给 1400h 01h 写
0x0000_0000 | 0x190使能 RPDO1
此时已经完成RPDO配置，接下来输入 2 start 发送 NMT指令，使电机进入 CiA301 的 Operational 状态。
但此时还不能通过 PDO 进行控制，因为还没有配置 CiA402 状态机。因为只需要上电配置一次，也使用 SDO 进行演示。
- 给 6040h 00h 写
0x80清除错误 - 给 6060h 00h 写
0x03设置电机为速度控制模式 - 给 6040h 00h 写
0x06Shutdown - 给 6040h 00h 写
0x07Switch On - 给 6040h 00h 写
0x0fOperation Enable
此时电机可以开始通过 PDO 进行控制。下面以将所有电机最大力矩限制为 80%，目标转速为 1Rev/s 为例。
如果正确配置了所有的电机，此时所有电机都会开始运行。
对象字典
仅提供与 CiA301 和 CiA402 不同的部分。由于分发问题，我们无法提供 CiA301.pdf 与 CiA402.pdf，请您使用搜索引擎搜索相关内容。
软件版本
通过读取 1018h 03h 我们可以获取电机的固件版本。不同固件版本的对象字典可能有所不同，请注意。
版本变更历史
- 0x07
- 首个对外公开版本
- 0x08
- 修改浮点数格式，由Q21改为Float32
- 增加压缩型MIT控制参数