# EtherCAT 电机接入教程

这份文档只说明新增一种 EtherCAT 电机需要改哪里、怎么写。

## 1. 增加 vendor

修改 `protos/cmvr/config/motor_config/motor_config.proto`：

```proto
enum MotorVendor {
  MOTOR_VENDOR_UNKNOWN = 0;
  MOTOR_VENDOR_TI5 = 1;
  MOTOR_VENDOR_MUJOCO = 2;
  MOTOR_VENDOR_XXX = 3;
}
```

`MOTOR_VENDOR_XXX` 改成真实厂商名，例如 `MOTOR_VENDOR_FOO`。不要复用 `TI5`。

## 2. 写电机配置

新增配置文件：

```text
cmvr-es/config/devices/motor/ethercat_motors.pb.txt
```

示例：

```proto
motor {
  id: "ethercat_motors"

  motor_groups {
    id: "right_arm_ethercat"
    bus_type: MOTOR_BUS_ETHERCAT
    vendor: MOTOR_VENDOR_XXX
    protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
    tool_frame: "R_FINGER_TIP"

    ethercat {
      master_id: "eth0"
      cycle_us: 1000
      slaves { motor_id: 1 slave_index: 0 vendor_id: 0x00000000 product_code: 0x00000000 }
      slaves { motor_id: 2 slave_index: 1 vendor_id: 0x00000000 product_code: 0x00000000 }
      slaves { motor_id: 3 slave_index: 2 vendor_id: 0x00000000 product_code: 0x00000000 }
      slaves { motor_id: 4 slave_index: 3 vendor_id: 0x00000000 product_code: 0x00000000 }
      slaves { motor_id: 5 slave_index: 4 vendor_id: 0x00000000 product_code: 0x00000000 }
      slaves { motor_id: 6 slave_index: 5 vendor_id: 0x00000000 product_code: 0x00000000 }
      slaves { motor_id: 7 slave_index: 6 vendor_id: 0x00000000 product_code: 0x00000000 }
    }

    joint_limits {
      enable: true
      source: JOINT_LIMIT_SOURCE_CUSTOM
      joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
      joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
      joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
      joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
      joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
      joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
      joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
    }

    motors {
      motors { id: 1 joint_name: "R_SHOULDER_P" }
      motors { id: 2 joint_name: "R_SHOULDER_R" }
      motors { id: 3 joint_name: "R_SHOULDER_Y" }
      motors { id: 4 joint_name: "R_ELBOW_R" }
      motors { id: 5 joint_name: "R_WRIST_P" }
      motors { id: 6 joint_name: "R_WRIST_Y" }
      motors { id: 7 joint_name: "R_WRIST_R" }
    }
  }
}
```

`motors.motors.id` 是系统内的电机逻辑 id。`ethercat.slaves.motor_id` 必须和它对应。

`joint_limits` 按 `joint_name` 读取，和 CAN、MuJoCo 电机保持同一个风格。

## 3. 注册设备

修改：

```text
cmvr-es/config/manager/device_manager.pb.txt
```

加入：

```proto
devices {
  id: "ethercat_motors"
  type: MOTOR_SYSTEM
  enable: true
  config_file: "devices/motor/ethercat_motors.pb.txt"
}
```

## 4. 机械臂使用 EtherCAT group

修改机械臂配置，例如：

```text
cmvr-es/config/devices/arm/arm.pb.txt
```

把 motor backend 改成：

```proto
motor {
  motor_system_id: "ethercat_motors"
  motor_group_ids: "right_arm_ethercat"
  joint_names: "R_SHOULDER_P"
  joint_names: "R_SHOULDER_R"
  joint_names: "R_SHOULDER_Y"
  joint_names: "R_ELBOW_R"
  joint_names: "R_WRIST_P"
  joint_names: "R_WRIST_Y"
  joint_names: "R_WRIST_R"
}
```

## 5. 实现 bus runtime

EtherCAT 总线资源放在：

```text
cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h
cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp
```

`EthercatMotorBusRuntime` 负责：

```text
读取 ethercat 配置
初始化 EtherCAT master
扫描/校验 slave_index、vendor_id、product_code
启动 cyclic loop
保存 command/feedback buffer
停止 cyclic loop
```

bus runtime 不创建具体电机，也不关心厂商；它只保存总线连接、线程和数据缓存。

## 6. 增加具体电机 driver

新增目录：

```text
cmvr-es/devices/motor/drivers/xxx_ethercat/
  CMakeLists.txt
  include/xxx_ethercat_motor.h
  include/xxx_ethercat_motor_protocol.h
  src/xxx_ethercat_motor.cpp
  src/xxx_ethercat_motor_protocol.cpp
```

`XxxEthercatMotor` 继承 `AbstractMotor`。

`XxxEthercatMotorProtocol` 继承 `MotorProtocolInterface`，把 `setTarget`、`setQd`、`getQ` 等接口转换成 EtherCAT command/feedback。

## 7. 在 MotorManager 里创建 EtherCAT 电机

修改：

```text
cmvr-es/devices/motor/manager/src/motor_manager.cpp
```

在 `MotorManager::createEthercatMotors_()` 里按 `vendor + protocol` 创建具体电机：

```cpp
auto ethercat_bus_runtime =
    std::dynamic_pointer_cast<EthercatMotorBusRuntime>(bus_runtime);

if (group_cfg.vendor() == config::MOTOR_VENDOR_XXX &&
    group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) {
    auto protocol = std::make_shared<XxxEthercatMotorProtocol>(ethercat_bus_runtime);

    std::vector<std::shared_ptr<AbstractMotor>> motors;
    motors.reserve(motor_cfgs.size());
    for (const auto& cfg : motor_cfgs) {
        auto motor = std::make_shared<XxxEthercatMotor>(cfg);
        motor->setProtocol(protocol);
        if (!motor->init()) {
            return {};
        }
        motors.push_back(std::move(motor));
    }
    return motors;
}
```

## 8. 加入 CMake

修改：

```text
cmvr-es/devices/motor/manager/CMakeLists.txt
```

给 `motor_manager` 链接新增的具体 EtherCAT 电机 target。

修改：

```text
cmvr-es/devices/motor/CMakeLists.txt
```

加入：

```cmake
add_subdirectory(drivers/xxx_ethercat)
```

## 9. 验证配置

先验证 proto：

```bash
./output/bin/protoc \
  --encode=cmvr.config.MotorRootConfig \
  -I protos \
  protos/cmvr/config/motor_config/motor_config.proto \
  < cmvr-es/config/devices/motor/ethercat_motors.pb.txt \
  > /tmp/ethercat_motors.pb.bin
```

再编译：

```bash
cmake --build cmake-build-debug --target mujoco_manual_ui_test
```

真机联调时先只验证初始化日志：master 打开、slave 数量、vendor/product 校验、cyclic loop 启动、每个 motor 注册成功。然后再下发运动命令。