达妙DM电机驱动详解 · 电机子模块

本文档深入解析达妙（Dynamixel-style/达妙科技）DM系列电机在本SDK中的驱动实现。作为面向中间层开发者的技术说明，内容覆盖DM驱动的类架构、通信协议编码细节、三种控制模式的指令格式、状态反馈解析方式以及寄存器级参数读写机制。阅读前建议先熟悉MotorDriver抽象基类与接口设计和工厂模式与多品牌电机实例化，以便理解DM驱动在整个多电机抽象层中的位置。

DM驱动类架构与继承关系

DmMotorDriver 直接继承自 MotorDriver 抽象基类，通过工厂方法 MotorDriver::create_motor() 完成实例化。当用户传入 motor_type == "DM" 时，工厂内部调用 DmMotorDriver 构造函数，并绑定到 SocketCAN 或 SocketCAN-FD 单例上。该设计使得 DM 驱动天然具备 CAN 与 CAN-FD 双栈通信能力，但当前实现不支持 EtherCAT。类内部通过 comm_type_ 字段区分底层总线类型，并在所有发送函数中采用 if/else 双分支分别构造 can_frame 与 canfd_frame，从而保证协议语义在不同物理层上的一致性。

classDiagram
    class MotorDriver {
        <<abstract>>
        +lock_motor()*
        +unlock_motor()*
        +init_motor()* uint8_t
        +motor_mit_cmd(float f_p, float f_v, float f_kp, float f_kd, float f_t)*
        +motor_pos_cmd(float pos, float spd, bool ignore_limit)*
        +motor_spd_cmd(float spd)*
        +refresh_motor_status()*
        #motor_id_: uint16_t
        #comm_type_: CommType
    }
    class DmMotorDriver {
        +DmMotorDriver(uint16_t motor_id, string interface_type, ...)
        -master_id_: uint16_t
        -motor_model_: DM_Motor_Model
        -limit_param_: DM_Limit_Param
        -can_rx_cbk(const can_frame&)
        -canfd_rx_cbk(const canfd_frame&)
        -write_register_dm(uint8_t rid, float value)
        -save_register_dm()
        -set_motor_zero_dm()
    }
    MotorDriver <|-- DmMotorDriver
    DmMotorDriver --> "1" MotorsCAN : can_
    DmMotorDriver --> "1" MotorsCANFD : canfd_

Sources: motor_driver.cpp, dm_motor_driver.hpp

支持的电机型号与限幅参数

SDK 当前内置两种 DM 电机的限幅表，存储在全局数组 dm_limit_param 中。该表在构造时根据 motor_model 索引拷贝到成员 limit_param_，后续所有控制指令均依据此表对输入进行饱和限幅，防止因超量程设定导致电机固件拒收或异常。限幅参数包含位置最大值（rad）、速度最大值（rad/s）、力矩最大值（N·m）以及 MIT 外环增益上限。

电机型号	PosMax (rad)	SpdMax (rad/s)	TauMax (N·m)	OKpMax	OKdMax
DM4340P_48V	12.5	20	28	500	5
DM10010L_48V	12.5	25	200	500	5

构造时若传入 interface_type 不是 can、canfd 或 ethercanfd，会立即抛出 std::runtime_error，确保驱动实例与总线能力严格匹配。同时 master_id_ 被计算为 motor_id_ + master_id_offset，用于注册 CAN 接收回调时的过滤与匹配。

Sources: dm_motor_driver.cpp, dm_motor_driver.cpp

初始化与生命周期管理

init_motor() 实现了 DM 电机的标准上电初始化序列：先调用 unlock_motor() 使电机进入空闲只读状态，延时 5 ms 后通过 set_motor_control_mode(MIT) 将控制模式寄存器写入 MIT，再次延时后调用 lock_motor() 切入使能状态，最后执行 refresh_motor_status() 拉取一次实时状态。若此时电机回传的错误码非零，函数会将对应 DMError 返回给调用方，便于上层在上电阶段即识别过压、过流或失联等故障。deinit_motor() 则仅执行 unlock_motor() 并延时，完成安全下电。

值得特别注意的是，set_motor_zero() 并非简单下发零位指令，而是包含验证闭环：发送置零帧后等待 500 ms，读取当前位置并与 judgment_accuracy_threshold（1e-2 rad）比对。若位置未落入阈值区间，函数记录警告日志并返回 false，提示机械或编码器可能存在异常；成功则返回 true。该设计为批量装机后的零位校准提供了可编程的自动化校验能力。

Sources: dm_motor_driver.cpp, dm_motor_driver.cpp

