# EtherCAT 电机接入教程

当前已接入意优 `EYOU_ServoModule_ECAT_V145`，协议为 EtherCAT CoE + CiA402，PDO 使用 `docs/EYOU_ServoModule_ECAT_V145_no_slot.xml` 中的 `0x1600/0x1A00` 映射。

ESI XML 作为厂商通信说明和对照资料保存，运行时不直接解析 XML。实际 PDO 映射写在：

```text
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h
```

## 1. 系统依赖

IgH EtherCAT userspace 已安装在：

```text
dependency/x86/third_party/ethercat/v1.7.0
```

真机运行前，系统里还需要安装/加载 IgH master 内核模块。使用仓库脚本启动 EtherCAT master：

```bash
sudo script/ethercat/start_ethercat.sh eno1
script/ethercat/status_ethercat.sh
```

其中 `eno1` 是连接 EtherCAT 从站的网卡。脚本会读取该网卡 MAC，写入：

```text
dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf
```

并通过 bundled IgH 的 `ethercatctl -c` 启动 master。能看到 EYOU slave 后再启动程序。

停止 EtherCAT：

```bash
sudo script/ethercat/stop_ethercat.sh eno1
```

如果这张网卡要恢复给普通网络使用：

```bash
sudo script/ethercat/stop_ethercat.sh eno1 --restore-network
```

## 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_EYOU
    protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402

    ethercat {
      master_index: 0
      cycle_us: 1000

      cia402 {
        profile_position_trigger_delay_ms: 2
        state_transition_timeout_ms: 1200
        velocity_stop_timeout_ms: 2000
        status_poll_period_ms: 10
        stopped_velocity_tolerance_rad_s: 0.001
      }

      zero_calibration {
        timeout_ms: 2000
        poll_period_ms: 10
        stable_sample_count: 5
        position_tolerance_counts: 10000
        stable_delta_counts: 1000
      }

      slaves { motor_id: 1 alias: 0 position: 0 }
      slaves { motor_id: 2 alias: 0 position: 1 }
      slaves { motor_id: 3 alias: 0 position: 2 }
      slaves { motor_id: 4 alias: 0 position: 3 }
      slaves { motor_id: 5 alias: 0 position: 4 }
      slaves { motor_id: 6 alias: 0 position: 5 }
      slaves { motor_id: 7 alias: 0 position: 6 }
    }

    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" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
      motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
      motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
      motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
      motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
      motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
      motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
    }
  }
}
```

字段说明：

- `master_index`：IgH master 编号，第一张 EtherCAT master 是 `0`。
- `alias`：EtherCAT alias，一般为 `0`。
- `position`：slave 在 EtherCAT 链路上的顺序，从 `0` 开始。
- `motor_id`：系统内电机 id，必须和 `motors.motors.id` 对应。
- `encoder_counts_per_rev`：电机编码器每转 count 数，用于 `rad <-> count` 换算。
- `gear_ratio`：电机轴到关节输出轴的减速比，用于接口层 `rad/rad/s` 和驱动器 raw count/counts/s 换算。

## 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 电机

修改机械臂配置中的 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. 代码结构

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
```

它负责：

- 打开 IgH master
- 创建 domain
- 使用 `vendor + protocol` 选择 PDO mapping
- 按 mapping 配置 `0x1600/0x1A00` PDO
- 注册每个非 padding PDO entry 的 offset
- 启动 cyclic loop
- 按 `motor_id + index + subindex` 提供通用 PDO 读写接口

CiA402 EtherCAT 电机驱动：

```text
cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h
cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h
cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp
```

EYOU 私有适配：

```text
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp
```

它负责：

- CiA402 `6040/6041` 状态机
- 设置 `6060` 运行模式
- 写 `607A/60FF/6071`
- 读 `6064/606C/6077/603F`
- `rad` 和 encoder count 的换算

## 6. 编译验证

```bash
cmake -S . -B cmake-build-debug
cmake --build cmake-build-debug --target motor_manager
```

真机验证顺序：

```bash
sudo script/ethercat/start_ethercat.sh eno1
script/ethercat/status_ethercat.sh
cmake --build cmake-build-debug --target cmvr_es
```

第一次联调先不要大幅度运动。先看 master 是否打开、slave 是否进入 OP、状态字是否更新，再给单个电机小角度目标。