lgv abe69c46f0 fix(eyou): make zero calibration transactional

2026-07-30 14:11:38 +08:00

6.7 KiB

Raw Blame History

EtherCAT 电机接入教程

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

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

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

1. 系统依赖

IgH EtherCAT userspace 已安装在：

dependency/x86/third_party/ethercat/v1.7.0

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

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

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

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

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

停止 EtherCAT：

sudo script/ethercat/stop_ethercat.sh eno1

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

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

2. 电机配置

新增或修改：

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

示例：

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. 注册设备

修改：

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

加入：

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

4. 机械臂使用 EtherCAT 电机

修改机械臂配置中的 motor backend：

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 总线运行时：

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 电机驱动：

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 私有适配：

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. 编译验证

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

真机验证顺序：

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

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

6.7 KiB Raw Blame History Unescape Escape