控制模式与指令协议编码

DM 电机支持三种控制模式：MIT 阻抗模式、位置模式与速度模式。模式切换通过向寄存器 DM_CTRL_MODE(0x0A) 写入对应枚举值实现。以下分别说明各模式在 CAN/CAN-FD 总线上的帧格式与编码方式。

MIT 阻抗控制模式

MIT 模式是 DM 电机最具特色的控制方式，允许在一次 8 字节传输中同时下发目标位置、目标速度、位置刚度 kp、速度阻尼 kd 以及前馈力矩。为了压缩到 8 字节负载，协议采用非对称定长编码：位置占 16 bit，速度、kp、kd、力矩各占 12 bit。编码前，驱动代码先利用 range_map 将浮点物理量线性映射到无符号整型区间，再按大端/位域方式拼装进 data[]。

packet-beta
    title MIT 模式 8-byte CAN Payload
    0-15: "p (16bit)"
    16-27: "v (12bit)"
    28-39: "kp (12bit)"
    40-51: "kd (12bit)"
    52-63: "t (12bit)"

具体字节布局为：data[0:1] 存储 p 的高 8 位与低 8 位；data[2] 存储 v 的高 8 位；data[3] 的低 4 位存储 v 的低 4 位，高 4 位存储 kp 的高 4 位；data[4] 存储 kp 的低 8 位；data[5] 存储 kd 的高 8 位；data[6] 的低 4 位存储 kd 的低 4 位，高 4 位存储 t 的高 4 位；data[7] 存储 t 的低 8 位。CAN ID 直接使用电机 ID（motor_id_）。该编码方式与达妙官方 MIT 控制包定义保持一致。

Sources: dm_motor_driver.cpp

位置控制模式

位置模式通过 CAN ID 0x100 + motor_id_ 下发。负载为 8 字节，前 4 字节为 IEEE-754 单精度浮点目标位置，后 4 字节为目标速度。驱动代码在发送前自动减去 motor_zero_offset_，并对位置、速度做限幅，保证它们落在当前电机型号对应的 PosMax 与 SpdMax 范围内。这意味着用户代码中传入的全局坐标已经过零偏修正，降低了上层控制器的坐标转换负担。

Sources: dm_motor_driver.cpp

速度控制模式

速度模式使用 CAN ID 0x200 + motor_id_，负载仅需 4 字节，即一个单精度浮点目标速度。与位置模式类似，驱动同样执行饱和限幅，确保速度指令在 [-SpdMax, SpdMax] 区间内。此模式适用于需要连续旋转或速度伺服的场景，帧长度短于位置模式，总线占用更低。

Sources: dm_motor_driver.cpp

状态反馈解析与错误码

DM 电机的状态回传帧由电机主动周期性发送，CAN ID 为 master_id_（即 motor_id_ + offset）。can_rx_cbk 是解析核心，其位域提取逻辑与 MIT 发送包呈对偶关系：data[1:2] 组成 16 bit 位置；data[3] 与 data[4] 高 4 位组成 12 bit 速度；data[4] 低 4 位与 data[5] 组成 12 bit 力矩。提取后的无符号整型通过 range_map 逆映射回浮点物理量，并分别写入原子成员 motor_pos_、motor_spd_、motor_current_，确保在多线程环境下读端无锁安全。

data[0] 的高 4 位承载错误码。当该值大于 7 时，驱动将错误码存入 error_id_ 并输出 spdlog 错误日志。下表列出 DM 电机定义的全部错误状态：

错误码	枚举名	含义
0x00	DM_DOWN	电机已失能/关闭
0x01	DM_UP	电机已使能/正常
0x08	DM_OVER_VOLT	过压保护
0x09	DM_UNDER_VOLT	欠压保护
0x0A	DM_OVER_CURRENT	过流保护
0x0B	DM_MOS_OVER_TEMP	MOS 管过温
0x0C	DM_COIL_OVER_TEMP	线圈过温
0x0D	DM_LOST_CONN	通信失联
0x0E	DM_OVER_LOAD	过载保护

data[6] 与 data[7] 分别对应 MOS 温度和电机本体温度，单位为摄氏度。canfd_rx_cbk 采用了一层薄适配：将 canfd_frame 的前 8 字节拷贝到 can_frame 后复用同一解析函数，避免重复逻辑。

Sources: dm_motor_driver.cpp, dm_motor_driver.hpp

寄存器级参数读写与持久化

除实时控制指令外，DM 驱动还提供寄存器读写能力，用于在线修改电机内部参数。所有寄存器操作统一使用 CAN ID 0x7FF（广播/管理帧），并通过 data[0:1] 携带目标电机 ID 实现寻址。

