本文档聚焦Roboto平台电机驱动栈的全链路架构与实现细节，覆盖从物理层USB2CAN适配器固件到应用层推理节点的高频控制闭环。核心目标是为具备嵌入式与ROS2基础的开发者提供一套可复现、可调试、可扩展的电机通信知识体系，内容涵盖CAN/CAN-FD协议抽象、SocketCAN实时收发线程、多品牌电机驱动（DM/EVO/LRO）的MIT阻抗控制模式，以及推理节点中的多总线并行调度策略。阅读前建议已完成部署架构与实时内核配置与IMU驱动与传感器集成章节的环境准备。

Sources: readme_cn.md, robot_interface.cpp

系统架构概览

电机驱动栈采用四层硬件抽象设计，确保协议层与驱动层解耦，使同一套上层控制逻辑可透明切换CAN、CAN-FD乃至EtherCAT后端。物理通信依赖自研的roboto_usb2can适配器，该设备在Linux下通过原生gs_usb驱动枚举为SocketCAN接口（如can0），从而允许用户空间程序以标准Berkeley Socket API收发帧。

flowchart TB
    subgraph APP["应用层"]
        A1["推理节点 inference_node"]
        A2["Python SDK / 标定脚本"]
    end
    subgraph DRV["驱动层"]
        D1["DmMotorDriver"]
        D2["EvoMotorDriver"]
        D3["LroMotorDriver"]
        D4["MotorDriver 抽象工厂"]
    end
    subgraph PROTO["协议层"]
        P1["MotorsCAN / MotorsCANFD"]
        P2["MotorsSocketCAN"]
        P3["MotorsSocketCANFD"]
    end
    subgraph HW["硬件层"]
        H1["主控板 USB Host"]
        H2["roboto_usb2can 适配器"]
        H3["CAN 总线"]
        H4["关节电机"]
    end

    A1 --> D4
    A2 --> D4
    D4 --> D1 & D2 & D3
    D1 & D2 & D3 --> P1 & P3
    P1 --> P2
    P3 --> P2 & P3
    P2 --> H1
    P3 --> H1
    H1 --> H2 --> H3 --> H4

Sources: motor_driver.hpp, can_iso.hpp

USB2CAN适配器固件

roboto_usb2can是运行于STM32G431CBT6的Zephyr RTOS固件，实现gs_usb协议以兼容candleLight生态。其核心价值在于：Linux系统无需额外驱动，插入即可生成can0/can1等标准SocketCAN网络接口；Windows系统则通过内嵌的Microsoft OS 2.0描述符实现WinUSB自动绑定，真正做到跨平台即插即用。

固件内置了多级CAN总线故障保护机制。can_state_change_callback持续监听CAN控制器状态机，当检测到错误帧洪水（每秒超过50帧）或持续ERROR_PASSIVE（累计超过10次）时，主动调用can_stop强制进入Bus-Off，防止故障节点拖垮整网。状态变化通过板载三色LED直观呈现：蓝灯指示USB就绪/错误，黄灯指示CAN活跃/警告/总线关闭，绿灯则作为数据收发活动指示灯。

Sources: main.c, roboto_usb2can.h

CAN协议层：SocketCAN实时收发

协议层以MotorsCAN和MotorsCANFD为抽象基类，当前提供基于Linux SocketCAN的具体实现。MotorsSocketCAN采用单例模式管理每个网络接口的生命周期，确保同一进程中对can0的多次引用共享同一个套接字与收发线程，避免资源冗余。

为达到足控所需的千赫兹级通信频率，收发线程均配置了硬实时属性。接收线程与发送线程均通过pthread_setschedparam设定SCHED_FIFO调度策略，优先级固定为80；同时依据接口名末位数字自动绑定到不同CPU核心（如can0绑定到倒数第1核，can1绑定到倒数第2核），以最小化缓存失效与上下文切换开销。发送侧使用boost::lockfree::queue实现无锁队列，容量4096帧，配合条件变量实现低延迟唤醒与流量反压。

sequenceDiagram
    participant M as 电机驱动实例
    participant TX as Sender线程<br/>(SCHED_FIFO, CPU N)
    participant Q as Lock-free TX Queue
    participant SOC as SocketCAN
    participant CAN as 物理CAN总线

    M->>Q: bounded_push(frame)
    M->>TX: notify_one()
    TX->>Q: pop(frame)
    TX->>SOC: write(sockfd, frame)
    SOC->>CAN: 电平信号
    loop MAX_RETRY_COUNT=3
        alt write失败
            TX->>TX: sleep 1ms
            TX->>SOC: 重试write
        end
    end

Sources: socket_can.hpp, socket_can.cpp

电机驱动层：抽象工厂与品牌实现

MotorDriver作为抽象基类定义了统一的控制接口，包括MIT阻抗控制、位置模式、速度模式、零位标定与错误清除。具体品牌驱动通过create_motor工厂方法按需实例化。当前支持DM（达妙）、EVO以及LRO（LeadRobot）三大系列，其中DM与EVO为Atom01主力关节电机。

DM与EVO驱动在CAN 2.0模式下共享高度相似的帧打包范式。以MIT控制命令为例，目标位置、速度、Kp、Kd、力矩五个浮点量首先被range_map映射到固定位宽的无符号整数（位置16位，其余12位），再按大端序与位域交错填入8字节数据段。回传帧的解析则是逆过程：从can_frame.data中提取位域后逆映射为物理量，并叠加motor_zero_offset补偿机械安装零偏。