写寄存器：write_register_dm(uint8_t rid, float/int32_t value) 发送 0x55 命令码 + 寄存器号 + 4 字节值。SDK 同时提供了浮点与整型两个重载，方便根据寄存器语义选择数据类型。
读寄存器：get_motor_param(uint8_t param_cmd) 发送 0x33 命令码 + 寄存器号，后 4 字节填 0xFF。
保存到 Flash：save_register_dm() 发送 0xAA + 0x01 命令组合，触发电机将当前内存参数写入内部 Flash。这是 DM 电机实现配置持久化的关键步骤。
刷新状态：refresh_motor_status() 发送 0xCC + 0x00，请求电机立即回传一帧实时状态。

DM_REG 枚举在头文件中定义了 37 个标准寄存器地址，涵盖欠压阈值、力矩常数、最大速度、控制模式、阻尼、惯量、硬件版本、极对数、相电阻/电感、磁链、减速比、位置/速度/力矩上限、电流环/速度环带宽与 PI 参数、ADC 偏置校准、方向设定等。开发者可通过这些寄存器对电机进行深度标定与性能调优。

Sources: dm_motor_driver.cpp, dm_motor_driver.cpp, dm_motor_driver.hpp

特殊指令与固件交互

DM 电机采用“魔数帧”方式实现若干关键状态切换，这些帧的 CAN ID 为电机自身 ID，负载前 7 字节固定为 0xFF，仅通过第 8 字节区分功能：

功能	第 8 字节	对应 SDK 接口
使能电机（lock）	`0xFC`	`lock_motor()`
失能电机（unlock）	`0xFD`	`unlock_motor()`
置零位	`0xFE`	`set_motor_zero_dm()`
清除错误	`0xFB`	`clear_motor_error_dm()`

clear_motor_error() 公开接口直接代理到 clear_motor_error_dm()，发送 0xFB 魔数帧后，若电机故障源已解除，固件将从保护态恢复至待机态。需要强调的是，此操作仅重置 MCU 的错误标志，并不能修复硬件层面的过压或过流根因。

此外，write_motor_flash() 在 DM 驱动中被实现为直接返回 true，因为 DM 电机的参数持久化由独立的 save_register_dm() 完成。这与 EVO 系列将置零与写 Flash 捆绑的设计形成差异，也体现了多品牌抽象层下各驱动对同一虚接口的不同实现策略。

Sources: dm_motor_driver.cpp, dm_motor_driver.cpp, dm_motor_driver.cpp, dm_motor_driver.cpp

精度与数值转换工具链

DM 驱动的数值转换依赖 utils.hpp 中提供的模板工具。limit<T>(val, min, max) 基于 std::clamp 实现饱和限幅；range_map<T, U>(val, in_min, in_max, out_min, out_max) 负责物理量与无符号整型之间的线性映射；bitmax<T>(n) 计算 n 位无符号整型的最大值，例如 bitmax<uint16_t>(12) 返回 4095。这些工具在 MIT 模式的打包与解包路径中被反复调用，构成了驱动层数值正确性的基石。

Sources: utils.hpp

使用建议与常见问题

零偏处理：DM 驱动在 motor_pos_cmd 与 motor_mit_cmd 中会自动减去 motor_zero_offset_，因此上层控制器应以机械零位为原点发出指令，无需手动补偿。
模式切换延迟：motor_pos_cmd、motor_spd_cmd、motor_mit_cmd 都会在检测到当前模式不匹配时先调用 set_motor_control_mode() 并立即返回，实际运动指令将在下一控制周期生效。建议在高频循环中保持模式一致，避免周期性切换导致电机抖动。
CAN-FD 兼容性：CAN-FD 帧在发送时均设置了 CANFD_BRS 标志以启用比特率切换，可提升总线吞吐。canfd_rx_cbk 仅解析前 8 字节，因此 DM 电机在 CAN-FD 模式下并未利用 64 字节负载扩展，兼容层保证了与传统 CAN 的语义等价。
读参回调缺失：当前 get_motor_param() 仅下发读寄存器指令，未在 can_rx_cbk 中实现对读返回帧的解析与缓存；若需要在线读取电机参数，开发者需自行扩展接收逻辑或改用厂商上位机工具。

如需进一步了解 MIT 控制模式的数学原理，请参阅 MIT阻抗控制原理与实现；若需对比其他品牌电机的协议差异，可阅读 EVO电机驱动详解与 LeadRobot电机驱动详解。