Sources: motor_driver.cpp, dm_motor_driver.cpp, evo_motor_driver.cpp

MIT阻抗控制与数据解析

MIT模式（又称阻抗控制模式）是足式机器人最常用的高带宽控制接口。其数学本质是在电机端实现一个虚拟弹簧阻尼系统，输出力矩由以下公式决定：

τ = Kp × (pos_target − pos_actual) + Kd × (vel_target − vel_actual) + τ_feedforward

在总线层面，DM与EVO电机要求发送方将五个参数压缩到单个8字节CAN帧内。以DM电机为例，motor_mit_cmd先将浮点参数裁剪到型号对应的物理极限（如DM4340P的位置极限±12.5 rad），再映射到定点数：位置占16位，速度、Kp、Kd、力矩各占12位。最终帧结构为：[15:0]p, [15:4]v, [3:0]v+[11:8]kp, [7:0]kp, [11:4]kd, [3:0]kd+[11:8]t, [7:0]t。

回传帧以主站ID（motor_id + master_id_offset）作为can_id。can_rx_cbk回调从中解析位置（16位）、速度（12位）、力矩（12位），并通过range_map恢复为浮点物理量。错误码位于data[0]高4位，当值大于7时触发日志报错。

Sources: dm_motor_driver.cpp, utils.hpp

CAN-FD批模式与多总线并行

在推理节点的高频闭环中，逐电机单帧发送会成为显著瓶颈。EVO驱动在CAN-FD模式下支持单帧多机批写：利用CAN-FD最大64字节的有效载荷，将最多8台电机的MIT数据按8字节/槽位紧凑排列，通过EVOFD_MIT_ID（0x20）统一广播。EvoMotorDriver::motor_mit_cmd(float*, ...)会查询静态bus_registry_，按motor_index_将各电机的目标值写入对应槽位，随后一次性发出。

RobotInterface::apply_action进一步在总线维度并行化：通过线程池为每条CAN/CAN-FD总线分配独立任务，各任务填充该总线下的电机数组后调用批写接口。这种“总线级并行 + 帧级批处理”的双重优化，使得23关节的全局控制周期可稳定运行在1kHz量级。

flowchart LR
    subgraph BUS0["can0 (6 motors)"]
        B0_M1["motor_id=1 slot=0"]
        B0_M2["motor_id=2 slot=1"]
    end
    subgraph BUS1["can1 (7 motors)"]
        B1_M1["motor_id=1 slot=0"]
        B1_M2["motor_id=2 slot=1"]
    end
    A["推理输出 action[23]"] --> T["ThreadPool 4 tasks"]
    T --> BUS0 & BUS1 & BUS2 & BUS3
    BUS0 --> F0["CAN-FD Frame 64B<br/>EVOFD_MIT_ID=0x20"]
    BUS1 --> F1["CAN-FD Frame 64B<br/>EVOFD_MIT_ID=0x20"]

Sources: evo_motor_driver.cpp, robot_interface.cpp

配置文件与电机拓扑

电机拓扑在robot.yaml中声明，字段语义如下：

RobotInterface::setup_motors按上述配置逐条总线、逐个电机调用MotorDriver::create_motor，并通过motor_sign数组在应用层处理关节方向反转，无需修改底层驱动。close_chain_motor_id则用于标记闭链传动关节（如脚踝），这些关节在apply_action中会经过ankle_decouple_进行力/位解耦计算。

Sources: robot.yaml, robot_interface.cpp

Python SDK快速上手

roboto_motors通过pybind11暴露Python接口，编译后生成motors_py模块。以下示例演示单台DM电机的创建、初始化、MIT控制与状态回读：

import motors_py
import time

motor = motors_py.MotorDriver.create_motor(
    motor_id=1,
    interface_type="can",
    interface="can0",
    motor_type="DM",
    motor_model=0,          # DM4340P_48V
    master_id_offset=16,
)

motor.init_motor()          # 解锁 → 设模式 → 锁定 → 刷新状态
motor.motor_mit_cmd(-0.5, 0.0, 5.0, 1.0, 0.0)

time.sleep(0.1)
print(f"位置: {motor.get_motor_pos():.4f} rad")
print(f"错误码: {motor.get_error_id()}")
motor.deinit_motor()

Sources: motors_py_example.py, pybind_module.cpp

故障排查与最佳实践

调试时建议首先使用roboto_usb2can_tool.py上位机扫描总线，确认电机原始回包正常后，再切入ROS2推理节点。若需修改电机ID或零位，可调用set_motor_id与set_motor_zero接口，后者会主动回读位置并校验绝对值是否小于judgment_accuracy_threshold（默认0.01 rad）。

Sources: socket_can.cpp, motor_driver.hpp

延伸阅读与下一步

完成电机驱动与CAN总线通信的部署后，你已具备让关节响应控制指令的基础能力。接下来建议阅读推理节点与ONNX策略部署，了解如何将训练好的策略模型接入本文档所述的电机栈，实现从Sim到Real的完整闭环。如需扩展新品牌电机，可参考drivers/dm或drivers/evo目录，继承MotorDriver并实现对应的帧打包/解包逻辑即可。