diff --git a/CMakeLists.txt b/CMakeLists.txt index ca7498b2..7c168288 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,9 @@ set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/output" CACHE PATH "" FORCE) set(CMAKE_BUILD_RPATH "\$ORIGIN:\$ORIGIN/../lib") set(CMAKE_INSTALL_RPATH "\$ORIGIN:\$ORIGIN/../lib") -# Use RUNPATH (new dtags) generally preferable +# Use transitive RPATH so CLion can run build-tree test executables without +# manually setting LD_LIBRARY_PATH for indirect third-party dependencies. +add_link_options(-Wl,--disable-new-dtags) set(CMAKE_BUILD_WITH_INSTALL_RPATH OFF) set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) diff --git a/README.md b/README.md index 03b74a5e..1bcb8565 100644 --- a/README.md +++ b/README.md @@ -1,23 +1,25 @@ # CMVR-ES -## Overview +## 简介 -## Installation +CMVR-ES 工程。 -### 1. Git submodules install +## 安装 + +### 1. 拉取 Git 子模块 ``` git submodule update --init --recursive ``` -### 2. Dependency install +### 2. 安装系统依赖 ```shell -# basic +# 基础工具 sudo apt-get update sudo apt install -y build-essential cmake git pkg-config patchelf libboost-all-dev libssl-dev -# opencv +# OpenCV sudo apt install -y \ libjpeg-dev libpng-dev libtiff-dev \ libavcodec-dev libavformat-dev libswscale-dev \ @@ -39,7 +41,7 @@ sudo apt-get install libassimp-dev # visp sudo apt-get install -y libx11-dev liblapack-dev libzbar-dev libpthread-stubs0-dev libdc1394-dev nlohmann-json3-dev -# realsense +# RealSense sudo apt-get install -y \ libusb-1.0-0-dev libudev-dev \ libglu1-mesa-dev @@ -48,4 +50,91 @@ sudo apt-get install -y \ sudo apt install gnuplot-qt ``` +### 3. 配置 IgH EtherCAT +工程已内置 IgH EtherCAT 1.7.0 的 userspace 文件： + +```text +dependency/x86/third_party/ethercat/v1.7.0 +``` + +先设置本机路径： + +```shell +export CMVR_ES_ROOT=/path/to/cmvr-es +export IGH_ETHERCAT_ROOT=$CMVR_ES_ROOT/dependency/x86/third_party/ethercat/v1.7.0 +``` + +安装当前内核的 header： + +```shell +sudo apt-get update +sudo apt-get install -y linux-headers-$(uname -r) +``` + +如果内置目录里已经有当前内核版本的 EtherCAT 内核模块，直接安装到系统： + +```shell +sudo mkdir -p /lib/modules/$(uname -r)/ethercat +sudo cp -r $IGH_ETHERCAT_ROOT/lib/modules/$(uname -r)/ethercat/* /lib/modules/$(uname -r)/ethercat/ +sudo depmod +``` + +内置内核模块只适用于相同内核版本。如果 `$IGH_ETHERCAT_ROOT/lib/modules/$(uname -r)` 不存在，说明这台机器的内核版本不匹配，需要在这台机器上重新编译安装 IgH EtherCAT： + +```shell +sudo apt-get update +sudo apt-get install -y \ + build-essential autoconf automake libtool pkg-config git \ + linux-headers-$(uname -r) + +cd /tmp +git clone --branch stable-1.7 --depth 1 https://gitlab.com/etherlab.org/ethercat.git ethercat-stable-1.7 +cd ethercat-stable-1.7 + +./bootstrap +./configure \ + --prefix=$IGH_ETHERCAT_ROOT \ + --libdir=$IGH_ETHERCAT_ROOT/lib \ + --includedir=$IGH_ETHERCAT_ROOT/include \ + --sysconfdir=$IGH_ETHERCAT_ROOT/etc \ + --with-systemdsystemunitdir=$IGH_ETHERCAT_ROOT/lib/systemd/system \ + --enable-generic \ + --with-linux-dir=/lib/modules/$(uname -r)/build + +make -j$(nproc) all modules +make install +sudo make modules_install +sudo depmod +``` + +启动 EtherCAT。`eno1` 换成实际连接 EtherCAT 从站的网卡： + +```shell +sudo script/ethercat/start_ethercat.sh eno1 +``` + +脚本会写入内置 IgH 配置文件： + +```text +$IGH_ETHERCAT_ROOT/etc/ethercat.conf +``` + +脚本会把该网卡的 MAC 写到 `MASTER0_DEVICE`，使用 `DEVICE_MODULES="generic"`，把网卡从 NetworkManager 断开，并通过 `ethercatctl -c` 启动 IgH master。 + +查看状态： + +```shell +script/ethercat/status_ethercat.sh + +$IGH_ETHERCAT_ROOT/bin/ethercat master +$IGH_ETHERCAT_ROOT/bin/ethercat slaves +$IGH_ETHERCAT_ROOT/bin/ethercat pdos +``` + +停止 EtherCAT： + +```shell +sudo script/ethercat/stop_ethercat.sh eno1 +sudo script/ethercat/stop_ethercat.sh eno1 --restore-network +``` diff --git a/cmvr-es/config/devices/motor/ethercat_motors.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt new file mode 100644 index 00000000..c49936e0 --- /dev/null +++ b/cmvr-es/config/devices/motor/ethercat_motors.pb.txt @@ -0,0 +1,65 @@ +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 + slave_op_timeout_ms: 12000 + slave_state_poll_period_ms: 10 + + 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 + } + + dc { + enable: true + reference_motor_id: 1 + sync0_cycle_us: 1000 + sync0_shift_us: 0 + sync_reference_clock_period: 1 + assign_activate: 768 + sync_monitor_period_ms: 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 } + } + } +} diff --git a/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt b/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt new file mode 100644 index 00000000..c473c5ce --- /dev/null +++ b/cmvr-es/config/devices/motor/ethercat_motors_two_real_test.pb.txt @@ -0,0 +1,56 @@ +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 + slave_op_timeout_ms: 15000 + slave_state_poll_period_ms: 10 + + 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 + } + + dc { + enable: false + reference_motor_id: 1 + sync0_cycle_us: 1000 + sync0_shift_us: 0 + sync_reference_clock_period: 1 + assign_activate: 768 + sync_monitor_period_ms: 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 } + } + + 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 } + } + + 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 } + } + } +} diff --git a/cmvr-es/config/devices/motor/mujoco_motors.pb.txt b/cmvr-es/config/devices/motor/mujoco_motors.pb.txt index 4ebaac09..7271eba6 100644 --- a/cmvr-es/config/devices/motor/mujoco_motors.pb.txt +++ b/cmvr-es/config/devices/motor/mujoco_motors.pb.txt @@ -6,7 +6,6 @@ motor { bus_type: MOTOR_BUS_MUJOCO vendor: MOTOR_VENDOR_MUJOCO protocol: MOTOR_PROTOCOL_MUJOCO - tool_frame: "R_FINGER_TIP" mujoco { world_id: "mujoco_world" } diff --git a/cmvr-es/config/devices/motor/ti5_motors.pb.txt b/cmvr-es/config/devices/motor/ti5_motors.pb.txt index ca50e0ec..20a28aff 100644 --- a/cmvr-es/config/devices/motor/ti5_motors.pb.txt +++ b/cmvr-es/config/devices/motor/ti5_motors.pb.txt @@ -6,7 +6,6 @@ motor { bus_type: MOTOR_BUS_CAN vendor: MOTOR_VENDOR_TI5 protocol: MOTOR_PROTOCOL_CANOPEN - tool_frame: "L_FINGER_TIP" can { channel_id: 0 } @@ -16,13 +15,13 @@ motor { urdf_path: "model/xiaoyan_description/dual_arm.urdf" } motors { - motors { id: 23 joint_name: "L_SHOULDER_P" } - motors { id: 24 joint_name: "L_SHOULDER_R" } - motors { id: 25 joint_name: "L_SHOULDER_Y" } - motors { id: 26 joint_name: "L_ELBOW_R" } - motors { id: 27 joint_name: "L_WRIST_P" } - motors { id: 28 joint_name: "L_WRIST_Y" } - motors { id: 29 joint_name: "L_WRIST_R" } + motors { id: 23 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 24 joint_name: "L_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 25 joint_name: "L_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 26 joint_name: "L_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 27 joint_name: "L_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 28 joint_name: "L_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 29 joint_name: "L_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } @@ -31,7 +30,6 @@ motor { bus_type: MOTOR_BUS_CAN vendor: MOTOR_VENDOR_TI5 protocol: MOTOR_PROTOCOL_CANOPEN - tool_frame: "R_FINGER_TIP" can { channel_id: 1 } @@ -47,13 +45,13 @@ motor { joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 } } motors { - motors { id: 16 joint_name: "R_SHOULDER_P" } - motors { id: 17 joint_name: "R_SHOULDER_R" } - motors { id: 18 joint_name: "R_SHOULDER_Y" } - motors { id: 19 joint_name: "R_ELBOW_R" } - motors { id: 20 joint_name: "R_WRIST_P" } - motors { id: 21 joint_name: "R_WRIST_Y" } - motors { id: 22 joint_name: "R_WRIST_R" } + motors { id: 16 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 17 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 18 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 19 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 20 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 21 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 22 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } @@ -73,9 +71,9 @@ motor { joints { joint_name: "HEAD_R" q_lb: -3.14 q_ub: 3.14 qd: 3.0 } } motors { - motors { id: 32 joint_name: "HEAD_Y" } - motors { id: 30 joint_name: "HEAD_P" } - motors { id: 31 joint_name: "HEAD_R" } + motors { id: 32 joint_name: "HEAD_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 30 joint_name: "HEAD_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 31 joint_name: "HEAD_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } @@ -94,8 +92,8 @@ motor { joints { joint_name: "WAIST_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 } } motors { - motors { id: 4 joint_name: "WAIST_Y" } - motors { id: 15 joint_name: "WAIST_P" } + motors { id: 4 joint_name: "WAIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } + motors { id: 15 joint_name: "WAIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 } } } } diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 54cd741b..d9ad1d68 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -2,6 +2,7 @@ device_manager { name: "cmvr_es" version: "0.1" description: "cmvr edge system version 0.1" + init_all_motors_when_no_active_joints: true devices { id: "mujoco_world" @@ -74,6 +75,13 @@ device_manager { enable: false } + devices { + id: "ethercat_motors" + type: DEVICE_TYPE_MOTOR_SYSTEM + config_file: "devices/motor/ethercat_motors.pb.txt" + enable: false + } + devices { id: "right_arm" type: DEVICE_TYPE_ROBOT_ARM diff --git a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h index 7a0af759..35471e29 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h +++ b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h @@ -73,7 +73,7 @@ public: bool isConnected() const override { return motor_manager_ != nullptr; } Result powerOn() override { return torqueOn(); } Result powerOff() override { return torqueOff(); } - Result brakeRelease() override { return torqueOn(); } + Result brakeRelease() override; Result shutdown() override; Result clearFault() override { return Result::success(); } Result unlockProtectiveStop() override { return Result::success(); } diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index 77e8abbc..5a5db3a3 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp @@ -196,7 +196,10 @@ Result MotorRobotArm::torqueOn() if (!motor) { return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); } - motor->brake(); + if (!motor->torqueOn()) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to torque on motor for joint: " + joint_name); + } } emergency_stopped_ = false; return Result::success(); @@ -209,7 +212,26 @@ Result MotorRobotArm::torqueOff() if (!motor) { return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); } - motor->torqueOff(); + if (!motor->torqueOff() || !motor->brakeRelease()) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to torque off motor for joint: " + joint_name); + } + } + return Result::success(); +} + +Result MotorRobotArm::brakeRelease() +{ + for (const auto& joint_name : joint_names_) { + auto motor = getMotor_(joint_name); + if (!motor) { + return Result::failure(ArmErrorCode::RobotNotReady, + "motor not found for joint: " + joint_name); + } + if (!motor->brakeRelease()) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to release brake for joint: " + joint_name); + } } return Result::success(); } @@ -238,7 +260,10 @@ Result MotorRobotArm::emergencyStop() if (!motor) { return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name); } - motor->brake(); + if (!motor->quickStop()) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to quick stop motor for joint: " + joint_name); + } } emergency_stopped_ = true; return Result::success(); @@ -298,7 +323,11 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti } for (std::size_t i = 0; i < motors.size(); ++i) { const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0; - motors[i]->setTarget(sample.position[i], qd); + if (!motors[i]->commandCyclicPosition(sample.position[i], qd)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to command cyclic position for joint: " + + motors[i]->jointName()); + } } if (k + 1 < samples.size()) { const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t @@ -331,7 +360,11 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity, if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); } - motor->setTarget(velocity.velocity[i] * speed_scaling_); + if (!motor->commandCyclicVelocity(velocity.velocity[i] * speed_scaling_)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to command cyclic velocity for joint: " + + joint_names_[i]); + } } } @@ -457,7 +490,11 @@ Result MotorRobotArm::servoJ(const JointPositionCommand& target) if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); } - motor->setTarget(target.position[i], 0.0); + if (!motor->commandCyclicPosition(target.position[i], 0.0)) { + return Result::failure(ArmErrorCode::CommandFailed, + "failed to command cyclic position for joint: " + + joint_names_[i]); + } } return Result::success(); } @@ -740,7 +777,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj return false; } for (std::size_t j = 0; j < motors.size(); ++j) { - motors[j]->setTarget(position[j], velocity[j]); + if (!motors[j]->commandCyclicPosition(position[j], velocity[j])) { + return false; + } } next_deadline += std::chrono::duration_cast( std::chrono::duration(dt_segment)); diff --git a/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h b/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h index 69366ef4..6dfd8374 100644 --- a/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h +++ b/cmvr-es/devices/canbus/canopen/sdo_request_protocol.h @@ -30,7 +30,7 @@ namespace cmvr { return BASE_ID + sdo_frame_.node_id(); } - void SetFrameData(msgs::CommandSpecifier cs, msgs::ObIndex index,msgs::ObSubIndex sub_index, uint32_t data) { + void SetFrameData(msgs::CommandSpecifier cs, uint32_t index, uint32_t sub_index, uint32_t data) { std::lock_guard lock(mutex_); sdo_frame_.set_cs(cs); sdo_frame_.set_index(index); diff --git a/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h b/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h index 0a48af3e..ca78acd4 100644 --- a/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h +++ b/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h @@ -49,10 +49,10 @@ namespace cmvr { auto command = static_cast(bytes[0]); // 解析 index（字节1和字节2，低字节优先） - auto index = static_cast(bytes[1] + (bytes[2] << 8)); + const uint32_t index = bytes[1] + (bytes[2] << 8); // 解析 subindex（字节3） - auto subindex = static_cast(bytes[3]); + const uint32_t subindex = bytes[3]; // 根据 command 解析 data（字节4~7） uint32_t data = 0; @@ -94,4 +94,4 @@ namespace cmvr { ParseSdoData(sdo_response_, sensor_data); } } -} \ No newline at end of file +} diff --git a/cmvr-es/devices/motor/CMakeLists.txt b/cmvr-es/devices/motor/CMakeLists.txt index 76689b2f..c3081330 100644 --- a/cmvr-es/devices/motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/CMakeLists.txt @@ -1,6 +1,10 @@ add_library(motor_core INTERFACE) -target_include_directories(motor_core INTERFACE ${CMAKE_SOURCE_DIR}/cmvr-es/devices) +target_include_directories(motor_core + INTERFACE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/devices +) target_link_libraries(motor_core INTERFACE @@ -12,4 +16,5 @@ add_library(cmvr_es::device::motor_core ALIAS motor_core) add_subdirectory(drivers/ti5_canopen) add_subdirectory(drivers/mujoco) add_subdirectory(bus_runtime) +add_subdirectory(drivers/ethercat_motor) add_subdirectory(manager) diff --git a/cmvr-es/devices/motor/abstract_motor.h b/cmvr-es/devices/motor/abstract_motor.h index 983ccec7..35eee16d 100644 --- a/cmvr-es/devices/motor/abstract_motor.h +++ b/cmvr-es/devices/motor/abstract_motor.h @@ -66,13 +66,22 @@ namespace cmvr::device{ return protocol_->getMode(node_id_); } - virtual void torqueOff() { + virtual bool torqueOn() { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->torqueOff(node_id_); + return protocol_->torqueOn(node_id_); + } + + virtual bool torqueOff() { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->torqueOff(node_id_); } virtual void setLimitQ(double ub, double lb) { @@ -101,43 +110,69 @@ namespace cmvr::device{ } // virtual void setLimitTau(double tau) = 0; // virtual void setLimitCurrent(double tau) = 0; - virtual void brake() { + virtual bool brakeRelease() { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->brake(node_id_); - } - /** - * - * @param q unit : rad - */ - virtual void setQ(double q) { - std::scoped_lock lock(mtx_); - if (!protocol_) { - CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; - } - protocol_->setQ(node_id_, q); + return protocol_->brakeRelease(node_id_); } - virtual void setTarget(double q,double qd) { + virtual bool quickStop() { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->setTarget(node_id_,q, qd); + return protocol_->quickStop(node_id_); + } + virtual bool commandProfilePosition(double target_q, + double max_qd = 0.0, + double max_qdd = 0.0) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->commandProfilePosition(node_id_, target_q, max_qd, max_qdd); } - virtual void setTarget(double qd) { + virtual bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) { std::scoped_lock lock(mtx_); if (!protocol_) { CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; + return false; } - protocol_->setTarget(node_id_, qd); + return protocol_->commandProfileVelocity(node_id_, target_qd, max_qdd); + } + + virtual bool commandCyclicPosition(double target_q, + double target_qd = 0.0) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->commandCyclicPosition(node_id_, target_q, target_qd); + } + + virtual bool commandCyclicVelocity(double target_qd) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->commandCyclicVelocity(node_id_, target_qd); + } + + virtual bool commandCyclicTorque(double target_tau) { + std::scoped_lock lock(mtx_); + if (!protocol_) { + CMVR_LOG(ERROR) << "Protocol not set for motor"; + return false; + } + return protocol_->commandCyclicTorque(node_id_, target_tau); } virtual bool calibrateZeroQ() { @@ -156,16 +191,6 @@ namespace cmvr::device{ } return protocol_->reachedTargetQ(node_id_); } - // rad /s - virtual void setQd(double qd) { - std::scoped_lock lock(mtx_); - if (!protocol_) { - CMVR_LOG(ERROR) << "Protocol not set for motor"; - return; - } - return protocol_->setQd(node_id_,qd); - } - // virtual void setQdd(double qdd) = 0; // rad /s^2 // virtual void setTau(double tau) = 0; // N m // virtual void clear_err() = 0; // virtual void getStatus() = 0; diff --git a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt index 238d1841..ce09edab 100644 --- a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt +++ b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt @@ -4,7 +4,18 @@ add_library(motor_bus_runtime SHARED ethercat/src/ethercat_motor_bus_runtime.cpp ) -target_include_directories(motor_bus_runtime PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +set(IGH_ETHERCAT_ROOT + ${CMAKE_SOURCE_DIR}/dependency/x86/third_party/ethercat/v1.7.0 +) + +target_include_directories(motor_bus_runtime + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} + PRIVATE + ${IGH_ETHERCAT_ROOT}/include +) + +target_link_directories(motor_bus_runtime PRIVATE ${IGH_ETHERCAT_ROOT}/lib) target_link_libraries(motor_bus_runtime PUBLIC @@ -12,9 +23,28 @@ target_link_libraries(motor_bus_runtime cmvr_es::device::motor_core cmvr_es::mujoco_world PRIVATE + ethercat cmvr_es::device::canbus glog ) add_library(cmvr_es::device::motor_bus_runtime ALIAS motor_bus_runtime) install(TARGETS motor_bus_runtime LIBRARY DESTINATION lib) + +add_executable(ethercat_motor_bus_runtime_real_test + ethercat/src/ethercat_motor_bus_runtime_real_test.cpp +) + +target_include_directories(ethercat_motor_bus_runtime_real_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es +) + +target_link_libraries(ethercat_motor_bus_runtime_real_test + PRIVATE + cmvr_es::device::motor_bus_runtime + gtest + gtest_main + pthread + glog +) diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h index 5d7f8edf..5b2c7607 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h @@ -1,10 +1,23 @@ #ifndef CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H #define CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H +#include +#include +#include +#include +#include #include +#include +#include #include +#include #include "../../abstract_motor_bus_runtime.h" +#include "motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h" + +typedef struct ec_domain ec_domain_t; +typedef struct ec_master ec_master_t; +typedef struct ec_slave_config ec_slave_config_t; namespace cmvr::device { @@ -17,12 +30,136 @@ public: const std::string& id() const { return id_; } const config::EtherCATConfig& config() const { return config_; } + void setPdoMapping(EthercatPdoMapping mapping); + const EthercatPdoMapping& pdoMapping() const { return pdo_mapping_; } const config::EthercatSlaveConfig* slaveForMotor(int motor_id) const; + bool hasMotor(int motor_id) const; + + bool hasPdoEntry(int motor_id, std::uint16_t index, std::uint8_t subindex) const; + + template + bool writePdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T value) + { + return writePdoRaw_(motor_id, index, subindex, valueBitLength_(), + toRawValue_(value)); + } + + template + bool readPdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value) const + { + std::uint64_t raw = 0; + if (!readPdoRaw_(motor_id, index, subindex, valueBitLength_(), raw)) { + return false; + } + value = fromRawValue_(raw); + return true; + } + + template + bool writeSdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T value) + { + return writeSdoRaw_(motor_id, index, subindex, valueBitLength_(), + toRawValue_(value)); + } + + template + bool readSdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value) + { + std::uint64_t raw = 0; + if (!readSdoRaw_(motor_id, index, subindex, valueBitLength_(), raw)) { + return false; + } + value = fromRawValue_(raw); + return true; + } private: + struct PdoEntryRuntime { + EthercatPdoEntryConfig cfg; + unsigned int offset{0}; + bool rx{false}; + std::uint64_t value{0}; + }; + + struct SlaveRuntime { + config::EthercatSlaveConfig cfg; + ec_slave_config_t* slave_config{nullptr}; + std::unordered_map pdo_entries; + }; + + bool configureSlave_(SlaveRuntime& slave); + bool configureDc_(); + bool waitSlavesOperational_(); + void cyclicLoop_(); + void readFeedbackLocked_(); + void writeCommandsLocked_(); + void releaseMaster_(); + bool hasValidPdoMapping_() const; + bool writePdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex, + std::uint8_t bit_len, std::uint64_t value); + bool readPdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex, + std::uint8_t bit_len, std::uint64_t& value) const; + bool writeSdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex, + std::uint8_t bit_len, std::uint64_t value); + bool readSdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex, + std::uint8_t bit_len, std::uint64_t& value); + + template + static constexpr std::uint8_t valueBitLength_() + { + using ValueType = std::remove_cv_t; + static_assert(std::is_integral_v, "EtherCAT object values must be integral"); + static_assert(!std::is_same_v, "bool is not a valid EtherCAT object value"); + static_assert(sizeof(ValueType) == 1 || sizeof(ValueType) == 2 || sizeof(ValueType) == 4, + "only 8/16/32-bit EtherCAT object values are supported"); + return static_cast(sizeof(ValueType) * 8); + } + + template + static std::uint64_t toRawValue_(T value) + { + using ValueType = std::remove_cv_t; + using UnsignedType = std::make_unsigned_t; + return static_cast(static_cast(value)); + } + + template + static T fromRawValue_(std::uint64_t raw) + { + using ValueType = std::remove_cv_t; + using UnsignedType = std::make_unsigned_t; + const auto unsigned_value = static_cast(raw); + ValueType value{}; + std::memcpy(&value, &unsigned_value, sizeof(ValueType)); + return value; + } + + static std::uint32_t pdoEntryKey_(std::uint16_t index, std::uint8_t subindex); + static std::string hexIndex_(std::uint32_t index); + static std::uint64_t maskValue_(std::uint64_t value, std::uint8_t bit_len); + static bool isSupportedBitLength_(std::uint8_t bit_len); + static std::uint64_t readEntryValue_(const std::uint8_t* domain_data, + const PdoEntryRuntime& entry); + static void writeEntryValue_(std::uint8_t* domain_data, + const PdoEntryRuntime& entry); + static std::uint64_t steadyTimeNs_(); + static std::uint64_t timePointNs_(std::chrono::steady_clock::time_point time_point); + static std::uint32_t usToNs_(std::uint32_t value_us); + static std::int32_t usToNs_(std::int32_t value_us); + std::string id_; config::EtherCATConfig config_; - std::unordered_map slaves_by_motor_id_; + EthercatPdoMapping pdo_mapping_; + std::unordered_map slaves_by_motor_id_; + + ec_master_t* master_{nullptr}; + ec_domain_t* domain_{nullptr}; + std::uint8_t* domain_data_{nullptr}; + + mutable std::mutex data_mutex_; + std::thread cyclic_thread_; + std::atomic running_{false}; + bool initialized_{false}; bool started_{false}; }; diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h new file mode 100644 index 00000000..30db0781 --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h @@ -0,0 +1,35 @@ +#ifndef CMVR_ES_ETHERCAT_PDO_MAPPING_H +#define CMVR_ES_ETHERCAT_PDO_MAPPING_H + +#include +#include +#include + +namespace cmvr::device { + +struct EthercatPdoEntryConfig { + std::uint16_t index{0}; + std::uint8_t subindex{0}; + std::uint8_t bit_len{0}; + std::string name; + bool padding{false}; +}; + +struct EthercatPdoConfig { + std::uint16_t index{0}; + std::uint8_t sync_manager{0}; + bool rx{false}; + std::vector entries; +}; + +struct EthercatPdoMapping { + std::uint32_t vendor_id{0}; + std::uint32_t product_code{0}; + std::string name; + std::vector rx_pdos; + std::vector tx_pdos; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_ETHERCAT_PDO_MAPPING_H diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp index 45c44243..4b5554c5 100644 --- a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp @@ -1,7 +1,18 @@ #include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" +#include +#include +#include +#include +#include +#include +#include +#include + #include "common/base/logging/logger.h" +#include + namespace cmvr::device { bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg) @@ -21,14 +32,34 @@ bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg) } config_ = group_cfg.ethercat(); - if (config_.master_id().empty()) { - CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] master_id is empty: " << id_; - return false; - } if (config_.cycle_us() <= 0) { CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] cycle_us must be positive: " << id_; return false; } + if (!config_.has_slave_op_timeout_ms()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing slave_op_timeout_ms: " << id_; + return false; + } + if (config_.slave_op_timeout_ms() == 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave_op_timeout_ms must be positive: " + << id_; + return false; + } + if (!config_.has_slave_state_poll_period_ms()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing slave_state_poll_period_ms: " + << id_; + return false; + } + if (config_.slave_state_poll_period_ms() == 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave_state_poll_period_ms must be positive: " + << id_; + return false; + } + if (!hasValidPdoMapping_()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] PDO mapping is not configured: " + << id_; + return false; + } slaves_by_motor_id_.clear(); for (const auto& slave : config_.slaves()) { @@ -36,34 +67,106 @@ bool EthercatMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg) CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid motor_id in slave config: " << id_; return false; } - if (slave.slave_index() < 0) { - CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] invalid slave_index for motor " - << slave.motor_id() << " in group: " << id_; - return false; - } if (slaves_by_motor_id_.count(slave.motor_id()) > 0) { CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] duplicate slave motor_id: " << slave.motor_id() << " in group: " << id_; return false; } - slaves_by_motor_id_[slave.motor_id()] = &slave; + SlaveRuntime runtime; + runtime.cfg = slave; + slaves_by_motor_id_.emplace(slave.motor_id(), std::move(runtime)); } + if (slaves_by_motor_id_.empty()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] no EtherCAT slaves configured: " << id_; + return false; + } + + master_ = ecrt_request_master(config_.master_index()); + if (!master_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to request EtherCAT master " + << config_.master_index() << ": " << id_; + return false; + } + + domain_ = ecrt_master_create_domain(master_); + if (!domain_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to create EtherCAT domain: " << id_; + releaseMaster_(); + return false; + } + + for (auto& [motor_id, slave] : slaves_by_motor_id_) { + (void)motor_id; + if (!configureSlave_(slave)) { + releaseMaster_(); + return false; + } + } + + if (!configureDc_()) { + releaseMaster_(); + return false; + } + + initialized_ = true; return true; } +void EthercatMotorBusRuntime::setPdoMapping(EthercatPdoMapping mapping) +{ + pdo_mapping_ = std::move(mapping); +} + bool EthercatMotorBusRuntime::start() { if (started_) { return true; } - CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] EtherCAT master is not implemented yet: " << id_; - return false; + if (!initialized_ || !master_ || !domain_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] runtime is not initialized: " << id_; + return false; + } + + if (ecrt_master_activate(master_) != 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to activate EtherCAT master: " << id_; + return false; + } + + domain_data_ = ecrt_domain_data(domain_); + if (!domain_data_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to get EtherCAT domain data: " << id_; + return false; + } + + { + std::lock_guard lock(data_mutex_); + writeCommandsLocked_(); + } + + running_.store(true); + cyclic_thread_ = std::thread(&EthercatMotorBusRuntime::cyclicLoop_, this); + started_ = true; + + if (!waitSlavesOperational_()) { + stop(); + return false; + } + + CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] started EtherCAT runtime: " << id_; + return true; } void EthercatMotorBusRuntime::stop() { + running_.store(false); + if (cyclic_thread_.joinable()) { + cyclic_thread_.join(); + } started_ = false; + initialized_ = false; + domain_data_ = nullptr; + releaseMaster_(); } const config::EthercatSlaveConfig* EthercatMotorBusRuntime::slaveForMotor(const int motor_id) const @@ -72,7 +175,780 @@ const config::EthercatSlaveConfig* EthercatMotorBusRuntime::slaveForMotor(const if (it == slaves_by_motor_id_.end()) { return nullptr; } - return it->second; + return &it->second.cfg; +} + +bool EthercatMotorBusRuntime::hasMotor(const int motor_id) const +{ + return slaves_by_motor_id_.count(motor_id) > 0; +} + +bool EthercatMotorBusRuntime::hasPdoEntry(const int motor_id, + const std::uint16_t index, + const std::uint8_t subindex) const +{ + std::lock_guard lock(data_mutex_); + const auto slave_it = slaves_by_motor_id_.find(motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + return false; + } + return slave_it->second.pdo_entries.count(pdoEntryKey_(index, subindex)) > 0; +} + +bool EthercatMotorBusRuntime::configureSlave_(SlaveRuntime& slave) +{ + slave.slave_config = ecrt_master_slave_config(master_, + static_cast(slave.cfg.alias()), + static_cast(slave.cfg.position()), + pdo_mapping_.vendor_id, + pdo_mapping_.product_code); + if (!slave.slave_config) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to get slave config: group=" << id_ + << ", motor_id=" << slave.cfg.motor_id() + << ", alias=" << slave.cfg.alias() + << ", position=" << slave.cfg.position() + << ", vendor=" << hexIndex_(pdo_mapping_.vendor_id) + << ", product=" << hexIndex_(pdo_mapping_.product_code); + return false; + } + + struct SyncBuild { + bool rx{false}; + std::vector pdo_indices; + std::vector> entry_storage; + std::vector pdo_infos; + }; + + auto add_pdo_to_sync_build = [](std::map& builds, + const EthercatPdoConfig& pdo) { + auto& build = builds[pdo.sync_manager]; + build.rx = pdo.rx; + build.pdo_indices.push_back(pdo.index); + auto& entries = build.entry_storage.emplace_back(); + entries.reserve(pdo.entries.size()); + for (const auto& entry : pdo.entries) { + entries.push_back({entry.index, entry.subindex, entry.bit_len}); + } + }; + + std::map sync_builds; + for (const auto& pdo : pdo_mapping_.rx_pdos) { + add_pdo_to_sync_build(sync_builds, pdo); + } + for (const auto& pdo : pdo_mapping_.tx_pdos) { + add_pdo_to_sync_build(sync_builds, pdo); + } + + for (auto& [sync_manager, build] : sync_builds) { + (void)sync_manager; + build.pdo_infos.reserve(build.entry_storage.size()); + for (std::size_t i = 0; i < build.entry_storage.size(); ++i) { + auto& entries = build.entry_storage[i]; + build.pdo_infos.push_back({ + build.pdo_indices[i], + static_cast(entries.size()), + entries.data(), + }); + } + } + + std::vector sync_infos; + sync_infos.push_back({0, EC_DIR_OUTPUT, 0, nullptr, EC_WD_DISABLE}); + sync_infos.push_back({1, EC_DIR_INPUT, 0, nullptr, EC_WD_DISABLE}); + for (auto& [sync_manager, build] : sync_builds) { + const auto direction = build.rx ? EC_DIR_OUTPUT : EC_DIR_INPUT; + const auto watchdog = build.rx ? EC_WD_ENABLE : EC_WD_DISABLE; + sync_infos.push_back({ + sync_manager, + direction, + static_cast(build.pdo_infos.size()), + build.pdo_infos.data(), + watchdog, + }); + } + sync_infos.push_back({0xff}); + + if (ecrt_slave_config_pdos(slave.slave_config, EC_END, sync_infos.data()) != 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to configure PDOs: group=" << id_ + << ", motor_id=" << slave.cfg.motor_id() + << ", position=" << slave.cfg.position(); + return false; + } + + auto register_entry = [&](const EthercatPdoEntryConfig& entry, const bool rx) -> bool { + if (entry.padding) { + return true; + } + if (!isSupportedBitLength_(entry.bit_len)) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] unsupported PDO entry bit length: " + << static_cast(entry.bit_len) + << ", entry=" << hexIndex_(entry.index) + << ":" << static_cast(entry.subindex) + << ", motor_id=" << slave.cfg.motor_id() + << ", group=" << id_; + return false; + } + + const auto key = pdoEntryKey_(entry.index, entry.subindex); + if (slave.pdo_entries.count(key) > 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] duplicate PDO entry: " + << hexIndex_(entry.index) + << ":" << static_cast(entry.subindex) + << ", motor_id=" << slave.cfg.motor_id() + << ", group=" << id_; + return false; + } + + const int result = ecrt_slave_config_reg_pdo_entry( + slave.slave_config, entry.index, entry.subindex, domain_, nullptr); + if (result < 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to register PDO entry " + << hexIndex_(entry.index) << ":" << static_cast(entry.subindex) + << ", motor_id=" << slave.cfg.motor_id() + << ", group=" << id_; + return false; + } + + PdoEntryRuntime runtime; + runtime.cfg = entry; + runtime.offset = static_cast(result); + runtime.rx = rx; + slave.pdo_entries.emplace(key, std::move(runtime)); + return true; + }; + + for (const auto& pdo : pdo_mapping_.rx_pdos) { + for (const auto& entry : pdo.entries) { + if (!register_entry(entry, true)) { + return false; + } + } + } + for (const auto& pdo : pdo_mapping_.tx_pdos) { + for (const auto& entry : pdo.entries) { + if (!register_entry(entry, false)) { + return false; + } + } + } + + return true; +} + +bool EthercatMotorBusRuntime::configureDc_() +{ + if (!config_.has_dc()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing explicit DC config: " + << id_ << ". Add dc { enable: false } or a complete enabled DC config."; + return false; + } + + const auto& dc = config_.dc(); + if (!dc.has_enable()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC enable: " << id_; + return false; + } + if (!dc.enable()) { + return true; + } + + if (!dc.has_reference_motor_id()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC reference_motor_id: " + << id_; + return false; + } + if (!dc.has_sync0_cycle_us()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync0_cycle_us: " + << id_; + return false; + } + if (!dc.has_sync0_shift_us()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync0_shift_us: " + << id_; + return false; + } + if (!dc.has_sync_reference_clock_period()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync_reference_clock_period: " + << id_; + return false; + } + if (!dc.has_assign_activate()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC assign_activate: " + << id_; + return false; + } + if (!dc.has_sync_monitor_period_ms()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing DC sync_monitor_period_ms: " + << id_; + return false; + } + if (dc.reference_motor_id() <= 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC reference_motor_id must be positive: " + << id_; + return false; + } + if (dc.sync0_cycle_us() == 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync0_cycle_us must be positive: " + << id_; + return false; + } + if (dc.sync_reference_clock_period() == 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync_reference_clock_period must be positive: " + << id_; + return false; + } + if (dc.assign_activate() == 0 || dc.assign_activate() > 0xFFFFU) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC assign_activate must be in [1, 0xFFFF]: " + << id_ << ", value=" << hexIndex_(dc.assign_activate()); + return false; + } + if (dc.sync_monitor_period_ms() == 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC sync_monitor_period_ms must be positive: " + << id_; + return false; + } + + const auto reference_it = slaves_by_motor_id_.find(dc.reference_motor_id()); + if (reference_it == slaves_by_motor_id_.end() || !reference_it->second.slave_config) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] DC reference motor is not configured: " + << id_ << ", reference_motor_id=" << dc.reference_motor_id(); + return false; + } + + const int select_result = ecrt_master_select_reference_clock( + master_, reference_it->second.slave_config); + if (select_result != 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to select DC reference clock: " + << id_ << ", reference_motor_id=" << dc.reference_motor_id() + << ", result=" << select_result; + return false; + } + + const auto assign_activate = static_cast(dc.assign_activate()); + const auto sync0_cycle_ns = usToNs_(dc.sync0_cycle_us()); + const auto sync0_shift_ns = usToNs_(dc.sync0_shift_us()); + for (auto& [motor_id, slave] : slaves_by_motor_id_) { + const int result = ecrt_slave_config_dc(slave.slave_config, + assign_activate, + sync0_cycle_ns, + sync0_shift_ns, + 0, + 0); + if (result != 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to configure DC: " + << id_ << ", motor_id=" << motor_id + << ", position=" << slave.cfg.position() + << ", result=" << result; + return false; + } + } + + CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] configured DC: " + << id_ + << ", reference_motor_id=" << dc.reference_motor_id() + << ", sync0_cycle_ns=" << sync0_cycle_ns + << ", sync0_shift_ns=" << sync0_shift_ns + << ", sync_reference_clock_period=" << dc.sync_reference_clock_period() + << ", assign_activate=" << hexIndex_(assign_activate) + << ", sync_monitor_period_ms=" << dc.sync_monitor_period_ms(); + return true; +} + +bool EthercatMotorBusRuntime::waitSlavesOperational_() +{ + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(config_.slave_op_timeout_ms()); + const auto poll_period = + std::chrono::milliseconds(config_.slave_state_poll_period_ms()); + + ec_domain_state_t last_domain_state{}; + int last_domain_result = 0; + do { + bool all_slaves_operational = true; + for (const auto& [motor_id, slave] : slaves_by_motor_id_) { + (void)motor_id; + ec_slave_config_state_t state{}; + const int result = ecrt_slave_config_state(slave.slave_config, &state); + if (result != 0 || + !state.online || + !state.operational || + state.al_state != EC_AL_STATE_OP) { + all_slaves_operational = false; + break; + } + } + + last_domain_result = ecrt_domain_state(domain_, &last_domain_state); + const bool domain_complete = + last_domain_result == 0 && + last_domain_state.wc_state == EC_WC_COMPLETE; + + if (all_slaves_operational && domain_complete) { + CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] all EtherCAT slaves operational: " + << id_ + << ", working_counter=" << last_domain_state.working_counter; + return true; + } + + std::this_thread::sleep_for(poll_period); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] timeout waiting for EtherCAT slaves OP: " + << id_ + << ", timeout_ms=" << config_.slave_op_timeout_ms() + << ", domain_result=" << last_domain_result + << ", domain_wc_state=" << static_cast(last_domain_state.wc_state) + << ", working_counter=" << last_domain_state.working_counter; + + for (const auto& [motor_id, slave] : slaves_by_motor_id_) { + ec_slave_config_state_t state{}; + const int result = ecrt_slave_config_state(slave.slave_config, &state); + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] slave state: " + << id_ + << ", motor_id=" << motor_id + << ", position=" << slave.cfg.position() + << ", result=" << result + << ", online=" << state.online + << ", operational=" << state.operational + << ", al_state=" << static_cast(state.al_state); + } + return false; +} + +void EthercatMotorBusRuntime::cyclicLoop_() +{ + const auto period = std::chrono::microseconds(config_.cycle_us()); + const bool dc_enabled = config_.dc().enable(); + const auto dc_sync_period = dc_enabled ? config_.dc().sync_reference_clock_period() : 0U; + const auto dc_monitor_period = + dc_enabled ? std::chrono::milliseconds(config_.dc().sync_monitor_period_ms()) + : std::chrono::milliseconds(0); + std::uint32_t dc_sync_counter = 0; + bool dc_monitor_queued = false; + auto cycle_time = std::chrono::steady_clock::now(); + auto next_dc_monitor_time = cycle_time + dc_monitor_period; + while (running_.load()) { + if (dc_enabled) { + ecrt_master_application_time(master_, timePointNs_(cycle_time)); + } + + ecrt_master_receive(master_); + ecrt_domain_process(domain_); + if (dc_enabled && dc_monitor_queued) { + const std::uint32_t dc_sync_diff_ns = + ecrt_master_sync_monitor_process(master_); + if (dc_sync_diff_ns == static_cast(-1)) { + CMVR_LOG(WARNING) << "[EthercatMotorBusRuntime] DC sync monitor failed: " + << id_; + } else { + CMVR_LOG(INFO) << "[EthercatMotorBusRuntime] dc_sync_diff_ns=" + << dc_sync_diff_ns + << ", group=" << id_; + } + dc_monitor_queued = false; + } + { + std::lock_guard lock(data_mutex_); + readFeedbackLocked_(); + writeCommandsLocked_(); + } + ecrt_domain_queue(domain_); + if (dc_enabled) { + ++dc_sync_counter; + if (dc_sync_counter >= dc_sync_period) { + dc_sync_counter = 0; + ecrt_master_sync_reference_clock(master_); + } + ecrt_master_sync_slave_clocks(master_); + + if (cycle_time >= next_dc_monitor_time) { + const int monitor_result = ecrt_master_sync_monitor_queue(master_); + if (monitor_result == 0) { + dc_monitor_queued = true; + } else { + CMVR_LOG(WARNING) << "[EthercatMotorBusRuntime] failed to queue DC sync monitor: " + << id_ << ", result=" << monitor_result; + } + do { + next_dc_monitor_time += dc_monitor_period; + } while (cycle_time >= next_dc_monitor_time); + } + } + ecrt_master_send(master_); + + cycle_time += period; + std::this_thread::sleep_until(cycle_time); + } +} + +void EthercatMotorBusRuntime::readFeedbackLocked_() +{ + if (!domain_data_) { + return; + } + for (auto& [motor_id, slave] : slaves_by_motor_id_) { + (void)motor_id; + for (auto& [key, entry] : slave.pdo_entries) { + (void)key; + if (!entry.rx) { + entry.value = readEntryValue_(domain_data_, entry); + } + } + } +} + +void EthercatMotorBusRuntime::writeCommandsLocked_() +{ + if (!domain_data_) { + return; + } + for (const auto& [motor_id, slave] : slaves_by_motor_id_) { + (void)motor_id; + for (const auto& [key, entry] : slave.pdo_entries) { + (void)key; + if (entry.rx) { + writeEntryValue_(domain_data_, entry); + } + } + } +} + +void EthercatMotorBusRuntime::releaseMaster_() +{ + if (master_) { + ecrt_release_master(master_); + } + master_ = nullptr; + domain_ = nullptr; + domain_data_ = nullptr; + for (auto& [motor_id, slave] : slaves_by_motor_id_) { + (void)motor_id; + slave.slave_config = nullptr; + slave.pdo_entries.clear(); + } +} + +bool EthercatMotorBusRuntime::hasValidPdoMapping_() const +{ + return pdo_mapping_.vendor_id != 0 && + pdo_mapping_.product_code != 0 && + !pdo_mapping_.rx_pdos.empty() && + !pdo_mapping_.tx_pdos.empty(); +} + +bool EthercatMotorBusRuntime::writePdoRaw_(const int motor_id, + const std::uint16_t index, + const std::uint8_t subindex, + const std::uint8_t bit_len, + const std::uint64_t value) +{ + std::lock_guard lock(data_mutex_); + auto slave_it = slaves_by_motor_id_.find(motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + return false; + } + auto entry_it = slave_it->second.pdo_entries.find(pdoEntryKey_(index, subindex)); + if (entry_it == slave_it->second.pdo_entries.end()) { + return false; + } + auto& entry = entry_it->second; + if (!entry.rx || entry.cfg.bit_len != bit_len) { + return false; + } + entry.value = maskValue_(value, entry.cfg.bit_len); + if (domain_data_) { + writeEntryValue_(domain_data_, entry); + } + return true; +} + +bool EthercatMotorBusRuntime::readPdoRaw_(const int motor_id, + const std::uint16_t index, + const std::uint8_t subindex, + const std::uint8_t bit_len, + std::uint64_t& value) const +{ + std::lock_guard lock(data_mutex_); + const auto slave_it = slaves_by_motor_id_.find(motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + return false; + } + const auto entry_it = slave_it->second.pdo_entries.find(pdoEntryKey_(index, subindex)); + if (entry_it == slave_it->second.pdo_entries.end()) { + return false; + } + const auto& entry = entry_it->second; + if (entry.rx || entry.cfg.bit_len != bit_len) { + return false; + } + value = maskValue_(entry.value, entry.cfg.bit_len); + return true; +} + +bool EthercatMotorBusRuntime::writeSdoRaw_(const int motor_id, + const std::uint16_t index, + const std::uint8_t subindex, + const std::uint8_t bit_len, + const std::uint64_t value) +{ + if (!isSupportedBitLength_(bit_len)) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] unsupported SDO write bit length: " + << static_cast(bit_len) + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", motor_id=" << motor_id; + return false; + } + + auto slave_it = slaves_by_motor_id_.find(motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing motor for SDO write: " + << motor_id << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex); + return false; + } + auto& slave = slave_it->second; + if (!slave.slave_config || !master_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] runtime is not initialized for SDO write: " + << id_ << ", motor_id=" << motor_id + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex); + return false; + } + + const auto raw_value = maskValue_(value, bit_len); + if (!started_) { + int result = -1; + switch (bit_len) { + case 8: + result = ecrt_slave_config_sdo8(slave.slave_config, index, subindex, + static_cast(raw_value)); + break; + case 16: + result = ecrt_slave_config_sdo16(slave.slave_config, index, subindex, + static_cast(raw_value)); + break; + case 32: + result = ecrt_slave_config_sdo32(slave.slave_config, index, subindex, + static_cast(raw_value)); + break; + default: + break; + } + if (result != 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to configure startup SDO: " + << "group=" << id_ << ", motor_id=" << motor_id + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", bit_len=" << static_cast(bit_len) + << ", value=" << raw_value + << ", result=" << result; + return false; + } + return true; + } + + std::array data{}; + switch (bit_len) { + case 8: + EC_WRITE_U8(data.data(), static_cast(raw_value)); + break; + case 16: + EC_WRITE_U16(data.data(), static_cast(raw_value)); + break; + case 32: + EC_WRITE_U32(data.data(), static_cast(raw_value)); + break; + default: + break; + } + + const auto data_size = static_cast(bit_len / 8); + std::uint32_t abort_code = 0; + const int result = ecrt_master_sdo_download( + master_, + static_cast(slave.cfg.position()), + index, + subindex, + data.data(), + data_size, + &abort_code); + if (result != 0) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to write SDO: " + << "group=" << id_ << ", motor_id=" << motor_id + << ", slave_position=" << slave.cfg.position() + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", bit_len=" << static_cast(bit_len) + << ", value=" << raw_value + << ", result=" << result + << ", abort_code=" << hexIndex_(abort_code); + return false; + } + return true; +} + +bool EthercatMotorBusRuntime::readSdoRaw_(const int motor_id, + const std::uint16_t index, + const std::uint8_t subindex, + const std::uint8_t bit_len, + std::uint64_t& value) +{ + if (!isSupportedBitLength_(bit_len)) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] unsupported SDO read bit length: " + << static_cast(bit_len) + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", motor_id=" << motor_id; + return false; + } + + auto slave_it = slaves_by_motor_id_.find(motor_id); + if (slave_it == slaves_by_motor_id_.end()) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] missing motor for SDO read: " + << motor_id << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex); + return false; + } + const auto& slave = slave_it->second; + if (!master_) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] runtime is not initialized for SDO read: " + << id_ << ", motor_id=" << motor_id + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex); + return false; + } + + std::array data{}; + const auto data_size = static_cast(bit_len / 8); + std::size_t result_size = 0; + std::uint32_t abort_code = 0; + const int result = ecrt_master_sdo_upload( + master_, + static_cast(slave.cfg.position()), + index, + subindex, + data.data(), + data_size, + &result_size, + &abort_code); + if (result != 0 || result_size != data_size) { + CMVR_LOG(ERROR) << "[EthercatMotorBusRuntime] failed to read SDO: " + << "group=" << id_ << ", motor_id=" << motor_id + << ", slave_position=" << slave.cfg.position() + << ", object=" << hexIndex_(index) + << ":" << static_cast(subindex) + << ", bit_len=" << static_cast(bit_len) + << ", result=" << result + << ", result_size=" << result_size + << ", abort_code=" << hexIndex_(abort_code); + return false; + } + + switch (bit_len) { + case 8: + value = EC_READ_U8(data.data()); + break; + case 16: + value = EC_READ_U16(data.data()); + break; + case 32: + value = EC_READ_U32(data.data()); + break; + default: + return false; + } + value = maskValue_(value, bit_len); + return true; +} + +std::uint32_t EthercatMotorBusRuntime::pdoEntryKey_(const std::uint16_t index, + const std::uint8_t subindex) +{ + return (static_cast(index) << 8U) | subindex; +} + +std::string EthercatMotorBusRuntime::hexIndex_(const std::uint32_t index) +{ + std::ostringstream oss; + oss << "0x" << std::hex << std::uppercase << index; + return oss.str(); +} + +std::uint64_t EthercatMotorBusRuntime::maskValue_(const std::uint64_t value, + const std::uint8_t bit_len) +{ + switch (bit_len) { + case 8: + return value & 0xFFU; + case 16: + return value & 0xFFFFU; + case 32: + return value & 0xFFFFFFFFULL; + default: + return value; + } +} + +bool EthercatMotorBusRuntime::isSupportedBitLength_(const std::uint8_t bit_len) +{ + return bit_len == 8 || bit_len == 16 || bit_len == 32; +} + +std::uint64_t EthercatMotorBusRuntime::readEntryValue_(const std::uint8_t* domain_data, + const PdoEntryRuntime& entry) +{ + switch (entry.cfg.bit_len) { + case 8: + return EC_READ_U8(domain_data + entry.offset); + case 16: + return EC_READ_U16(domain_data + entry.offset); + case 32: + return EC_READ_U32(domain_data + entry.offset); + default: + return 0; + } +} + +void EthercatMotorBusRuntime::writeEntryValue_(std::uint8_t* domain_data, + const PdoEntryRuntime& entry) +{ + switch (entry.cfg.bit_len) { + case 8: + EC_WRITE_U8(domain_data + entry.offset, static_cast(entry.value)); + break; + case 16: + EC_WRITE_U16(domain_data + entry.offset, static_cast(entry.value)); + break; + case 32: + EC_WRITE_U32(domain_data + entry.offset, static_cast(entry.value)); + break; + default: + break; + } +} + +std::uint64_t EthercatMotorBusRuntime::steadyTimeNs_() +{ + return timePointNs_(std::chrono::steady_clock::now()); +} + +std::uint64_t EthercatMotorBusRuntime::timePointNs_( + const std::chrono::steady_clock::time_point time_point) +{ + const auto time_since_epoch = time_point.time_since_epoch(); + return static_cast( + std::chrono::duration_cast(time_since_epoch).count()); +} + +std::uint32_t EthercatMotorBusRuntime::usToNs_(const std::uint32_t value_us) +{ + return value_us * 1000U; +} + +std::int32_t EthercatMotorBusRuntime::usToNs_(const std::int32_t value_us) +{ + return value_us * 1000; } } // namespace cmvr::device diff --git a/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp new file mode 100644 index 00000000..d767f4bd --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp @@ -0,0 +1,102 @@ +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" + +#include +#include +#include +#include +#include +#include + +#include + +#include "cmvr/msgs/cia402.pb.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h" + +namespace cmvr::device { +namespace { + + + +config::MotorGroupConfig createSingleSlaveGroup() +{ + config::MotorGroupConfig group; + group.set_id("ethercat_real_test"); + group.set_bus_type(config::MOTOR_BUS_ETHERCAT); + group.set_vendor(config::MOTOR_VENDOR_EYOU); + group.set_protocol(config::MOTOR_PROTOCOL_ETHERCAT_CIA402); + + auto* ethercat = group.mutable_ethercat(); + ethercat->set_master_index(0); + ethercat->set_cycle_us(1000); + ethercat->set_slave_op_timeout_ms(12000); + ethercat->set_slave_state_poll_period_ms(10); + + auto* dc = ethercat->mutable_dc(); + dc->set_enable(true); + dc->set_reference_motor_id(1); + dc->set_sync0_cycle_us(1000); + dc->set_sync0_shift_us(0); + dc->set_sync_reference_clock_period(1); + dc->set_assign_activate(768); + dc->set_sync_monitor_period_ms(1000); + + auto* slave = ethercat->add_slaves(); + slave->set_motor_id(1); + slave->set_alias(0); + slave->set_position(0); + + return group; +} + +} // namespace + +TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword) +{ + + + EthercatMotorBusRuntime runtime; + runtime.setPdoMapping(createEyouCia402PdoMapping()); + + ASSERT_TRUE(runtime.init(createSingleSlaveGroup())); + EXPECT_EQ(runtime.busType(), config::MOTOR_BUS_ETHERCAT); + EXPECT_TRUE(runtime.hasMotor(1)); + EXPECT_NE(runtime.slaveForMotor(1), nullptr); + EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_CONTROL_WORD_6040, 0x00)); + EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_STATUS_WORD_6041, 0x00)); + EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_OPERATION_MODE_6060, 0x00)); + EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00)); + + const bool started = runtime.start(); + EXPECT_TRUE(started); + if (!started) { + runtime.stop(); + return; + } + + EXPECT_TRUE(runtime.writePdo(1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000)); + EXPECT_TRUE(runtime.writePdo(1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0)); + + const int settle_ms = 1000; + std::this_thread::sleep_for(std::chrono::milliseconds(settle_ms)); + + std::uint16_t statusword = 0; + EXPECT_TRUE(runtime.readPdo(1, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword)); + + std::int8_t mode_display = 0; + EXPECT_TRUE(runtime.readPdo(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display)); + + std::cout << "CIA402 statusword: 0x" << std::hex << statusword + << ", mode display: " << std::dec << static_cast(mode_display) + << std::endl; + + const int hold_ms = 10000; + if (hold_ms > 0) { + std::cout << "Holding EtherCAT runtime for " << hold_ms + << " ms. Check slave state in another terminal." << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(hold_ms)); + } + + runtime.stop(); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt b/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt new file mode 100644 index 00000000..8c7d93df --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt @@ -0,0 +1,49 @@ +add_library(ethercat_motor_driver SHARED + src/cia402/cia402_protocol.cpp + src/vendor/eyou/eyou_motor.cpp + src/vendor/eyou/eyou_motor_adapter.cpp +) + +target_include_directories(ethercat_motor_driver + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +target_link_libraries(ethercat_motor_driver + PUBLIC + cmvr_es::device::motor_core + cmvr_es::device::motor_bus_runtime + PRIVATE + cmvr_es::proto + glog +) + +add_library(cmvr_es::device::ethercat_motor_driver ALIAS ethercat_motor_driver) +install(TARGETS ethercat_motor_driver LIBRARY DESTINATION lib) + +add_executable(eyou_motor_real_test + src/vendor/eyou/eyou_motor_real_test.cpp +) + +target_link_libraries(eyou_motor_real_test + PRIVATE + cmvr_es::device::ethercat_motor_driver + gtest + gtest_main + pthread + glog +) + +add_executable(eyou_motor_device_manager_real_test + src/vendor/eyou/eyou_motor_device_manager_real_test.cpp +) + +target_link_libraries(eyou_motor_device_manager_real_test + PRIVATE + cmvr_es::device_manager + cmvr_es::device::motor_manager + gtest + gtest_main + pthread + glog +) diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h new file mode 100644 index 00000000..e3e92dcb --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h @@ -0,0 +1,161 @@ +#ifndef CMVR_ES_CIA402_OBJECTS_H +#define CMVR_ES_CIA402_OBJECTS_H + +#include + +namespace cmvr::device::cia402 { + +union Controlword { + std::uint16_t value; + struct { + std::uint16_t switch_on : 1; + std::uint16_t enable_voltage : 1; + std::uint16_t quick_stop : 1; + std::uint16_t enable_operation : 1; + std::uint16_t new_set_point : 1; + std::uint16_t change_set_immediately : 1; + std::uint16_t relative : 1; + std::uint16_t fault_reset : 1; + std::uint16_t halt : 1; + std::uint16_t reserved : 2; + std::uint16_t manufacturer_specific : 5; + }; +}; + +union Statusword { + std::uint16_t value; + struct { + std::uint16_t ready_to_switch_on : 1; + std::uint16_t switched_on : 1; + std::uint16_t operation_enabled : 1; + std::uint16_t fault : 1; + std::uint16_t voltage_enabled : 1; + std::uint16_t quick_stop : 1; + std::uint16_t switch_on_disabled : 1; + std::uint16_t warning : 1; + std::uint16_t manufacturer_specific_8 : 1; + std::uint16_t remote : 1; + std::uint16_t target_reached : 1; + std::uint16_t internal_limit_active : 1; + std::uint16_t operation_mode_specific : 2; + std::uint16_t manufacturer_specific : 2; + }; +}; + +static_assert(sizeof(Controlword) == sizeof(std::uint16_t)); +static_assert(sizeof(Statusword) == sizeof(std::uint16_t)); + +enum class DeviceState { + SwitchOnDisabled, + ReadyToSwitchOn, + SwitchedOn, + OperationEnabled, +}; + +namespace detail { + +struct StateRule { + std::uint16_t relevant_bits; + std::uint16_t expected_bits; +}; + +inline StateRule stateRule(const DeviceState state) +{ + // CiA402 device states are matched by selected 0x6041 statusword bits. + switch (state) { + case DeviceState::SwitchOnDisabled: + return {0x004F, 0x0040}; + case DeviceState::ReadyToSwitchOn: + return {0x006F, 0x0021}; + case DeviceState::SwitchedOn: + return {0x006F, 0x0023}; + case DeviceState::OperationEnabled: + return {0x006F, 0x0027}; + } + return {0x006F, 0x0000}; +} + +} // namespace detail + +inline Controlword controlword(const std::uint16_t value) +{ + Controlword cw{}; + cw.value = value; + return cw; +} + +inline Statusword statusword(const std::uint16_t value) +{ + Statusword sw{}; + sw.value = value; + return sw; +} + +inline Controlword shutdownControlword() +{ + Controlword cw{}; + cw.quick_stop = 1; + cw.enable_voltage = 1; + return cw; +} + +inline Controlword switchOnControlword() +{ + auto cw = shutdownControlword(); + cw.switch_on = 1; + return cw; +} + +inline Controlword enableOperationControlword() +{ + auto cw = switchOnControlword(); + cw.enable_operation = 1; + return cw; +} + +inline Controlword quickStopControlword() +{ + auto cw = enableOperationControlword(); + cw.quick_stop = 0; + return cw; +} + +inline Controlword faultResetControlword() +{ + Controlword cw{}; + cw.fault_reset = 1; + return cw; +} + +inline Controlword profilePositionControlword(const bool new_set_point) +{ + auto cw = enableOperationControlword(); + cw.change_set_immediately = 1; + cw.new_set_point = new_set_point ? 1 : 0; + return cw; +} + +inline bool hasState(const Statusword status, const DeviceState state) +{ + const auto rule = detail::stateRule(state); + return (status.value & rule.relevant_bits) == rule.expected_bits; +} + +inline bool isSwitchOnDisabled(const Statusword status) +{ + return hasState(status, DeviceState::SwitchOnDisabled); +} + +inline bool isOperationEnabled(const Statusword status) +{ + return hasState(status, DeviceState::OperationEnabled); +} + +inline bool targetReached(const Statusword status) +{ + return status.target_reached != 0; +} + +} // namespace cmvr::device::cia402 + +#endif // CMVR_ES_CIA402_OBJECTS_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h new file mode 100644 index 00000000..c4cc46b5 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h @@ -0,0 +1,116 @@ +#ifndef CMVR_ES_CIA402_PROTOCOL_H +#define CMVR_ES_CIA402_PROTOCOL_H + +#include +#include +#include + +#include "cmvr/config/motor_config/motor_config.pb.h" +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h" +#include "devices/motor/motor_protocol_interface.h" + +namespace cmvr::device { + +class Cia402Protocol final : public MotorProtocolInterface { +public: + explicit Cia402Protocol(std::shared_ptr bus_runtime, + const config::Cia402ProtocolConfig& config); + ~Cia402Protocol() override = default; + + bool initNode(std::uint8_t node_id) override; + + void setMode(std::uint8_t node_id, msgs::RunMode mode) override; + msgs::RunMode getMode(std::uint8_t node_id) override; + void setLimitQdd(std::uint8_t node_id, double u_qdd, double l_qdd) override; + void setLimitQd(std::uint8_t node_id, double qd) override; + void setLimitQ(std::uint8_t node_id, double ub, double lb) override; + bool calibrateZeroQ(std::uint8_t node_id) override; + bool reachedTargetQ(std::uint8_t node_id) override; + bool commandProfilePosition(std::uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) override; + bool commandProfileVelocity(std::uint8_t node_id, + double target_qd, + double max_qdd) override; + bool commandCyclicPosition(std::uint8_t node_id, + double target_q, + double target_qd) override; + bool commandCyclicVelocity(std::uint8_t node_id, + double target_qd) override; + bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override; + void setMotorConversion(std::uint8_t node_id, + double encoder_counts_per_rev, + double gear_ratio) override; + bool torqueOn(std::uint8_t node_id) override; + bool torqueOff(std::uint8_t node_id) override; + bool brakeRelease(std::uint8_t node_id) override; + bool quickStop(std::uint8_t node_id) override; + + double getQ(std::uint8_t node_id) override; + double getQd(std::uint8_t node_id) override; + bool syncTargetToActualPosition(std::uint8_t node_id); + +private: + struct NodeState { + msgs::RunMode mode{msgs::RUN_MODE_CYCLIC_SYNC_POSITION}; + cia402::Controlword controlword{}; + std::int32_t target_position{0}; + std::int32_t target_velocity{0}; + std::int16_t target_torque{0}; + std::int32_t profile_velocity{0}; + std::int32_t profile_acceleration{0}; + std::int32_t profile_deceleration{0}; + double limit_q_lb{0.0}; + double limit_q_ub{0.0}; + double limit_qd{0.0}; + double limit_qdd{0.0}; + double encoder_counts_per_rev{0.0}; + double gear_ratio{0.0}; + }; + + static std::int8_t toCia402Mode_(msgs::RunMode mode); + static msgs::RunMode fromCia402Mode_(std::int8_t mode); + static cia402::Controlword nextControlword_(cia402::Statusword statusword); + static bool isOperationEnabled_(cia402::Statusword statusword); + static bool targetReached_(cia402::Statusword statusword); + + std::int32_t radToCounts_(double angle_rad, const NodeState& state) const; + double countsToRad_(std::int32_t counts, const NodeState& state) const; + std::int32_t radPerSecToCounts_(double velocity_rad_s, const NodeState& state) const; + std::int32_t radPerSec2ToCounts_(double acceleration_rad_s2, const NodeState& state) const; + double countsToRadPerSec_(std::int32_t velocity_counts_s, const NodeState& state) const; + NodeState& nodeState_(std::uint8_t node_id); + const NodeState* findNodeState_(std::uint8_t node_id) const; + bool hasValidConversion_(std::uint8_t node_id, const NodeState& state) const; + bool validateNodePdos_(std::uint8_t node_id) const; + bool readStatusword_(std::uint8_t node_id, std::uint16_t& statusword) const; + bool readActualPosition_(std::uint8_t node_id, std::int32_t& actual_position) const; + bool readActualVelocity_(std::uint8_t node_id, std::int32_t& actual_velocity) const; + bool readModeDisplay_(std::uint8_t node_id, std::int8_t& mode_display) const; + bool writeControlword_(std::uint8_t node_id, cia402::Controlword controlword); + bool writeControlwordAndWait_(std::uint8_t node_id, + cia402::Controlword controlword, + cia402::DeviceState target_state, + const char* state_name); + bool waitStatus_(std::uint8_t node_id, + cia402::DeviceState target_state, + const char* state_name) const; + bool waitVelocityNearZero_(std::uint8_t node_id, const char* action_name) const; + bool writePositionLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const; + bool writeVelocityLimitToDictionary_(std::uint8_t node_id, const NodeState& state) const; + bool writeAccelerationLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const; + bool prepareSafeTargetsForMode_(std::uint8_t node_id, msgs::RunMode mode, NodeState& state); + void writeTargetsForMode_(std::uint8_t node_id, msgs::RunMode mode, const NodeState& state) const; + void writeProfilePositionTarget_(std::uint8_t node_id, NodeState& state); + void writeNode_(std::uint8_t node_id, NodeState& state); + + std::shared_ptr bus_runtime_; + config::Cia402ProtocolConfig config_; + std::unordered_map nodes_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_CIA402_PROTOCOL_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h new file mode 100644 index 00000000..0993dd57 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h @@ -0,0 +1,81 @@ +#ifndef CMVR_ES_EYOU_CIA402_PDO_MAPPING_H +#define CMVR_ES_EYOU_CIA402_PDO_MAPPING_H + +#include +#include +#include + +#include "cmvr/msgs/canopen.pb.h" +#include "cmvr/msgs/cia402.pb.h" +#include "devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h" + +namespace cmvr::device { + +namespace eyou_cia402_pdo_mapping_detail { + +inline constexpr std::uint32_t VENDOR_ID = 0x00001097; +inline constexpr std::uint32_t PRODUCT_CODE = 0x00002406; + +inline EthercatPdoEntryConfig entry(const std::uint16_t index, + const std::uint8_t subindex, + const std::uint8_t bit_len, + std::string name) +{ + EthercatPdoEntryConfig cfg; + cfg.index = index; + cfg.subindex = subindex; + cfg.bit_len = bit_len; + cfg.name = std::move(name); + cfg.padding = index == 0 || bit_len == 0; + return cfg; +} + +} // namespace eyou_cia402_pdo_mapping_detail + +inline EthercatPdoMapping createEyouCia402PdoMapping() +{ + using namespace eyou_cia402_pdo_mapping_detail; + + EthercatPdoConfig rx_pdo; + rx_pdo.index = msgs::CANOPEN_RPDO2_MAP_1601; + rx_pdo.sync_manager = 2; + rx_pdo.rx = true; + rx_pdo.entries = { + entry(msgs::CIA402_CONTROL_WORD_6040, 0x00, 16, "Control Word"), + entry(msgs::CIA402_TARGET_POSITION_607A, 0x00, 32, "Target Position"), + entry(msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, 32, "Target Velocity"), + entry(msgs::CIA402_TARGET_TORQUE_6071, 0x00, 16, "Target Torque"), + entry(msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00, 32, "Profile Acceleration"), + entry(msgs::CIA402_PROFILE_DECELERATION_6084, 0x00, 32, "Profile Deceleration"), + entry(msgs::CIA402_PROFILE_VELOCITY_6081, 0x00, 32, "Profile Velocity"), + entry(msgs::CIA402_TORQUE_SLOPE_6087, 0x00, 32, "Torque Slope"), + entry(msgs::CIA402_OPERATION_MODE_6060, 0x00, 8, "Mode Of Operation"), + entry(0x0000, 0x00, 8, "Padding"), + }; + + EthercatPdoConfig tx_pdo; + tx_pdo.index = msgs::CANOPEN_TPDO1_MAP_1A00; + tx_pdo.sync_manager = 3; + tx_pdo.rx = false; + tx_pdo.entries = { + entry(msgs::CIA402_STATUS_WORD_6041, 0x00, 16, "Status Word"), + entry(msgs::CIA402_ACTUAL_POSITION_6064, 0x00, 32, "Actual Position"), + entry(msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, 32, "Actual Velocity"), + entry(msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, 16, "Actual Torque"), + entry(msgs::CIA402_MODE_DISPLAY_6061, 0x00, 8, "Mode Of Operation Display"), + entry(msgs::CIA402_ERROR_CODE_603F, 0x00, 16, "Error Code"), + entry(0x0000, 0x00, 8, "Padding"), + }; + + EthercatPdoMapping mapping; + mapping.vendor_id = VENDOR_ID; + mapping.product_code = PRODUCT_CODE; + mapping.name = "EYOU ServoModule ECAT V145 CiA402"; + mapping.rx_pdos.push_back(std::move(rx_pdo)); + mapping.tx_pdos.push_back(std::move(tx_pdo)); + return mapping; +} + +} // namespace cmvr::device + +#endif // CMVR_ES_EYOU_CIA402_PDO_MAPPING_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h new file mode 100644 index 00000000..10f3ee96 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h @@ -0,0 +1,43 @@ +#ifndef CMVR_ES_EYOU_MOTOR_H +#define CMVR_ES_EYOU_MOTOR_H + +#include +#include + +#include "cmvr/config/motor_config/motor_config.pb.h" +#include "devices/motor/abstract_motor.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" + +namespace cmvr::device { + +class EyouMotor final : public AbstractMotor { +public: + EyouMotor(const config::MotorConfigItem& config, + std::shared_ptr cia402_protocol, + std::unique_ptr vendor_adapter); + + std::string typeName() const override { return "EyouMotor"; } + bool init() override; + void setLimitQ(double ub, double lb) override; + void setLimitQd(double qd) override; + bool calibrateZeroQ() override; + bool brakeRelease() override; + +private: + bool hasDependencies_() const; + bool hasValidConversion_() const; + bool writeVendorPositionLimits_() const; + bool writeVendorVelocityLimit_() const; + std::int32_t radToCounts_(double angle_rad) const; + std::uint32_t radPerSecToCounts_(double velocity_rad_s) const; + + std::shared_ptr cia402_protocol_; + std::unique_ptr vendor_adapter_; + double encoder_counts_per_rev_{0.0}; + double gear_ratio_{0.0}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_EYOU_MOTOR_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h new file mode 100644 index 00000000..a9d95490 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h @@ -0,0 +1,33 @@ +#ifndef CMVR_ES_EYOU_MOTOR_ADAPTER_H +#define CMVR_ES_EYOU_MOTOR_ADAPTER_H + +#include +#include + +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h" + +namespace cmvr::device { + +class EyouMotorAdapter final : public MotorVendorAdapter { +public: + explicit EyouMotorAdapter(std::shared_ptr bus_runtime); + ~EyouMotorAdapter() override = default; + + bool initNode(std::uint8_t node_id) override; + bool writePositionLimits(std::uint8_t node_id, + std::int32_t lower_limit, + std::int32_t upper_limit) override; + bool writeVelocityLimit(std::uint8_t node_id, + std::uint32_t velocity_limit) override; + bool calibrateZero(std::uint8_t node_id, + std::int32_t& zeroed_position) override; + bool brakeRelease(std::uint8_t node_id) override; + +private: + std::shared_ptr bus_runtime_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_EYOU_MOTOR_ADAPTER_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h new file mode 100644 index 00000000..cee1369d --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h @@ -0,0 +1,20 @@ +#ifndef CMVR_ES_EYOU_OBJECTS_H +#define CMVR_ES_EYOU_OBJECTS_H + +#include + +namespace cmvr::device::eyou { + +inline constexpr std::uint16_t EYOU_SOFT_LIMIT_STATE_2003 = 0x2003; + +inline constexpr std::uint16_t EYOU_BRAKE_CONTROL_2014 = 0x2014; + +inline constexpr std::uint16_t EYOU_OVER_SPEED_THRESHOLD_2024 = 0x2024; +inline constexpr std::uint16_t EYOU_FIRST_ENCODER_VALUE_202A = 0x202A; +inline constexpr std::uint16_t EYOU_SECOND_ENCODER_VALUE_202B = 0x202B; + +inline constexpr std::uint16_t EYOU_STORE_PARAMETERS_1010 = 0x1010; + +} // namespace cmvr::device::eyou + +#endif // CMVR_ES_EYOU_OBJECTS_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h new file mode 100644 index 00000000..5af59549 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h @@ -0,0 +1,25 @@ +#ifndef CMVR_ES_MOTOR_VENDOR_ADAPTER_H +#define CMVR_ES_MOTOR_VENDOR_ADAPTER_H + +#include + +namespace cmvr::device { + +class MotorVendorAdapter { +public: + virtual ~MotorVendorAdapter() = default; + + virtual bool initNode(std::uint8_t node_id) = 0; + virtual bool writePositionLimits(std::uint8_t node_id, + std::int32_t lower_limit, + std::int32_t upper_limit) = 0; + virtual bool writeVelocityLimit(std::uint8_t node_id, + std::uint32_t velocity_limit) = 0; + virtual bool calibrateZero(std::uint8_t node_id, + std::int32_t& zeroed_position) = 0; + virtual bool brakeRelease(std::uint8_t node_id) = 0; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MOTOR_VENDOR_ADAPTER_H diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp new file mode 100644 index 00000000..7dbd8e65 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp @@ -0,0 +1,872 @@ +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" + +#include +#include +#include +#include +#include + +#include "common/base/logging/logger.h" +#include "cmvr/msgs/cia402.pb.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h" + +namespace cmvr::device { + +Cia402Protocol::Cia402Protocol(std::shared_ptr bus_runtime, + const config::Cia402ProtocolConfig& config) + : bus_runtime_(std::move(bus_runtime)), + config_(config) +{ + comm_proto = CommProto::ETHERCAT; +} + +bool Cia402Protocol::initNode(const std::uint8_t node_id) +{ + if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] missing EtherCAT motor: " + << static_cast(node_id); + return false; + } + + if (!validateNodePdos_(node_id)) { + return false; + } + auto& state = nodeState_(node_id); + std::int32_t actual_position = 0; + if (readActualPosition_(node_id, actual_position)) { + state.target_position = actual_position; + } + writeNode_(node_id, state); + return true; +} + +bool Cia402Protocol::commandProfilePosition(const std::uint8_t node_id, + const double target_q, + const double max_qd, + const double max_qdd) +{ + auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return false; + } + state.target_position = radToCounts_(target_q, state); + state.profile_velocity = std::abs(radPerSecToCounts_(max_qd, state)); + if (max_qdd > 0.0) { + const auto profile_acceleration = std::abs(radPerSec2ToCounts_(max_qdd, state)); + state.profile_acceleration = profile_acceleration; + state.profile_deceleration = profile_acceleration; + } + writeProfilePositionTarget_(node_id, state); + return true; +} + +bool Cia402Protocol::commandProfileVelocity(const std::uint8_t node_id, + const double target_qd, + const double max_qdd) +{ + auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return false; + } + state.target_velocity = radPerSecToCounts_(target_qd, state); + if (max_qdd > 0.0) { + const auto profile_acceleration = std::abs(radPerSec2ToCounts_(max_qdd, state)); + state.profile_acceleration = profile_acceleration; + state.profile_deceleration = profile_acceleration; + } + writeNode_(node_id, state); + return true; +} + +bool Cia402Protocol::commandCyclicPosition(const std::uint8_t node_id, + const double target_q, + const double target_qd) +{ + auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return false; + } + state.target_position = radToCounts_(target_q, state); + state.target_velocity = radPerSecToCounts_(target_qd, state); + writeNode_(node_id, state); + return true; +} + +bool Cia402Protocol::commandCyclicVelocity(const std::uint8_t node_id, + const double target_qd) +{ + auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return false; + } + state.target_velocity = radPerSecToCounts_(target_qd, state); + writeNode_(node_id, state); + return true; +} + +bool Cia402Protocol::commandCyclicTorque(const std::uint8_t node_id, + const double target_tau) +{ + (void)target_tau; + CMVR_LOG(ERROR) << "[Cia402Protocol] cyclic torque command is not implemented, node=" + << static_cast(node_id); + return false; +} + +void Cia402Protocol::setMode(const std::uint8_t node_id, const msgs::RunMode mode) +{ + auto& state = nodeState_(node_id); + if (!prepareSafeTargetsForMode_(node_id, mode, state)) { + return; + } + writeTargetsForMode_(node_id, mode, state); + state.mode = mode; + writeNode_(node_id, state); +} + +msgs::RunMode Cia402Protocol::getMode(const std::uint8_t node_id) +{ + std::int8_t mode_display = 0; + if (readModeDisplay_(node_id, mode_display)) { + return fromCia402Mode_(mode_display); + } + return nodeState_(node_id).mode; +} + +void Cia402Protocol::setLimitQdd(const std::uint8_t node_id, + const double u_qdd, + const double l_qdd) +{ + auto& state = nodeState_(node_id); + state.limit_qdd = std::max(std::abs(u_qdd), std::abs(l_qdd)); + if (hasValidConversion_(node_id, state)) { + const auto profile_acceleration = std::abs(radPerSec2ToCounts_(state.limit_qdd, state)); + state.profile_acceleration = profile_acceleration; + state.profile_deceleration = profile_acceleration; + writeAccelerationLimitsToDictionary_(node_id, state); + } +} + +void Cia402Protocol::setLimitQd(const std::uint8_t node_id, const double qd) +{ + auto& state = nodeState_(node_id); + state.limit_qd = std::abs(qd); + if (hasValidConversion_(node_id, state)) { + state.profile_velocity = std::abs(radPerSecToCounts_(state.limit_qd, state)); + writeVelocityLimitToDictionary_(node_id, state); + } +} + +void Cia402Protocol::setLimitQ(const std::uint8_t node_id, + const double ub, + const double lb) +{ + auto& state = nodeState_(node_id); + state.limit_q_ub = ub; + state.limit_q_lb = lb; + if (hasValidConversion_(node_id, state)) { + writePositionLimitsToDictionary_(node_id, state); + } +} + +bool Cia402Protocol::calibrateZeroQ(const std::uint8_t node_id) +{ + CMVR_LOG(ERROR) << "[Cia402Protocol] zero calibration is vendor-specific, node=" + << static_cast(node_id); + return false; +} + +bool Cia402Protocol::reachedTargetQ(const std::uint8_t node_id) +{ + std::uint16_t statusword = 0; + if (!readStatusword_(node_id, statusword)) { + return false; + } + return targetReached_(cia402::statusword(statusword)); +} + +void Cia402Protocol::setMotorConversion( + const std::uint8_t node_id, + const double encoder_counts_per_rev, + const double gear_ratio) +{ + auto& state = nodeState_(node_id); + state.encoder_counts_per_rev = encoder_counts_per_rev; + state.gear_ratio = gear_ratio; +} + +bool Cia402Protocol::torqueOn(const std::uint8_t node_id) +{ + if (!bus_runtime_) { + return false; + } + + auto& state = nodeState_(node_id); + std::uint16_t statusword = 0; + if (readStatusword_(node_id, statusword) && cia402::statusword(statusword).fault != 0) { + if (!writeControlword_(node_id, cia402::faultResetControlword())) { + return false; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.status_poll_period_ms())); + } + + if (!prepareSafeTargetsForMode_(node_id, msgs::RUN_MODE_PROFILE_POSITION, state)) { + return false; + } + state.mode = msgs::RUN_MODE_PROFILE_POSITION; + state.target_velocity = 0; + state.target_torque = 0; + writeTargetsForMode_(node_id, state.mode, state); + if (!bus_runtime_->writePdo(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, + toCia402Mode_(state.mode))) { + return false; + } + + if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(), + cia402::DeviceState::ReadyToSwitchOn, + "Ready To Switch On")) { + return false; + } + if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(), + cia402::DeviceState::SwitchedOn, + "Switched On")) { + return false; + } + if (!writeControlwordAndWait_(node_id, cia402::enableOperationControlword(), + cia402::DeviceState::OperationEnabled, + "Operation Enabled")) { + return false; + } + + std::int32_t actual_position = 0; + if (readActualPosition_(node_id, actual_position)) { + state.target_position = actual_position; + } + writeTargetsForMode_(node_id, state.mode, state); + + state.controlword = cia402::profilePositionControlword(true); + if (!writeControlword_(node_id, state.controlword)) { + return false; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.profile_position_trigger_delay_ms())); + + state.controlword = cia402::profilePositionControlword(false); + return writeControlword_(node_id, state.controlword); +} + +bool Cia402Protocol::torqueOff(const std::uint8_t node_id) +{ + if (!bus_runtime_) { + return false; + } + + auto& state = nodeState_(node_id); + std::int32_t actual_position = 0; + if (readActualPosition_(node_id, actual_position)) { + state.target_position = actual_position; + } + state.target_velocity = 0; + state.target_torque = 0; + writeTargetsForMode_(node_id, state.mode, state); + waitVelocityNearZero_(node_id, "torqueOff"); + + if (!writeControlwordAndWait_(node_id, cia402::switchOnControlword(), + cia402::DeviceState::SwitchedOn, + "Switched On")) { + return false; + } + if (!writeControlwordAndWait_(node_id, cia402::shutdownControlword(), + cia402::DeviceState::ReadyToSwitchOn, + "Ready To Switch On")) { + return false; + } + + return true; +} + +bool Cia402Protocol::brakeRelease(const std::uint8_t node_id) +{ + CMVR_LOG(ERROR) << "[Cia402Protocol] brake release is vendor-specific, node=" + << static_cast(node_id); + return false; +} + +bool Cia402Protocol::quickStop(const std::uint8_t node_id) +{ + if (!bus_runtime_) { + return false; + } + + auto& state = nodeState_(node_id); + state.target_velocity = 0; + state.target_torque = 0; + writeTargetsForMode_(node_id, state.mode, state); + + state.controlword = cia402::quickStopControlword(); + if (!writeControlword_(node_id, state.controlword)) { + return false; + } + return waitVelocityNearZero_(node_id, "quickStop"); +} + +double Cia402Protocol::getQ(const std::uint8_t node_id) +{ + std::int32_t actual_position = 0; + if (!readActualPosition_(node_id, actual_position)) { + return 0.0; + } + const auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return 0.0; + } + return countsToRad_(actual_position, state); +} + +double Cia402Protocol::getQd(const std::uint8_t node_id) +{ + std::int32_t actual_velocity = 0; + if (!readActualVelocity_(node_id, actual_velocity)) { + return 0.0; + } + const auto& state = nodeState_(node_id); + if (!hasValidConversion_(node_id, state)) { + return 0.0; + } + return countsToRadPerSec_(actual_velocity, state); +} + +bool Cia402Protocol::syncTargetToActualPosition(const std::uint8_t node_id) +{ + std::int32_t actual_position = 0; + if (!readActualPosition_(node_id, actual_position)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to read actual position, node=" + << static_cast(node_id); + return false; + } + + auto& state = nodeState_(node_id); + state.target_position = actual_position; + state.target_velocity = 0; + state.target_torque = 0; + writeTargetsForMode_(node_id, state.mode, state); + return true; +} + +std::int8_t Cia402Protocol::toCia402Mode_(const msgs::RunMode mode) +{ + switch (mode) { + case msgs::RUN_MODE_PROFILE_POSITION: + return 1; + case msgs::RUN_MODE_PROFILE_VELOCITY: + return 3; + case msgs::RUN_MODE_HOMING: + return 6; + case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: + return 8; + case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: + return 9; + case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT: + return 10; + default: + return 8; + } +} + +msgs::RunMode Cia402Protocol::fromCia402Mode_(const std::int8_t mode) +{ + switch (mode) { + case 1: + return msgs::RUN_MODE_PROFILE_POSITION; + case 3: + return msgs::RUN_MODE_PROFILE_VELOCITY; + case 6: + return msgs::RUN_MODE_HOMING; + case 8: + return msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + case 9: + return msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; + case 10: + return msgs::RUN_MODE_CYCLIC_SYNC_CURRENT; + default: + return msgs::RUN_MODE_UNSPECIFIED; + } +} + +cia402::Controlword Cia402Protocol::nextControlword_(const cia402::Statusword statusword) +{ + if (statusword.fault != 0) { + return cia402::faultResetControlword(); + } + if (cia402::hasState(statusword, cia402::DeviceState::SwitchOnDisabled)) { + return cia402::shutdownControlword(); + } + if (cia402::hasState(statusword, cia402::DeviceState::ReadyToSwitchOn)) { + return cia402::switchOnControlword(); + } + if (cia402::hasState(statusword, cia402::DeviceState::SwitchedOn)) { + return cia402::enableOperationControlword(); + } + if (cia402::hasState(statusword, cia402::DeviceState::OperationEnabled)) { + return cia402::enableOperationControlword(); + } + return cia402::shutdownControlword(); +} + +bool Cia402Protocol::isOperationEnabled_(const cia402::Statusword statusword) +{ + return cia402::isOperationEnabled(statusword); +} + +bool Cia402Protocol::targetReached_(const cia402::Statusword statusword) +{ + return cia402::targetReached(statusword); +} + +std::int32_t Cia402Protocol::radToCounts_(const double angle_rad, + const NodeState& state) const +{ + const double rev = angle_rad / (2.0 * M_PI); + return static_cast( + std::llround(rev * state.gear_ratio * state.encoder_counts_per_rev)); +} + +double Cia402Protocol::countsToRad_(const std::int32_t counts, + const NodeState& state) const +{ + return static_cast(counts) / + (state.gear_ratio * state.encoder_counts_per_rev) * 2.0 * M_PI; +} + +std::int32_t Cia402Protocol::radPerSecToCounts_( + const double velocity_rad_s, + const NodeState& state) const +{ + const double rev_per_sec = velocity_rad_s / (2.0 * M_PI); + return static_cast( + std::llround(rev_per_sec * state.gear_ratio * state.encoder_counts_per_rev)); +} + +std::int32_t Cia402Protocol::radPerSec2ToCounts_( + const double acceleration_rad_s2, + const NodeState& state) const +{ + const double rev_per_sec2 = acceleration_rad_s2 / (2.0 * M_PI); + return static_cast( + std::llround(rev_per_sec2 * state.gear_ratio * state.encoder_counts_per_rev)); +} + +double Cia402Protocol::countsToRadPerSec_( + const std::int32_t velocity_counts_s, + const NodeState& state) const +{ + return static_cast(velocity_counts_s) / + (state.gear_ratio * state.encoder_counts_per_rev) * 2.0 * M_PI; +} + +Cia402Protocol::NodeState& Cia402Protocol::nodeState_(const std::uint8_t node_id) +{ + return nodes_[node_id]; +} + +const Cia402Protocol::NodeState* Cia402Protocol::findNodeState_( + const std::uint8_t node_id) const +{ + const auto it = nodes_.find(node_id); + if (it == nodes_.end()) { + return nullptr; + } + return &it->second; +} + +bool Cia402Protocol::hasValidConversion_(const std::uint8_t node_id, + const NodeState& state) const +{ + if (state.encoder_counts_per_rev > 0.0 && state.gear_ratio > 0.0) { + return true; + } + CMVR_LOG(ERROR) << "[Cia402Protocol] missing conversion config for node " + << static_cast(node_id) + << ": encoder_counts_per_rev=" << state.encoder_counts_per_rev + << ", gear_ratio=" << state.gear_ratio; + return false; +} + +bool Cia402Protocol::validateNodePdos_(const std::uint8_t node_id) const +{ + if (!bus_runtime_) { + return false; + } + struct RequiredEntry { + std::uint16_t index; + std::uint8_t subindex; + const char* name; + }; + const RequiredEntry required[] = { + {msgs::CIA402_CONTROL_WORD_6040, 0x00, "controlword"}, + {msgs::CIA402_TARGET_POSITION_607A, 0x00, "target position"}, + {msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, "target velocity"}, + {msgs::CIA402_TARGET_TORQUE_6071, 0x00, "target torque"}, + {msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00, "profile acceleration"}, + {msgs::CIA402_PROFILE_DECELERATION_6084, 0x00, "profile deceleration"}, + {msgs::CIA402_PROFILE_VELOCITY_6081, 0x00, "profile velocity"}, + {msgs::CIA402_OPERATION_MODE_6060, 0x00, "operation mode"}, + {msgs::CIA402_STATUS_WORD_6041, 0x00, "statusword"}, + {msgs::CIA402_ACTUAL_POSITION_6064, 0x00, "actual position"}, + {msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, "actual velocity"}, + {msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, "actual torque"}, + {msgs::CIA402_MODE_DISPLAY_6061, 0x00, "mode display"}, + {msgs::CIA402_ERROR_CODE_603F, 0x00, "error code"}, + }; + + for (const auto& entry : required) { + if (!bus_runtime_->hasPdoEntry(node_id, entry.index, entry.subindex)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] missing PDO entry for node " + << static_cast(node_id) + << ": " << entry.name + << " 0x" << std::hex << entry.index + << ":" << static_cast(entry.subindex) << std::dec; + return false; + } + } + return true; +} + +bool Cia402Protocol::readStatusword_(const std::uint8_t node_id, + std::uint16_t& statusword) const +{ + return bus_runtime_ && + bus_runtime_->readPdo(node_id, msgs::CIA402_STATUS_WORD_6041, 0x00, + statusword); +} + +bool Cia402Protocol::readActualPosition_(const std::uint8_t node_id, + std::int32_t& actual_position) const +{ + return bus_runtime_ && + bus_runtime_->readPdo(node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00, + actual_position); +} + +bool Cia402Protocol::readActualVelocity_(const std::uint8_t node_id, + std::int32_t& actual_velocity) const +{ + return bus_runtime_ && + bus_runtime_->readPdo(node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, + actual_velocity); +} + +bool Cia402Protocol::readModeDisplay_(const std::uint8_t node_id, + std::int8_t& mode_display) const +{ + return bus_runtime_ && + bus_runtime_->readPdo(node_id, msgs::CIA402_MODE_DISPLAY_6061, 0x00, + mode_display); +} + +bool Cia402Protocol::writeControlword_(const std::uint8_t node_id, + const cia402::Controlword controlword) +{ + if (!bus_runtime_) { + return false; + } + auto& state = nodeState_(node_id); + state.controlword = controlword; + return bus_runtime_->writePdo(node_id, msgs::CIA402_CONTROL_WORD_6040, + 0x00, controlword.value); +} + +bool Cia402Protocol::writeControlwordAndWait_( + const std::uint8_t node_id, + const cia402::Controlword controlword, + const cia402::DeviceState target_state, + const char* state_name) +{ + if (!writeControlword_(node_id, controlword)) { + return false; + } + return waitStatus_(node_id, target_state, state_name); +} + +bool Cia402Protocol::waitStatus_(const std::uint8_t node_id, + const cia402::DeviceState target_state, + const char* state_name) const +{ + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(config_.state_transition_timeout_ms()); + std::uint16_t last_statusword = 0; + do { + if (readStatusword_(node_id, last_statusword) && + cia402::hasState(cia402::statusword(last_statusword), target_state)) { + return true; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.status_poll_period_ms())); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting for " + << state_name << ", node=" << static_cast(node_id) + << ", last_statusword=0x" << std::hex << last_statusword << std::dec; + return false; +} + +bool Cia402Protocol::waitVelocityNearZero_(const std::uint8_t node_id, + const char* action_name) const +{ + const auto* state = findNodeState_(node_id); + if (state == nullptr || !hasValidConversion_(node_id, *state)) { + return false; + } + + const auto tolerance_counts = std::max( + 1, std::abs(radPerSecToCounts_(config_.stopped_velocity_tolerance_rad_s(), *state))); + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(config_.velocity_stop_timeout_ms()); + std::int32_t last_velocity = 0; + do { + if (readActualVelocity_(node_id, last_velocity) && + std::abs(last_velocity) <= tolerance_counts) { + return true; + } + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.status_poll_period_ms())); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[Cia402Protocol] timeout waiting velocity near zero " + << "during " << action_name + << ", node=" << static_cast(node_id) + << ", last_velocity=" << last_velocity + << ", tolerance=" << tolerance_counts; + return false; +} + +bool Cia402Protocol::writePositionLimitsToDictionary_( + const std::uint8_t node_id, + const NodeState& state) const +{ + if (!bus_runtime_) { + return false; + } + if (!std::isfinite(state.limit_q_lb) || !std::isfinite(state.limit_q_ub) || + state.limit_q_ub <= state.limit_q_lb) { + return true; + } + + const auto lower_limit = radToCounts_(state.limit_q_lb, state); + const auto upper_limit = radToCounts_(state.limit_q_ub, state); + + const bool ok = + bus_runtime_->writeSdo(node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x01, lower_limit) && + bus_runtime_->writeSdo(node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x02, upper_limit); + if (!ok) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write software position " + << "limits to dictionary, node=" << static_cast(node_id) + << ", lower=" << lower_limit + << ", upper=" << upper_limit; + } + return ok; +} + +bool Cia402Protocol::writeVelocityLimitToDictionary_( + const std::uint8_t node_id, + const NodeState& state) const +{ + if (!bus_runtime_) { + return false; + } + if (!std::isfinite(state.limit_qd) || state.limit_qd <= 0.0) { + return true; + } + + const auto velocity_limit = + static_cast(std::abs(radPerSecToCounts_(state.limit_qd, state))); + const bool ok = + bus_runtime_->writeSdo(node_id, msgs::CIA402_MAX_PROFILE_VELOCITY_607F, + 0x00, velocity_limit); + if (!ok) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write velocity limit " + << "to dictionary, node=" << static_cast(node_id) + << ", velocity_limit=" << velocity_limit; + } + return ok; +} + +bool Cia402Protocol::writeAccelerationLimitsToDictionary_( + const std::uint8_t node_id, + const NodeState& state) const +{ + if (!bus_runtime_) { + return false; + } + if (!std::isfinite(state.limit_qdd) || state.limit_qdd <= 0.0) { + return true; + } + + const auto acceleration_limit = + static_cast(std::abs(radPerSec2ToCounts_(state.limit_qdd, state))); + const bool ok = + bus_runtime_->writeSdo(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, + 0x00, acceleration_limit) && + bus_runtime_->writeSdo(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, + 0x00, acceleration_limit); + if (!ok) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to write acceleration limits " + << "to dictionary, node=" << static_cast(node_id) + << ", acceleration_limit=" << acceleration_limit; + } + return ok; +} + +void Cia402Protocol::writeProfilePositionTarget_(const std::uint8_t node_id, + NodeState& state) +{ + if (!bus_runtime_) { + return; + } + + if (state.profile_velocity <= 0 && state.limit_qd > 0.0) { + state.profile_velocity = std::abs(radPerSecToCounts_(state.limit_qd, state)); + } + if (state.profile_acceleration <= 0 && state.limit_qdd > 0.0) { + const auto profile_acceleration = std::abs(radPerSec2ToCounts_(state.limit_qdd, state)); + state.profile_acceleration = profile_acceleration; + state.profile_deceleration = profile_acceleration; + } + + state.controlword = cia402::enableOperationControlword(); + writeNode_(node_id, state); + + state.controlword = cia402::profilePositionControlword(true); + writeControlword_(node_id, state.controlword); + std::this_thread::sleep_for( + std::chrono::milliseconds(config_.profile_position_trigger_delay_ms())); + + state.controlword = cia402::profilePositionControlword(false); + writeControlword_(node_id, state.controlword); +} + +bool Cia402Protocol::prepareSafeTargetsForMode_(const std::uint8_t node_id, + const msgs::RunMode mode, + NodeState& state) +{ + state.target_velocity = 0; + state.target_torque = 0; + + switch (mode) { + case msgs::RUN_MODE_PROFILE_POSITION: + case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: { + std::int32_t actual_position = 0; + if (!readActualPosition_(node_id, actual_position)) { + CMVR_LOG(ERROR) << "[Cia402Protocol] failed to read actual " + << "position before switching mode, node=" + << static_cast(node_id) + << ", mode=" << static_cast(mode); + return false; + } + state.target_position = actual_position; + break; + } + + case msgs::RUN_MODE_PROFILE_VELOCITY: + case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: + case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT: + case msgs::RUN_MODE_HOMING: + case msgs::RUN_MODE_UNSPECIFIED: + default: + break; + } + + return true; +} + +void Cia402Protocol::writeTargetsForMode_(const std::uint8_t node_id, + const msgs::RunMode mode, + const NodeState& state) const +{ + if (!bus_runtime_) { + return; + } + + switch (mode) { + case msgs::RUN_MODE_PROFILE_POSITION: + bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_POSITION_607A, + 0x00, state.target_position); + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_VELOCITY_6081, 0x00)) { + bus_runtime_->writePdo(node_id, msgs::CIA402_PROFILE_VELOCITY_6081, + 0x00, state.profile_velocity); + } + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) { + bus_runtime_->writePdo(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, + 0x00, state.profile_acceleration); + } + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) { + bus_runtime_->writePdo(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, + 0x00, state.profile_deceleration); + } + break; + + case msgs::RUN_MODE_PROFILE_VELOCITY: + bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_VELOCITY_60FF, + 0x00, state.target_velocity); + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00)) { + bus_runtime_->writePdo(node_id, msgs::CIA402_PROFILE_ACCELERATION_6083, + 0x00, state.profile_acceleration); + } + if (bus_runtime_->hasPdoEntry(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, 0x00)) { + bus_runtime_->writePdo(node_id, msgs::CIA402_PROFILE_DECELERATION_6084, + 0x00, state.profile_deceleration); + } + break; + + case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: + bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_POSITION_607A, + 0x00, state.target_position); + bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_VELOCITY_60FF, + 0x00, state.target_velocity); + break; + + case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: + bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_VELOCITY_60FF, + 0x00, state.target_velocity); + break; + + case msgs::RUN_MODE_CYCLIC_SYNC_CURRENT: + bus_runtime_->writePdo(node_id, msgs::CIA402_TARGET_TORQUE_6071, + 0x00, state.target_torque); + break; + + default: + break; + } +} + +void Cia402Protocol::writeNode_(const std::uint8_t node_id, NodeState& state) +{ + if (!bus_runtime_) { + return; + } + + std::uint16_t statusword = 0; + if (readStatusword_(node_id, statusword)) { + const auto status = cia402::statusword(statusword); + state.controlword = nextControlword_(status); + std::int32_t actual_position = 0; + if (!isOperationEnabled_(status) && + readActualPosition_(node_id, actual_position) && + actual_position != 0) { + state.target_position = actual_position; + } + } + + bus_runtime_->writePdo(node_id, msgs::CIA402_CONTROL_WORD_6040, 0x00, + state.controlword.value); + bus_runtime_->writePdo(node_id, msgs::CIA402_OPERATION_MODE_6060, 0x00, + toCia402Mode_(state.mode)); + + writeTargetsForMode_(node_id, state.mode, state); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp new file mode 100644 index 00000000..73ae01ef --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp @@ -0,0 +1,174 @@ +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" + +#include +#include +#include + +#include "common/base/logging/logger.h" + +namespace cmvr::device { + +EyouMotor::EyouMotor(const config::MotorConfigItem& config, + std::shared_ptr cia402_protocol, + std::unique_ptr vendor_adapter) + : cia402_protocol_(std::move(cia402_protocol)), + vendor_adapter_(std::move(vendor_adapter)) +{ + info_.id = config.id(); + info_.joint_name = config.joint_name(); + info_.limit_q_lb = config.limit_q_lb(); + info_.limit_q_ub = config.limit_q_ub(); + info_.limit_qd = config.limit_qd(); + info_.limit_qdd = config.limit_qdd(); + encoder_counts_per_rev_ = config.encoder_counts_per_rev(); + gear_ratio_ = config.gear_ratio(); + node_id_ = static_cast(info_.id); + id_ = info_.joint_name; + protocol_ = cia402_protocol_; +} + +bool EyouMotor::init() +{ + std::scoped_lock lock(mtx_); + if (!hasDependencies_() || !hasValidConversion_()) { + return false; + } + + if (!cia402_protocol_->initNode(node_id_)) { + CMVR_LOG(ERROR) << "[EyouMotor] failed to init CiA402 node: " << info_.joint_name; + return false; + } + if (!vendor_adapter_->initNode(node_id_)) { + CMVR_LOG(ERROR) << "[EyouMotor] failed to init vendor adapter: " << info_.joint_name; + return false; + } + cia402_protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_); + + cia402_protocol_->setLimitQd(node_id_, info_.limit_qd); + if (!writeVendorVelocityLimit_()) { + return false; + } + if (info_.limit_qdd > 0.0) { + cia402_protocol_->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd); + } + if (!writeVendorPositionLimits_()) { + return false; + } + return true; +} + +void EyouMotor::setLimitQ(const double ub, const double lb) +{ + std::scoped_lock lock(mtx_); + info_.limit_q_ub = ub; + info_.limit_q_lb = lb; + if (!hasDependencies_() || !hasValidConversion_()) { + return; + } + writeVendorPositionLimits_(); +} + +void EyouMotor::setLimitQd(const double qd) +{ + std::scoped_lock lock(mtx_); + info_.limit_qd = qd; + if (!hasDependencies_() || !hasValidConversion_()) { + return; + } + cia402_protocol_->setLimitQd(node_id_, info_.limit_qd); + writeVendorVelocityLimit_(); +} + +bool EyouMotor::calibrateZeroQ() +{ + std::scoped_lock lock(mtx_); + if (!hasDependencies_() || !hasValidConversion_()) { + return false; + } + + std::int32_t zeroed_position = 0; + if (!vendor_adapter_->calibrateZero(node_id_, zeroed_position)) { + CMVR_LOG(ERROR) << "[EyouMotor] zero calibration failed: " << info_.joint_name; + return false; + } + if (!cia402_protocol_->syncTargetToActualPosition(node_id_)) { + return false; + } + if (!writeVendorPositionLimits_()) { + return false; + } + return true; +} + +bool EyouMotor::brakeRelease() +{ + std::scoped_lock lock(mtx_); + if (!hasDependencies_()) { + return false; + } + return vendor_adapter_->brakeRelease(node_id_); +} + +bool EyouMotor::hasDependencies_() const +{ + if (!cia402_protocol_ || !vendor_adapter_) { + CMVR_LOG(ERROR) << "[EyouMotor] missing protocol or vendor adapter: " + << info_.joint_name; + return false; + } + if (cia402_protocol_->comm_proto != MotorProtocolInterface::CommProto::ETHERCAT) { + CMVR_LOG(ERROR) << "[EyouMotor] invalid protocol for motor: " << info_.joint_name; + return false; + } + return true; +} + +bool EyouMotor::hasValidConversion_() const +{ + if (encoder_counts_per_rev_ > 0.0 && gear_ratio_ > 0.0) { + return true; + } + CMVR_LOG(ERROR) << "[EyouMotor] missing encoder conversion config: " + << info_.joint_name + << ", encoder_counts_per_rev=" << encoder_counts_per_rev_ + << ", gear_ratio=" << gear_ratio_; + return false; +} + +bool EyouMotor::writeVendorPositionLimits_() const +{ + if (!std::isfinite(info_.limit_q_lb) || !std::isfinite(info_.limit_q_ub) || + info_.limit_q_ub <= info_.limit_q_lb) { + return true; + } + + cia402_protocol_->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb); + return vendor_adapter_->writePositionLimits(node_id_, + radToCounts_(info_.limit_q_lb), + radToCounts_(info_.limit_q_ub)); +} + +bool EyouMotor::writeVendorVelocityLimit_() const +{ + if (!std::isfinite(info_.limit_qd) || info_.limit_qd <= 0.0) { + return true; + } + + return vendor_adapter_->writeVelocityLimit(node_id_, radPerSecToCounts_(info_.limit_qd)); +} + +std::int32_t EyouMotor::radToCounts_(const double angle_rad) const +{ + const double rev = angle_rad / (2.0 * M_PI); + return static_cast( + std::llround(rev * gear_ratio_ * encoder_counts_per_rev_)); +} + +std::uint32_t EyouMotor::radPerSecToCounts_(const double velocity_rad_s) const +{ + const double rev_per_sec = std::abs(velocity_rad_s) / (2.0 * M_PI); + return static_cast( + std::llround(rev_per_sec * gear_ratio_ * encoder_counts_per_rev_)); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp new file mode 100644 index 00000000..f43fee3e --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp @@ -0,0 +1,221 @@ +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" + +#include +#include +#include +#include +#include +#include + +#include "cmvr/msgs/cia402.pb.h" +#include "common/base/logging/logger.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h" + +namespace cmvr::device { + +EyouMotorAdapter::EyouMotorAdapter( + std::shared_ptr bus_runtime) + : bus_runtime_(std::move(bus_runtime)) +{ +} + +bool EyouMotorAdapter::initNode(const std::uint8_t node_id) +{ + return bus_runtime_ && bus_runtime_->hasMotor(node_id); +} + +bool EyouMotorAdapter::writePositionLimits(const std::uint8_t node_id, + const std::int32_t lower_limit, + const std::int32_t upper_limit) +{ + if (!bus_runtime_) { + return false; + } + + const auto write_limits = [&]() { + return bus_runtime_->writeSdo(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, 0) && + bus_runtime_->writeSdo( + node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x02, upper_limit) && + bus_runtime_->writeSdo( + node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x01, lower_limit) && + bus_runtime_->writeSdo( + node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, 0x4C494D54); + }; + const auto readback_matches = [&]() { + std::int32_t actual_lower = 0; + std::int32_t actual_upper = 0; + return bus_runtime_->readSdo( + node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x01, actual_lower) && + bus_runtime_->readSdo( + node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D, + 0x02, actual_upper) && + actual_lower == lower_limit && + actual_upper == upper_limit; + }; + + const bool ok = write_limits() && readback_matches(); + if (!ok) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write software position " + << "limits, node=" << static_cast(node_id) + << ", soft_limit_state=" << 0x4C494D54 + << ", lower=" << lower_limit + << ", upper=" << upper_limit; + } + return ok; +} + +bool EyouMotorAdapter::writeVelocityLimit(const std::uint8_t node_id, + const std::uint32_t velocity_limit) +{ + if (!bus_runtime_) { + return false; + } + + std::uint32_t actual_velocity_limit = 0; + const bool ok = + bus_runtime_->writeSdo(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024, + 0x00, velocity_limit) && + bus_runtime_->readSdo(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024, + 0x00, actual_velocity_limit) && + actual_velocity_limit == velocity_limit; + if (!ok) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write over speed " + << "threshold, node=" << static_cast(node_id) + << ", expected=" << velocity_limit + << ", actual=" << actual_velocity_limit; + } + return ok; +} + +bool EyouMotorAdapter::calibrateZero(const std::uint8_t node_id, + std::int32_t& zeroed_position) +{ + if (!bus_runtime_) { + return false; + } + + if (!bus_runtime_->writeSdo(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003, + 0x00, 0)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to disable software position " + << "limit before home offset calibration, node=" + << static_cast(node_id); + return false; + } + + if (!bus_runtime_->writeSdo(node_id, msgs::CIA402_HOME_OFFSET_607C, + 0x00, 0)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to clear home offset, node=" + << static_cast(node_id); + return false; + } + std::this_thread::sleep_for(std::chrono::milliseconds{50}); + + std::int32_t actual_position = 0; + if (!bus_runtime_->readSdo(node_id, msgs::CIA402_ACTUAL_POSITION_6064, + 0x00, actual_position)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position " + << "after clearing home offset, node=" + << static_cast(node_id); + return false; + } + + if (actual_position == std::numeric_limits::min()) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] invalid actual position for home " + << "offset calibration, node=" << static_cast(node_id) + << ", actual_position=" << actual_position; + return false; + } + + const auto home_offset = static_cast(-actual_position); + if (!bus_runtime_->writeSdo(node_id, msgs::CIA402_HOME_OFFSET_607C, + 0x00, home_offset)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write home offset, node=" + << static_cast(node_id) + << ", home_offset=" << home_offset; + return false; + } + + if (!bus_runtime_->writeSdo( + node_id, eyou::EYOU_STORE_PARAMETERS_1010, + 0x01, + 0x65766173)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to save home offset parameter, node=" + << static_cast(node_id); + return false; + } + std::this_thread::sleep_for(std::chrono::milliseconds{50}); + + std::int32_t home_offset_readback = 0; + if (!bus_runtime_->readSdo(node_id, msgs::CIA402_HOME_OFFSET_607C, + 0x00, home_offset_readback)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read back home offset, node=" + << static_cast(node_id); + return false; + } + if (home_offset_readback != home_offset) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset readback mismatch, node=" + << static_cast(node_id) + << ", expected=" << home_offset + << ", actual=" << home_offset_readback; + return false; + } + + if (!bus_runtime_->readSdo(node_id, msgs::CIA402_ACTUAL_POSITION_6064, + 0x00, zeroed_position)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to read actual position " + << "after writing home offset, node=" + << static_cast(node_id); + return false; + } + if (std::abs(static_cast(zeroed_position)) > 10000) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] home offset did not zero actual position, " + << "node=" << static_cast(node_id) + << ", actual_position_before=" << actual_position + << ", home_offset=" << home_offset + << ", home_offset_readback=" << home_offset_readback + << ", actual_position_after=" << zeroed_position + << ", tolerance_counts=" << 10000; + return false; + } + + return true; +} + +bool EyouMotorAdapter::brakeRelease(const std::uint8_t node_id) +{ + if (!bus_runtime_) { + return false; + } + if (!bus_runtime_->writeSdo( + node_id, eyou::EYOU_BRAKE_CONTROL_2014, + 0x01, + 1)) { + CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to release brake, node=" + << static_cast(node_id); + return false; + } + + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::milliseconds{1000}; + do { + std::uint8_t brake_state = 0; + if (bus_runtime_->readSdo( + node_id, eyou::EYOU_BRAKE_CONTROL_2014, + 0x02, brake_state) && + (brake_state == 1 || brake_state == 2)) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds{10}); + } while (std::chrono::steady_clock::now() < deadline); + + CMVR_LOG(ERROR) << "[EyouMotorAdapter] brake release timeout, node=" + << static_cast(node_id); + return false; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp new file mode 100644 index 00000000..3532cf73 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp @@ -0,0 +1,309 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "common/config/config_files.h" +#include "devices/motor/abstract_motor.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::device { +namespace { + +constexpr const char* kMotorManagerId = "ethercat_motors"; +constexpr const char* kMotorConfigFile = + "devices/motor/ethercat_motors_two_real_test.pb.txt"; +constexpr std::array kFourMotorIds{1, 2, 3, 4}; +constexpr std::chrono::milliseconds kCyclicCommandPeriod{1}; +constexpr std::chrono::milliseconds kPrintPeriod{100}; +constexpr std::chrono::milliseconds kStatsSamplePeriod{10}; +constexpr std::chrono::milliseconds kHoldAfterTrajectoryDuration{500}; +constexpr std::chrono::milliseconds kFourMotorTrajectoryDuration{20000}; +constexpr double kPi = 3.14159265358979323846; +constexpr double kFourMotorAmplitudeRad = 0.2; +constexpr double kFourMotorPeriodS = 1.0; +constexpr std::array kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0}; + +class DeviceManagerDestroyGuard { +public: + ~DeviceManagerDestroyGuard() + { + DeviceManager::destroyInstance(); + } +}; + +struct TrackingErrorStats { + std::int64_t sample_count{0}; + double sum_error{0.0}; + double sum_error_sq{0.0}; + double max_abs_error{0.0}; + double sin_projection{0.0}; + double cos_projection{0.0}; + + void add(const double error, const double theta) + { + ++sample_count; + sum_error += error; + sum_error_sq += error * error; + max_abs_error = std::max(max_abs_error, std::fabs(error)); + sin_projection += error * std::sin(theta); + cos_projection += error * std::cos(theta); + } + + double mean() const + { + return sample_count > 0 ? sum_error / static_cast(sample_count) : 0.0; + } + + double rms() const + { + return sample_count > 0 + ? std::sqrt(sum_error_sq / static_cast(sample_count)) + : 0.0; + } + + double fundamentalAmplitude() const + { + if (sample_count == 0) { + return 0.0; + } + const double scale = 2.0 / static_cast(sample_count); + return scale * std::sqrt(sin_projection * sin_projection + + cos_projection * cos_projection); + } + + double phaseRad() const + { + return std::atan2(cos_projection, sin_projection); + } +}; + +double normalizePhaseRad(double phase) +{ + while (phase > kPi) { + phase -= 2.0 * kPi; + } + while (phase < -kPi) { + phase += 2.0 * kPi; + } + return phase; +} + +double radToDeg(const double rad) +{ + return rad * 180.0 / kPi; +} + +config::DeviceManagerConfig createEthercatOnlyDeviceManagerConfig() +{ + config::DeviceManagerConfig config; + config.set_name("eyou_motor_device_manager_real_test"); + config.set_version("test"); + config.set_init_all_motors_when_no_active_joints(true); + + auto* motor_entry = config.add_devices(); + motor_entry->set_id(kMotorManagerId); + motor_entry->set_type(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM); + motor_entry->set_config_file(kMotorConfigFile); + motor_entry->set_enable(true); + + return config; +} + +void printMotorState(const int motor_id, const std::shared_ptr& motor) +{ + ASSERT_NE(motor, nullptr); + std::cout << "motor_id=" << motor_id + << ", joint_name=" << motor->jointName() + << ", q=" << motor->getQ() << " rad" + << ", qd=" << motor->getQd() << " rad/s" + << std::endl; +} + +} // namespace + +TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState) +{ + ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt"); + DeviceManagerDestroyGuard guard; + + auto& device_manager = + DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig()); + auto motor_manager = device_manager.getDevice(kMotorManagerId); + ASSERT_NE(motor_manager, nullptr); + + // for (int motor_id = 1; motor_id <= 4; ++motor_id) { + // printMotorState(motor_id, motor_manager->getMotor(motor_id)); + // } + + auto motor = motor_manager->getMotor(4); + motor->calibrateZeroQ(); + + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); + motor->commandProfileVelocity(-2,5); + + + for (int i = 1; i <= 50; ++i) + { + auto q = motor->getQ(); + auto qd = motor->getQd(); + std::cout << "q=" << q << ", qd=" << qd << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + motor->quickStop(); + +} + +TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory) +{ + ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt"); + DeviceManagerDestroyGuard guard; + + auto& device_manager = + DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig()); + auto motor_manager = device_manager.getDevice(kMotorManagerId); + ASSERT_NE(motor_manager, nullptr); + + std::array, kFourMotorIds.size()> motors; + for (std::size_t i = 0; i < kFourMotorIds.size(); ++i) { + const int motor_id = kFourMotorIds[i]; + motors[i] = motor_manager->getMotor(static_cast(motor_id)); + ASSERT_NE(motors[i], nullptr); + printMotorState(motor_id, motors[i]); + } + + std::cout << "calibrate zero for four EtherCAT motors" << std::endl; + for (std::size_t i = 0; i < motors.size(); ++i) { + const int motor_id = kFourMotorIds[i]; + std::cout << "before calibrateZeroQ: "; + printMotorState(motor_id, motors[i]); + ASSERT_TRUE(motors[i]->calibrateZeroQ()); + std::cout << "after calibrateZeroQ: "; + printMotorState(motor_id, motors[i]); + } + + for (const auto& motor : motors) { + ASSERT_TRUE(motor->torqueOn()); + } + for (const auto& motor : motors) { + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + } + + std::array center_q{}; + for (std::size_t i = 0; i < motors.size(); ++i) { + center_q[i] = motors[i]->getQ(); + } + + const double omega = 2.0 * kPi / kFourMotorPeriodS; + std::cout << "command four motors in CSP, duration=" + << kFourMotorTrajectoryDuration.count() + << " ms, command_period=" << kCyclicCommandPeriod.count() + << " ms, amplitude=" << kFourMotorAmplitudeRad + << " rad, period=" << kFourMotorPeriodS + << " s" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = kFourMotorTrajectoryDuration / kCyclicCommandPeriod; + std::array error_stats; + double max_error_spread_rad = 0.0; + double sum_error_spread_sq = 0.0; + std::int64_t error_spread_sample_count = 0; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + + std::array target_q{}; + std::array target_qd{}; + for (std::size_t i = 0; i < motors.size(); ++i) { + const double theta = omega * t_s + kFourMotorPhaseRad[i]; + target_q[i] = center_q[i] + kFourMotorAmplitudeRad * (1.0 - std::cos(theta)); + target_qd[i] = kFourMotorAmplitudeRad * omega * std::sin(theta); + ASSERT_TRUE(motors[i]->commandCyclicPosition(target_q[i], target_qd[i])); + } + + if (elapsed.count() % kStatsSamplePeriod.count() == 0) { + double min_error = std::numeric_limits::max(); + double max_error = std::numeric_limits::lowest(); + for (std::size_t i = 0; i < motors.size(); ++i) { + const double theta = omega * t_s + kFourMotorPhaseRad[i]; + const double error = motors[i]->getQ() - target_q[i]; + error_stats[i].add(error, theta); + min_error = std::min(min_error, error); + max_error = std::max(max_error, error); + } + const double error_spread = max_error - min_error; + max_error_spread_rad = std::max(max_error_spread_rad, error_spread); + sum_error_spread_sq += error_spread * error_spread; + ++error_spread_sample_count; + } + + // if (elapsed.count() % kPrintPeriod.count() == 0) { + // std::cout << "t=" << elapsed.count() << " ms" << std::endl; + // for (std::size_t i = 0; i < motors.size(); ++i) { + // std::cout << " motor_id=" << kFourMotorIds[i] + // << ", target_q=" << target_q[i] + // << " rad, target_qd=" << target_qd[i] + // << " rad/s, q=" << motors[i]->getQ() + // << " rad, qd=" << motors[i]->getQd() + // << " rad/s" << std::endl; + // } + // } + + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } + + const auto hold_start_time = std::chrono::steady_clock::now(); + const auto hold_ticks = kHoldAfterTrajectoryDuration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= hold_ticks; ++tick) { + for (std::size_t i = 0; i < motors.size(); ++i) { + ASSERT_TRUE(motors[i]->commandCyclicPosition(center_q[i], 0.0)); + } + std::this_thread::sleep_until(hold_start_time + (tick + 1) * kCyclicCommandPeriod); + } + + std::cout << "after four motor CSP trajectory" << std::endl; + for (std::size_t i = 0; i < motors.size(); ++i) { + printMotorState(kFourMotorIds[i], motors[i]); + } + + const double reference_phase = error_stats.front().phaseRad(); + const double rms_error_spread = + error_spread_sample_count > 0 + ? std::sqrt(sum_error_spread_sq / static_cast(error_spread_sample_count)) + : 0.0; + std::cout << "four motor CSP tracking error statistics, sample_period=" + << kStatsSamplePeriod.count() + << " ms, samples=" << error_stats.front().sample_count + << ", max_error_spread=" << max_error_spread_rad + << " rad, rms_error_spread=" << rms_error_spread + << " rad" << std::endl; + for (std::size_t i = 0; i < motors.size(); ++i) { + const double phase = error_stats[i].phaseRad(); + const double relative_phase = normalizePhaseRad(phase - reference_phase); + const double relative_phase_ms = relative_phase / omega * 1000.0; + std::cout << " motor_id=" << kFourMotorIds[i] + << ", mean_error=" << error_stats[i].mean() + << " rad, rms_error=" << error_stats[i].rms() + << " rad, max_abs_error=" << error_stats[i].max_abs_error + << " rad, error_fundamental_amp=" + << error_stats[i].fundamentalAmplitude() + << " rad, error_phase=" << phase + << " rad (" << radToDeg(phase) + << " deg), relative_phase_to_motor1=" << relative_phase + << " rad (" << radToDeg(relative_phase) + << " deg, " << relative_phase_ms + << " ms)" << std::endl; + } +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp new file mode 100644 index 00000000..80a1156c --- /dev/null +++ b/cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp @@ -0,0 +1,559 @@ +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "cmvr/msgs/cia402.pb.h" +#include "devices/motor/abstract_motor.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" + +namespace cmvr::device { +namespace { + +constexpr int kMotorId = 1; +constexpr std::chrono::milliseconds kModeSettleDelay{100}; +constexpr std::chrono::milliseconds kCommandSamplePeriod{100}; +constexpr std::chrono::milliseconds kCyclicCommandPeriod{1}; +constexpr std::chrono::milliseconds kFeedbackSampleDuration{5000}; +constexpr double kDefaultGearRatio = 101.0; +constexpr double kEncoderCountsPerMotorRev = 65536.0; +constexpr double kPi = 3.14159265358979323846; + +config::MotorGroupConfig createSingleSlaveGroup() +{ + config::MotorGroupConfig group; + group.set_id("eyou_motor_real_test"); + group.set_bus_type(config::MOTOR_BUS_ETHERCAT); + group.set_vendor(config::MOTOR_VENDOR_EYOU); + group.set_protocol(config::MOTOR_PROTOCOL_ETHERCAT_CIA402); + + auto* ethercat = group.mutable_ethercat(); + ethercat->set_master_index(0); + ethercat->set_cycle_us(1000); + ethercat->set_slave_op_timeout_ms(12000); + ethercat->set_slave_state_poll_period_ms(10); + + auto* cia402 = ethercat->mutable_cia402(); + cia402->set_profile_position_trigger_delay_ms(2); + cia402->set_state_transition_timeout_ms(1200); + cia402->set_velocity_stop_timeout_ms(2000); + cia402->set_status_poll_period_ms(10); + cia402->set_stopped_velocity_tolerance_rad_s(0.001); + + auto* dc = ethercat->mutable_dc(); + dc->set_enable(true); + dc->set_reference_motor_id(kMotorId); + dc->set_sync0_cycle_us(1000); + dc->set_sync0_shift_us(0); + dc->set_sync_reference_clock_period(1); + dc->set_assign_activate(768); + dc->set_sync_monitor_period_ms(1000); + + auto* slave = ethercat->add_slaves(); + slave->set_motor_id(kMotorId); + slave->set_alias(0); + slave->set_position(0); + + return group; +} + +class RuntimeStopGuard { +public: + explicit RuntimeStopGuard(std::shared_ptr runtime) + : runtime_(std::move(runtime)) + { + } + + ~RuntimeStopGuard() + { + if (runtime_) { + runtime_->stop(); + } + } + +private: + std::shared_ptr runtime_; +}; + +std::shared_ptr startRuntime() +{ + auto runtime = std::make_shared(); + runtime->setPdoMapping(createEyouCia402PdoMapping()); + if (!runtime->init(createSingleSlaveGroup())) { + return nullptr; + } + if (!runtime->start()) { + runtime->stop(); + return nullptr; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + return runtime; +} + +std::shared_ptr createProtocol( + const std::shared_ptr& runtime) +{ + return std::make_shared(runtime, runtime->config().cia402()); +} + +config::MotorConfigItem createMotorConfig() +{ + config::MotorConfigItem config; + config.set_id(kMotorId); + config.set_joint_name("ethercat_test_joint"); + config.set_limit_q_lb(-6.14); + config.set_limit_q_ub(6.14); + config.set_limit_qd(10.0); + config.set_limit_qdd(10.0); + config.set_encoder_counts_per_rev(kEncoderCountsPerMotorRev); + config.set_gear_ratio(kDefaultGearRatio); + return config; +} + +std::unique_ptr createMotor( + const std::shared_ptr& runtime) +{ + auto motor = std::make_unique( + createMotorConfig(), + createProtocol(runtime), + std::make_unique(runtime)); + if (!motor->init()) { + return nullptr; + } + return motor; +} + +void printMotorState(const char* label, AbstractMotor& motor) +{ + std::cout << label + << ": motor_q=" << motor.getQ() << " rad" + << ", motor_qd=" << motor.getQd() << " rad/s" + << std::endl; +} + +std::string hex16(const std::uint16_t value) +{ + std::ostringstream oss; + oss << "0x" << std::uppercase << std::hex << std::setw(4) << std::setfill('0') + << value; + return oss.str(); +} + +void printRawEthercatFeedback(const char* label, + const std::shared_ptr& runtime) +{ + std::uint16_t statusword = 0; + std::int8_t mode_display = 0; + std::int32_t actual_position = 0; + std::int32_t actual_velocity = 0; + std::int16_t actual_torque = 0; + std::uint16_t error_code = 0; + + runtime->readPdo(kMotorId, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword); + runtime->readPdo(kMotorId, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display); + runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_POSITION_6064, 0x00, + actual_position); + runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, + actual_velocity); + runtime->readPdo(kMotorId, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, + actual_torque); + runtime->readPdo(kMotorId, msgs::CIA402_ERROR_CODE_603F, 0x00, error_code); + + std::cout << label + << ": statusword=" << hex16(statusword) + << ", mode_display=" << static_cast(mode_display) + << ", actual_position=" << actual_position + << ", actual_velocity=" << actual_velocity + << ", actual_torque=" << actual_torque + << ", error_code=" << hex16(error_code) + << std::endl; +} + +void sampleMotorState(AbstractMotor& motor, + const std::chrono::milliseconds duration) +{ + for (auto elapsed = std::chrono::milliseconds{0}; + elapsed < duration; + elapsed += kCommandSamplePeriod) { + std::this_thread::sleep_for(kCommandSamplePeriod); + std::cout << "t=" << (elapsed + kCommandSamplePeriod).count() << " ms"; + printMotorState("", motor); + } +} + +double nearbySafeTarget(const double current_q, const double delta_rad) +{ + return current_q + (current_q > 0.0 ? -std::abs(delta_rad) : std::abs(delta_rad)); +} + +} // namespace + +TEST(EyouMotorRealTest, ReadMotorStateOnly) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); + printMotorState("motor state", *motor); + printRawEthercatFeedback("raw feedback", runtime); + for (int i = 1; i <= 10; ++i) { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + printMotorState("motor state", *motor); + printRawEthercatFeedback("raw feedback", runtime); + } +} + +TEST(EyouMotorRealTest, CalibrateZeroQPrintBeforeAndAfter) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + printMotorState("before calibrateZeroQ", *motor); + ASSERT_TRUE(motor->calibrateZeroQ()); + printMotorState("after calibrateZeroQ", *motor); + ASSERT_TRUE(motor->torqueOn()); +} + +TEST(EyouMotorRealTest, CommandProfilePosition) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); + std::this_thread::sleep_for(kModeSettleDelay); + + ASSERT_TRUE(motor->commandProfilePosition(-3.0, 0.5, 1.0)); + sampleMotorState(*motor, kFeedbackSampleDuration); +} + +TEST(EyouMotorRealTest, CommandProfileVelocity) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); + std::this_thread::sleep_for(kModeSettleDelay); + + std::cout << "motor.commandProfileVelocity(0.3 rad/s, 1.0 rad/s^2)" << std::endl; + ASSERT_TRUE(motor->commandProfileVelocity(0.3, 1.0)); + sampleMotorState(*motor, kFeedbackSampleDuration); + + std::cout << "motor.commandProfileVelocity(0 rad/s, 1.0 rad/s^2)" << std::endl; + ASSERT_TRUE(motor->commandProfileVelocity(0.0, 1.0)); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); +} + +TEST(EyouMotorRealTest, CommandCyclicPosition) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + std::this_thread::sleep_for(kModeSettleDelay); + + const std::chrono::milliseconds trajectory_duration{15000}; + const double period_s = 6.0; + const double amplitude_rad = 3; + const double phase_rad = 0.0; + const double center_q = motor->getQ(); + const double omega = 2.0 * kPi / period_s; + + std::cout << "motor.commandCyclicPosition(sin), center_q=" << center_q + << " rad, period=" << period_s + << " s, amplitude=" << amplitude_rad + << " rad, phase=" << phase_rad + << " rad, command_period=" << kCyclicCommandPeriod.count() + << " ms" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = trajectory_duration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + const double theta = omega * t_s + phase_rad; + const double target_q = center_q + amplitude_rad * std::sin(theta); + const double target_qd = amplitude_rad * omega * std::cos(theta); + + ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd)); + if (elapsed.count() % kCommandSamplePeriod.count() == 0) { + std::cout << "t=" << elapsed.count() + << " ms, target_q=" << target_q + << " rad, target_qd=" << target_qd + << " rad/s"; + printMotorState("", *motor); + } + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } +} + +TEST(EyouMotorRealTest, CommandCyclicVelocity) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); + std::this_thread::sleep_for(kModeSettleDelay); + + const std::chrono::milliseconds trajectory_duration{15000}; + const double period_s = 6.0; + const double velocity_amplitude_rad_s = 5.0; + const double phase_rad = 0.0; + const double omega = 2.0 * kPi / period_s; + + std::cout << "motor.commandCyclicVelocity(sin), period=" << period_s + << " s, velocity_amplitude=" << velocity_amplitude_rad_s + << " rad/s, phase=" << phase_rad + << " rad, command_period=" << kCyclicCommandPeriod.count() + << " ms" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = trajectory_duration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + const double theta = omega * t_s + phase_rad; + const double target_qd = velocity_amplitude_rad_s * std::sin(theta); + + ASSERT_TRUE(motor->commandCyclicVelocity(target_qd)); + if (elapsed.count() % kCommandSamplePeriod.count() == 0) { + std::cout << "t=" << elapsed.count() + << " ms, target_qd=" << target_qd + << " rad/s"; + printMotorState("", *motor); + } + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } + + std::cout << "motor.commandCyclicVelocity(0 rad/s)" << std::endl; + ASSERT_TRUE(motor->commandCyclicVelocity(0.0)); + sampleMotorState(*motor, std::chrono::milliseconds{500}); +} + +TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); + std::this_thread::sleep_for(kModeSettleDelay); + + const std::chrono::milliseconds run_duration{2000}; + const double period_s = 6.0; + const double velocity_amplitude_rad_s = 4.5; + const double phase_rad = 0.0; + const double omega = 2.0 * kPi / period_s; + + std::cout << "motor.commandCyclicVelocity(sin), then quickStop at " + << run_duration.count() + << " ms, period=" << period_s + << " s, velocity_amplitude=" << velocity_amplitude_rad_s + << " rad/s, phase=" << phase_rad + << " rad, command_period=" << kCyclicCommandPeriod.count() + << " ms" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = run_duration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + const double theta = omega * t_s + phase_rad; + const double target_qd = velocity_amplitude_rad_s * std::sin(theta); + + ASSERT_TRUE(motor->commandCyclicVelocity(target_qd)); + if (elapsed.count() % kCommandSamplePeriod.count() == 0) { + std::cout << "t=" << elapsed.count() + << " ms, target_qd=" << target_qd + << " rad/s"; + printMotorState("", *motor); + printRawEthercatFeedback("raw feedback", runtime); + } + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } + + std::cout << "motor.quickStop()" << std::endl; + ASSERT_TRUE(motor->quickStop()); + printMotorState("after quickStop", *motor); + printRawEthercatFeedback("raw feedback", runtime); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); + printRawEthercatFeedback("raw feedback", runtime); +} + +TEST(EyouMotorRealTest, QuickStopInProfilePosition) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); + std::this_thread::sleep_for(kModeSettleDelay); + + const std::chrono::milliseconds quick_stop_time{1000}; + const double start_q = motor->getQ(); + const double target_q = nearbySafeTarget(start_q, 4.0); + const double max_qd = 2.0; + const double max_qdd = 10.0; + + std::cout << "motor.commandProfilePosition(" << target_q + << " rad, " << max_qd + << " rad/s, " << max_qdd + << " rad/s^2), then quickStop at " + << quick_stop_time.count() << " ms" << std::endl; + ASSERT_TRUE(motor->commandProfilePosition(target_q, max_qd, max_qdd)); + std::cout << "wait " << quick_stop_time.count() + << " ms before quickStop" << std::endl; + sampleMotorState(*motor, quick_stop_time); + printRawEthercatFeedback("raw feedback before quickStop", runtime); + + std::cout << "motor.quickStop()" << std::endl; + ASSERT_TRUE(motor->quickStop()); + printMotorState("after quickStop", *motor); + printRawEthercatFeedback("raw feedback", runtime); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); + printRawEthercatFeedback("raw feedback", runtime); +} + +TEST(EyouMotorRealTest, QuickStopInProfileVelocity) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); + std::this_thread::sleep_for(kModeSettleDelay); + + const std::chrono::milliseconds quick_stop_time{2000}; + const double target_qd = motor->getQ() > 0.0 ? -2.0 : 2.0; + const double max_qdd = 10.0; + + std::cout << "motor.commandProfileVelocity(" << target_qd + << " rad/s, " << max_qdd + << " rad/s^2), then quickStop at " + << quick_stop_time.count() << " ms" << std::endl; + ASSERT_TRUE(motor->commandProfileVelocity(target_qd, max_qdd)); + std::cout << "wait " << quick_stop_time.count() + << " ms before quickStop" << std::endl; + sampleMotorState(*motor, quick_stop_time); + printRawEthercatFeedback("raw feedback before quickStop", runtime); + + std::cout << "motor.quickStop()" << std::endl; + ASSERT_TRUE(motor->quickStop()); + printMotorState("after quickStop", *motor); + printRawEthercatFeedback("raw feedback", runtime); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); + printRawEthercatFeedback("raw feedback", runtime); +} + +TEST(EyouMotorRealTest, QuickStopInCyclicPosition) +{ + auto runtime = startRuntime(); + ASSERT_NE(runtime, nullptr); + RuntimeStopGuard runtime_guard(runtime); + + auto motor = createMotor(runtime); + ASSERT_NE(motor, nullptr); + + ASSERT_TRUE(motor->torqueOff()); + ASSERT_TRUE(motor->calibrateZeroQ()); + ASSERT_TRUE(motor->torqueOn()); + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + std::this_thread::sleep_for(kModeSettleDelay); + + const std::chrono::milliseconds run_duration{2000}; + const double start_q = motor->getQ(); + const double target_qd = start_q > 0.0 ? -2.0 : 2.0; + + std::cout << "motor.commandCyclicPosition(linear), start_q=" << start_q + << " rad, target_qd=" << target_qd + << " rad/s, then quickStop at " + << run_duration.count() + << " ms, command_period=" << kCyclicCommandPeriod.count() + << " ms" << std::endl; + + const auto start_time = std::chrono::steady_clock::now(); + const auto total_ticks = run_duration / kCyclicCommandPeriod; + for (std::int64_t tick = 0; tick <= total_ticks; ++tick) { + const auto elapsed = tick * kCyclicCommandPeriod; + const double t_s = static_cast(elapsed.count()) / 1000.0; + const double target_q = start_q + target_qd * t_s; + + ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd)); + if (elapsed.count() % kCommandSamplePeriod.count() == 0) { + std::cout << "t=" << elapsed.count() + << " ms, target_q=" << target_q + << " rad, target_qd=" << target_qd + << " rad/s"; + printMotorState("", *motor); + printRawEthercatFeedback("raw feedback", runtime); + } + std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod); + } + + std::cout << "motor.quickStop()" << std::endl; + ASSERT_TRUE(motor->quickStop()); + printMotorState("after quickStop", *motor); + printRawEthercatFeedback("raw feedback", runtime); + sampleMotorState(*motor, std::chrono::milliseconds{1000}); + printRawEthercatFeedback("raw feedback", runtime); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h b/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h index b0293806..5eb7b5e3 100644 --- a/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h +++ b/cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h @@ -23,19 +23,25 @@ public: void setMode(msgs::RunMode mode) override; msgs::RunMode getMode() override; - void torqueOff() override; + bool torqueOn() override; + bool torqueOff() override; + bool brakeRelease() override; + bool quickStop() override; void setLimitQ(double ub, double lb) override; void setLimitQd(double qd) override; void setLimitQdd(double u_qdd, double l_qdd) override; - void brake() override; - void setQ(double q) override; - void setTarget(double q, double qd) override; - void setTarget(double qd) override; bool calibrateZeroQ() override; bool reachedTargetQ() override; - void setQd(double qd) override; + bool commandProfilePosition(double target_q, + double max_qd = 0.0, + double max_qdd = 0.0) override; + bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) override; + bool commandCyclicPosition(double target_q, + double target_qd = 0.0) override; + bool commandCyclicVelocity(double target_qd) override; + bool commandCyclicTorque(double target_tau) override; double getQ() override; double getQd() override; @@ -44,6 +50,7 @@ public: const std::vector& velocities); private: + bool holdPosition_(); double clampQ_(double q) const; double clampQd_(double qd) const; std::shared_ptr worldLocked_() const; diff --git a/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp b/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp index 160c1005..07b0bda4 100644 --- a/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp +++ b/cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp @@ -54,11 +54,27 @@ msgs::RunMode MujocoMotor::getMode() return mode_; } -void MujocoMotor::torqueOff() +bool MujocoMotor::torqueOn() { - brake(); + return holdPosition_(); +} + +bool MujocoMotor::torqueOff() +{ + const bool ok = holdPosition_(); std::scoped_lock lock(mtx_); mode_ = msgs::RUN_MODE_UNSPECIFIED; + return ok; +} + +bool MujocoMotor::brakeRelease() +{ + return true; +} + +bool MujocoMotor::quickStop() +{ + return holdPosition_(); } void MujocoMotor::setLimitQ(const double ub, const double lb) @@ -82,38 +98,78 @@ void MujocoMotor::setLimitQdd(const double u_qdd, const double l_qdd) info_.limit_qdd = std::max(limit_qdd_upper_, std::abs(limit_qdd_lower_)); } -void MujocoMotor::brake() +bool MujocoMotor::holdPosition_() { const auto world = worldLocked_(); double q = 0.0; if (!world || !world->getJointPosition(info_.joint_name, q)) { - return; + return false; } std::scoped_lock lock(mtx_); target_q_ = q; mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; world->setJointTargetState(info_.joint_name, q, 0.0); + return true; } -void MujocoMotor::setQ(const double q) +bool MujocoMotor::commandProfilePosition(const double target_q, + const double max_qd, + const double max_qdd) { - setTarget(q, 0.0); + (void)max_qdd; + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + return false; + } + mode_ = msgs::RUN_MODE_PROFILE_POSITION; + target_q_ = clampQ_(target_q); + const double profile_qd = max_qd > 0.0 ? max_qd : info_.limit_qd; + return world->setJointTargetState(info_.joint_name, target_q_, clampQd_(profile_qd)); } -void MujocoMotor::setTarget(const double q, const double qd) +bool MujocoMotor::commandProfileVelocity(const double target_qd, const double max_qdd) +{ + (void)max_qdd; + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + return false; + } + mode_ = msgs::RUN_MODE_PROFILE_VELOCITY; + return world->setJointTargetVelocity(info_.joint_name, clampQd_(target_qd)); +} + +bool MujocoMotor::commandCyclicPosition(const double target_q, + const double target_qd) { std::scoped_lock lock(mtx_); const auto world = worldLocked_(); if (!world) { - return; + return false; } - target_q_ = clampQ_(q); - world->setJointTargetState(info_.joint_name, target_q_, clampQd_(qd)); + mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + target_q_ = clampQ_(target_q); + return world->setJointTargetState(info_.joint_name, target_q_, clampQd_(target_qd)); } -void MujocoMotor::setTarget(const double qd) +bool MujocoMotor::commandCyclicVelocity(const double target_qd) { - setQd(qd); + std::scoped_lock lock(mtx_); + const auto world = worldLocked_(); + if (!world) { + return false; + } + mode_ = msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; + return world->setJointTargetVelocity(info_.joint_name, clampQd_(target_qd)); +} + +bool MujocoMotor::commandCyclicTorque(const double target_tau) +{ + (void)target_tau; + CMVR_LOG(ERROR) << "[MujocoMotor] cyclic torque command is not implemented: " + << info_.joint_name; + return false; } bool MujocoMotor::calibrateZeroQ() @@ -140,16 +196,6 @@ bool MujocoMotor::reachedTargetQ() } } -void MujocoMotor::setQd(const double qd) -{ - std::scoped_lock lock(mtx_); - const auto world = worldLocked_(); - if (!world) { - return; - } - world->setJointTargetVelocity(info_.joint_name, clampQd_(qd)); -} - double MujocoMotor::getQ() { const auto world = worldLocked_(); diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h index 64db8ea2..d1fc84ae 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h @@ -22,6 +22,8 @@ namespace cmvr { info_.limit_q_ub = config.limit_q_ub(); info_.limit_qd = config.limit_qd() > 0.0 ? config.limit_qd() : 0.5; info_.limit_qdd = config.limit_qdd() > 0.0 ? config.limit_qdd() : 10.0; + encoder_counts_per_rev_ = config.encoder_counts_per_rev(); + gear_ratio_ = config.gear_ratio(); node_id_ = info_.id; } @@ -37,6 +39,19 @@ namespace cmvr { } if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) { auto canopen_protocol = std::dynamic_pointer_cast(protocol_); + if (!canopen_protocol) { + CMVR_LOG(ERROR) << "[Ti5Motor] invalid CANopen protocol for motor: " + << info_.joint_name; + return false; + } + if (encoder_counts_per_rev_ <= 0.0 || gear_ratio_ <= 0.0) { + CMVR_LOG(ERROR) << "[Ti5Motor] missing encoder conversion config: " + << info_.joint_name + << ", encoder_counts_per_rev=" << encoder_counts_per_rev_ + << ", gear_ratio=" << gear_ratio_; + return false; + } + protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_); // torqueOff(node_id_); canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_RESET_COMMUNICATION); // canopen_protocol->torqueOff(node_id_); @@ -45,8 +60,8 @@ namespace cmvr { canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL); canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_START_REMOTE_NODE); canopen_protocol->setMode(node_id_,msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15); - // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15); + // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15); + // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15); canopen_protocol->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb); canopen_protocol->setLimitQd(node_id_, info_.limit_qd); canopen_protocol->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd); @@ -55,6 +70,10 @@ namespace cmvr { } return true; } + + private: + double encoder_counts_per_rev_{0.0}; + double gear_ratio_{0.0}; }; diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h index e837cd89..0588fd3f 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h @@ -18,6 +18,7 @@ #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h" #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h" #include +#include namespace cmvr { namespace device { @@ -29,28 +30,40 @@ namespace cmvr { bool initNode(uint8_t node_id) override; - void setMode(uint8_t node_id, msgs::RunMode mode); - void setTarget(uint8_t node_id, double angle_rad, double vel) override; - void setTarget(uint8_t node_id, double vel) override; - void setQ(uint8_t node_id, double angle_rad) override; + void setMode(uint8_t node_id, msgs::RunMode mode) override; void setLimitQ(uint8_t node_id, double ub, double lb) override; void setLimitQd(uint8_t node_id, double qd) override; void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override; bool calibrateZeroQ(uint8_t node_id) override; - void brake(uint8_t node_id) override; + bool torqueOn(uint8_t node_id) override; + bool torqueOff(uint8_t node_id) override; + bool brakeRelease(uint8_t node_id) override; + bool quickStop(uint8_t node_id) override; bool reachedTargetQ(uint8_t node_id) override; double getQ(uint8_t node_id) override; double getQd(uint8_t node_id) override; - void setQd(uint8_t node_id, double qd) override; - void setQdd(uint8_t node_id, double qdd) override; - - void torqueOff(uint8_t node_id) override; + bool commandProfilePosition(uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) override; + bool commandProfileVelocity(uint8_t node_id, + double target_qd, + double max_qdd) override; + bool commandCyclicPosition(uint8_t node_id, + double target_q, + double target_qd) override; + bool commandCyclicVelocity(uint8_t node_id, + double target_qd) override; + bool commandCyclicTorque(uint8_t node_id, double target_tau) override; + void setMotorConversion(uint8_t node_id, + double encoder_counts_per_rev, + double gear_ratio) override; void seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms = 10); - void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index, - msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms = 10); + void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, uint32_t index, + uint32_t sub_index, uint32_t data, uint32_t delay_ms = 10); void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel); void configPdo(uint8_t node_id); @@ -70,14 +83,20 @@ namespace cmvr { private: - static constexpr double GearRatio = 101.0; // 电机减速比 static constexpr double RADTODEG = 180.0 / M_PI; + static constexpr double Ti5VelocityUnitScale = 100.0; + static constexpr double Ti5AccelerationTimeScale = 1000.0; + + struct MotorConversion { + double encoder_counts_per_rev{0.0}; + double gear_ratio{0.0}; + }; + std::shared_ptr can_client_{nullptr}; // key node_id // std::unordered_map cur_mode_{}; - std::unordered_map last_Qd_{}; - std::unordered_map last_Qdd_{}; + std::unordered_map motor_conversions_{}; std::shared_ptr > can_sender_{nullptr}; std::shared_ptr > message_manager_{nullptr}; @@ -94,11 +113,7 @@ namespace cmvr { std::map rpdo1_commands_{}; std::map rpdo2_commands_{}; - void setPPTargetPosBySdo(uint8_t node_id, int32_t pos); - - void setPPTargetPosByPdo(uint8_t node_id, int32_t pos); - - void setCSPTargetPosByPdo(uint8_t node_id, int32_t pos); + void writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos); void configTPDO1(uint8_t node_id); @@ -107,6 +122,14 @@ namespace cmvr { void configRPDO1(uint8_t node_id, bool enable); void configRPDO2(uint8_t node_id, bool enable); + const MotorConversion* conversionForNode(uint8_t node_id) const; + double radToCounts(double angle_rad, const MotorConversion& conversion) const; + double countsToRad(int32_t counts, const MotorConversion& conversion) const; + double radPerSecToVelocityRaw(double velocity_rad_s, const MotorConversion& conversion) const; + uint32_t radPerSec2ToAccelerationRaw(double acceleration_rad_s2, + const MotorConversion& conversion) const; + double velocityRawToRadPerSec(int32_t velocity_raw, const MotorConversion& conversion) const; + bool waitUntil(std::function condition, int timeout_ms) { auto start = std::chrono::steady_clock::now(); diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp index b5486dec..4410332d 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp @@ -3,6 +3,7 @@ // Created by lgv on 2025/7/24. // +#include "cmvr/msgs/cia402.pb.h" #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h" using namespace cmvr::device::motor; @@ -19,17 +20,17 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response, switch (sdo_response.index()) { - case msgs::CONTROL_WORD_6040: + case msgs::CIA402_CONTROL_WORD_6040: motor_status->set_ctrl_word(sdo_response.data()); break; - case msgs::STATUS_WORD_6041: + case msgs::CIA402_STATUS_WORD_6041: motor_status->set_status_word(sdo_response.data()); break; - case msgs::ACTUAL_POSITION_6064: + case msgs::CIA402_ACTUAL_POSITION_6064: motor_status->set_position(static_cast(sdo_response.data())); CMVR_LOG(INFO) << "pos = " << motor_status->position(); break; - case msgs::POSITION_OFFSET_2008: + case msgs::CANOPEN_POSITION_OFFSET_2008: motor_status->set_position_offset(sdo_response.data()); } // diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp index e4c98b36..d1f67403 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp @@ -19,16 +19,4 @@ void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]); motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]); - - - double gearRatio = 101.0; - double radToDeg = 180.0 / M_PI; - auto speed = (motor_status->speed() * 360.0) / (radToDeg * gearRatio * 100.0); - - auto angle_rad = (motor_status->position() * 360.0) / (gearRatio * 65536.0 * radToDeg); - - // CMVR_LOG(INFO) << " Motor ID " << int(this->node_id_) << " pos = " << angle_rad << " rad speed = " << speed << " rad/s"; - - - } diff --git a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp index c5546b2a..69e1f562 100644 --- a/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp +++ b/cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp @@ -3,6 +3,7 @@ // Created by lgv on 2025/8/1. // +#include "cmvr/msgs/cia402.pb.h" #include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" #include "canbus/canopen/register.h" #include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h" @@ -94,45 +95,153 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) { return ErrorCode::OK; } +void Ti5MotorCanopenProtocol::setMotorConversion( + const uint8_t node_id, + const double encoder_counts_per_rev, + const double gear_ratio) { + motor_conversions_[node_id] = {encoder_counts_per_rev, gear_ratio}; +} -void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index, +const Ti5MotorCanopenProtocol::MotorConversion* +Ti5MotorCanopenProtocol::conversionForNode(const uint8_t node_id) const { + const auto it = motor_conversions_.find(node_id); + if (it != motor_conversions_.end() && + it->second.encoder_counts_per_rev > 0.0 && + it->second.gear_ratio > 0.0) { + return &it->second; + } + CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] missing conversion config for node " + << static_cast(node_id); + return nullptr; +} + +double Ti5MotorCanopenProtocol::radToCounts( + const double angle_rad, + const MotorConversion& conversion) const { + return (angle_rad * RADTODEG) / 360.0 * + conversion.gear_ratio * conversion.encoder_counts_per_rev; +} + +double Ti5MotorCanopenProtocol::countsToRad( + const int32_t counts, + const MotorConversion& conversion) const { + return (counts * 360.0) / + (conversion.gear_ratio * conversion.encoder_counts_per_rev * RADTODEG); +} + +double Ti5MotorCanopenProtocol::radPerSecToVelocityRaw( + const double velocity_rad_s, + const MotorConversion& conversion) const { + return ((velocity_rad_s * RADTODEG) * conversion.gear_ratio * Ti5VelocityUnitScale) / + 360.0; +} + +uint32_t Ti5MotorCanopenProtocol::radPerSec2ToAccelerationRaw( + const double acceleration_rad_s2, + const MotorConversion& conversion) const { + const auto raw = ((std::abs(acceleration_rad_s2) * RADTODEG) * + conversion.gear_ratio * Ti5VelocityUnitScale) / + 360.0 / Ti5AccelerationTimeScale; + return static_cast(std::abs(raw)); +} + +double Ti5MotorCanopenProtocol::velocityRawToRadPerSec( + const int32_t velocity_raw, + const MotorConversion& conversion) const { + return (velocity_raw * 360.0) / + (conversion.gear_ratio * Ti5VelocityUnitScale * RADTODEG); +} + + +void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, uint32_t index, uint32_t sub_index, uint32_t data, uint32_t delay_ms) { sdo_commands_[node_id]->SetFrameData(cs, index, sub_index, data); can_sender_->Update(sdo_commands_[node_id]->ID()); std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); } -void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) { - auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0; - - switch (getMode(node_id)) { - // case RUN_MODE_CYCLIC_SYNC_POSITION: - // setCSPTargetPosByPdo(node_id, static_cast(cmd)); - // break; - case RUN_MODE_PROFILE_POSITION: - // setPPTargetPosByPdo(node_id, static_cast(cmd)); - setPPTargetPosBySdo(node_id, static_cast(cmd)); - break; +bool Ti5MotorCanopenProtocol::commandProfilePosition(uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return false; } + if (max_qd > 0.0) { + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_VELOCITY_6081, + SUB_INDEX_0, + static_cast(std::abs(radPerSecToVelocityRaw(max_qd, *conversion))), + 0); + } + if (max_qdd > 0.0) { + const auto accel = radPerSec2ToAccelerationRaw(max_qdd, *conversion); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, + SUB_INDEX_0, accel, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, + SUB_INDEX_0, accel, 0); + } + writeProfilePositionTargetBySdo(node_id, static_cast(radToCounts(target_q, *conversion))); + return true; } -void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double angle_rad, double vel) { - auto pos_cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0; - auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0; +bool Ti5MotorCanopenProtocol::commandProfileVelocity(uint8_t node_id, + double target_qd, + double max_qdd) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return false; + } + if (max_qdd > 0.0) { + const auto accel = radPerSec2ToAccelerationRaw(max_qdd, *conversion); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, + SUB_INDEX_0, accel, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, + SUB_INDEX_0, accel, 0); + } + const auto speed = radPerSecToVelocityRaw(target_qd, *conversion); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_VELOCITY_60FF, + SUB_INDEX_0, + static_cast(static_cast(std::llround(speed))), + 0); + return true; +} + +bool Ti5MotorCanopenProtocol::commandCyclicPosition(uint8_t node_id, + double target_q, + double target_qd) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return false; + } + auto pos_cmd = radToCounts(target_q, *conversion); + auto speed = radPerSecToVelocityRaw(target_qd, *conversion); rpdo1_commands_[node_id]->SetTargetPos(pos_cmd); rpdo1_commands_[node_id]->SetTargetVel(uint32_t(std::abs(speed))); can_sender_->Update(rpdo1_commands_[node_id]->ID()); + return true; } - -void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double vel) { - auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0; +bool Ti5MotorCanopenProtocol::commandCyclicVelocity(uint8_t node_id, + double target_qd) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return false; + } + auto speed = radPerSecToVelocityRaw(target_qd, *conversion); rpdo2_commands_[node_id]->SetTargetVel(int16_t(speed)); can_sender_->Update(rpdo2_commands_[node_id]->ID()); + return true; } +bool Ti5MotorCanopenProtocol::commandCyclicTorque(uint8_t node_id, double target_tau) { + (void)target_tau; + CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] cyclic torque command is not implemented, node=" + << static_cast(node_id); + return false; +} -void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) { +void Ti5MotorCanopenProtocol::writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos) { controlword_t cw = {}; cw.switch_on = 1; cw.enable_voltage = 1; @@ -141,44 +250,19 @@ void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) cw.change_set_immediately = 1; // 1. 设置目标位置 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, pos); // 2. 设置触发位（bit4 = 1） cw.new_set_point = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); // 3. 清除触发位（bit4 = 0），准备下一次触发 cw.new_set_point = 0; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); } -void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) { - // 触发目标位置运动 - controlword_t cw; - cw.value = 0x0F; - cw.new_set_point = 1; - cw.change_set_immediately = 1; - - rpdo1_commands_[node_id]->SetTargetPos(pos); - rpdo1_commands_[node_id]->SetCtrlWord(cw.value); - can_sender_->Update(rpdo1_commands_[node_id]->ID()); - - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - - cw.new_set_point = 0; - rpdo1_commands_[node_id]->SetCtrlWord(cw.value); - can_sender_->Update(rpdo1_commands_[node_id]->ID()); -} - -void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos) { - rpdo1_commands_[node_id]->SetTargetPos(pos); - rpdo1_commands_[node_id]->SetCtrlWord(0x0F); - can_sender_->Update(rpdo1_commands_[node_id]->ID()); -} - - void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { // cur_mode_[node_id] = mode; @@ -188,36 +272,36 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { controlword_t cw = {}; cw.quick_stop = 1; cw.enable_voltage = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); // configRPDO1(node_id, false); // configRPDO2(node_id, false); // 1 : 先设置模式 auto data = static_cast(mode); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CIA402_OPERATION_MODE_6060, SUB_INDEX_0, data); // 3 : 状态机步进 —— Switch On & Enable Operation（0x0F） cw.switch_on = 1; cw.enable_operation = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20); switch (mode) { case RUN_MODE_PROFILE_POSITION: { // 4 : 设置目标位置（为当前位置） auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); // 5 : 触发位置运动（new_set_point 翻转） cw.new_set_point = 1; cw.change_set_immediately = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); // 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标） cw.new_set_point = 0; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } @@ -225,19 +309,19 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { // configRPDO1(node_id, true); // 设置目标位置为当前位置 auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, cur_pos); //3 : 使能 15 cw.enable_operation = 1; cw.switch_on = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } case RUN_MODE_PROFILE_VELOCITY: { cw.enable_operation = 1; cw.switch_on = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } @@ -245,7 +329,7 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { // configRPDO2(node_id, true); cw.enable_operation = 1; cw.switch_on = 1; - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value); break; } default: @@ -262,9 +346,9 @@ void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand c void Ti5MotorCanopenProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) { - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_VELOCITY_6081, SUB_INDEX_0, speed); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, SUB_INDEX_0, decel); } @@ -272,128 +356,128 @@ void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id) { //TDPO1 配置 状态字 和 控制字 // 1: 失能 pdo uint32_t cob_id = TPDO1_BASE_ID_180 + node_id; - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31)); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_0, 0); // 2: 配置为异步 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN); // 3：配置约束时间 unit:0.1ms - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_3, 10); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_3, 10); // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送 - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_5, 0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_5, 0); // 5 ：映射控制字 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_1, - CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_1, + CIA402_CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16); //6 : 映射状态字 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_2, - STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_2, + CIA402_STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16); //7 : 映射模式 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_3, - MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_3, + CIA402_MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8); //8 映射错误码 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_4, - ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_4, + CIA402_ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16); //9 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 4); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_0, 4); //10 使能 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31)); } void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id) { // 1: 失能 pdo uint32_t cob_id = TPDO2_BASE_ID_280 + node_id; - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31)); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_0, 0); // 2: 配置为异步 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN); // 3：配置约束时间 unit:0.1ms - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_3, 100); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_3, 100); // 4 : 配置周期发送时间 unit : ms - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_5, 0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_5, 0); // 5 ：映射当前位置 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_1, - ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_1, + CIA402_ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32); //6 : 映射当前速度 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_2, - ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_2, + CIA402_ACTUAL_VELOCITY_606C << 16 | SUB_INDEX_0 << 8 | 32); //9 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 2); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_0, 2); //10 使能 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31)); } void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool enable) { // 1: 失能 pdo uint32_t cob_id = RPDO1_BASE_ID_200 + node_id; - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31)); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_0, 0); // 2: 配置为 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN); // // 3：配置约束时间 unit:0.1ms - // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10); + // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CANOPEN_RPDO1_COMM_1400,SUB_INDEX_3,10); // // // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送 - // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0); + // seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CANOPEN_RPDO1_COMM_1400,SUB_INDEX_5,0); // 5 ：映射位置 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_1, - TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_1, + CIA402_TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32); //6 : 映射控制字 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2, - PROFILE_SPEED_6081 << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_2, + CIA402_PROFILE_VELOCITY_6081 << 16 | SUB_INDEX_0 << 8 | 32); //7 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_0, 2); //8 使能 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31)); } void Ti5MotorCanopenProtocol::configRPDO2(uint8_t node_id, bool enable) { // 1: 失能 pdo uint32_t cob_id = RPDO2_BASE_ID_300 + node_id; - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (1U << 31)); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (1U << 31)); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_0, 0); if (!enable) return; // 2: 配置为 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_COMM_1401, SUB_INDEX_2, SYNC_EVENT_DRIVEN); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_2, SYNC_EVENT_DRIVEN); // 5 ：映射位置 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_MAP_1601, SUB_INDEX_1, - TARGET_SPEED_60FF << 16 | SUB_INDEX_0 << 8 | 32); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_1, + CIA402_TARGET_VELOCITY_60FF << 16 | SUB_INDEX_0 << 8 | 32); //7 写入该PDO映射对象总个数 - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 1); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_0, 1); //8 使能 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (0U << 31)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (0U << 31)); } @@ -405,36 +489,49 @@ void Ti5MotorCanopenProtocol::configPdo(uint8_t node_id) { } void Ti5MotorCanopenProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) { - auto accel = ((u_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0; - auto decel = ((l_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0; - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel)); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel)); + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return; + } + auto accel = ((u_qdd * RADTODEG) * conversion->gear_ratio * Ti5VelocityUnitScale) / + 360.0 / Ti5AccelerationTimeScale; + auto decel = ((l_qdd * RADTODEG) * conversion->gear_ratio * Ti5VelocityUnitScale) / + 360.0 / Ti5AccelerationTimeScale; + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel)); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel)); } void Ti5MotorCanopenProtocol::setLimitQd(uint8_t node_id, double qd) { - auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0; - // seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, MAX_SPEED_607F, SUB_INDEX_0, speed); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed); + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return; + } + auto speed = radPerSecToVelocityRaw(qd, *conversion); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_MAX_PROFILE_VELOCITY_607F, SUB_INDEX_0, speed); } void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb) { - ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0; - lb = (lb * RADTODEG) / 360.0 * GearRatio * 65536.0; + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return; + } + ub = radToCounts(ub, *conversion); + lb = radToCounts(lb, *conversion); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub); } bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { // 0: 设置控制字为 0x06，确保停机状态 - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000); // 1: 清除偏置值 0x2008 ← 0 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0); // 2: 等待确认清除成功 - seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0); if (!waitUntil([&]() { return GetRobotDetail()->motors().at(node_id).position_offset() == 0; }, 1000)) { @@ -443,18 +540,18 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { } // 3: 读取当前位置 0x6064 - seedSdoRequest(node_id, CS_READ_REQUEST, ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20); + seedSdoRequest(node_id, CS_READ_REQUEST, CIA402_ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20); auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); // 4: 将当前位置写入偏置寄存器 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos); // 5: 保存参数到永久区（0x2000 ← 1） - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100); // 6: 确认写入成功 - seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20); + seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20); if (!waitUntil([&]() { return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos; }, 500)) { @@ -465,16 +562,28 @@ bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) { return true; } -void Ti5MotorCanopenProtocol::brake(uint8_t node_id) { +bool Ti5MotorCanopenProtocol::torqueOn(uint8_t node_id) { + setMode(node_id, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + return true; +} + +bool Ti5MotorCanopenProtocol::brakeRelease(uint8_t node_id) { + (void)node_id; + CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] brakeRelease is not implemented"; + return false; +} + +bool Ti5MotorCanopenProtocol::quickStop(uint8_t node_id) { // // 开机未使能电机时调用 - // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); // 6 抱闸 0 ： 立即停机 自由 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, QUICK_STOP_DECEL_6085, SUB_INDEX_0, 0XFFFFFFF0); - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6); - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100); + seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_QUICK_STOP_DECELERATION_6085, SUB_INDEX_0, 0XFFFFFFF0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100); // 必须要发送 0xf 才能按照6085中设定的减速度减速 - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + return true; } bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) { @@ -483,62 +592,37 @@ bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) { return st.target_reached == 1; } -void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) { - auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0; - switch (getMode(node_id)) { - case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: - case msgs::RUN_MODE_PROFILE_POSITION: { - auto it = last_Qd_.find(node_id); - if (it == last_Qd_.end() || it->second != speed) { - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)), - 0); - last_Qd_[node_id] = speed; - } - break; - } - case msgs::RUN_MODE_PROFILE_VELOCITY: - case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: { - // 在速度模式下，直接设置目标速度 - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0); - break; - } - default: - break; - } -} - -void Ti5MotorCanopenProtocol::setQdd(uint8_t node_id, double qdd) { - uint32_t accel = ((std::abs(qdd) * RADTODEG) * GearRatio * 100.0 * 65536.0) / (360.0 * 1000.0); - auto it = last_Qdd_.find(node_id); - if (it == last_Qdd_.end() || it->second != accel) { - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); - seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel); - last_Qdd_[node_id] = accel; - } -} - -void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) { +bool Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) { // 0 ： 立即停机 自由 - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0); - seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0); + seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20); // 必须要发送 0xf 才能按照6085中设定的减速度减速 - // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); + // seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F); // 停机之后，要重新使能？ // cur_mode_[node_id] = msgs::RUN_MODE_UNSPECIFIED; + return true; } double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return 0.0; + } auto data_ptr = std::make_unique(); message_manager_->GetSensorData(data_ptr.get()); auto cnt = data_ptr->motors().at(node_id).position(); - return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG); + return countsToRad(cnt, *conversion); } double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) { + const auto* conversion = conversionForNode(node_id); + if (!conversion) { + return 0.0; + } auto data_ptr = std::make_unique(); message_manager_->GetSensorData(data_ptr.get()); auto cnt = data_ptr->motors().at(node_id).speed(); - return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG); + return velocityRawToRadPerSec(cnt, *conversion); } diff --git a/cmvr-es/devices/motor/manager/CMakeLists.txt b/cmvr-es/devices/motor/manager/CMakeLists.txt index 8eea8209..8ea5f495 100644 --- a/cmvr-es/devices/motor/manager/CMakeLists.txt +++ b/cmvr-es/devices/motor/manager/CMakeLists.txt @@ -12,6 +12,7 @@ target_link_libraries(motor_manager PRIVATE cmvr_es::device::ti5_canopen_motor_driver cmvr_es::device::mujoco_motor_driver + cmvr_es::device::ethercat_motor_driver cmvr_es::ik_solver glog ) diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp index d1e6affe..5e83b043 100644 --- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -1,21 +1,27 @@ -#include "motor/manager/include/motor_manager.h" +#include "devices/motor/manager/include/motor_manager.h" +#include #include #include #include +#include #include #include #include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h" #include "common/base/logging/logger.h" #include "common/config/config_files.h" -#include "../../bus_runtime/abstract_motor_bus_runtime.h" -#include "motor/bus_runtime/can/include/can_motor_bus_runtime.h" -#include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" -#include "motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h" -#include "motor/drivers/mujoco/include/mujoco_motor.h" -#include "motor/drivers/ti5_canopen/include/ti5_motor.h" -#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" +#include "devices/motor/bus_runtime/abstract_motor_bus_runtime.h" +#include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h" +#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" +#include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h" +#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" +#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" +#include "devices/motor/drivers/mujoco/include/mujoco_motor.h" +#include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h" +#include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" namespace cmvr::device { @@ -91,11 +97,17 @@ bool MotorManager::init() all_ok = false; continue; } - if (!bus_runtime->start()) { + + const bool start_before_motor_init = + motor_group_cfg.bus_type() == config::MOTOR_BUS_ETHERCAT; + if (start_before_motor_init && !bus_runtime->start()) { bus_runtime->stop(); all_ok = false; continue; } + if (start_before_motor_init) { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } auto motors = createMotors_(motor_group_cfg, selected_motor_cfgs, bus_runtime); if (motors.empty()) { @@ -116,6 +128,11 @@ bool MotorManager::init() all_ok = false; continue; } + if (!start_before_motor_init && !bus_runtime->start()) { + bus_runtime->stop(); + all_ok = false; + continue; + } bus_runtimes_.push_back(std::move(bus_runtime)); } @@ -398,8 +415,19 @@ std::shared_ptr MotorManager::createBusRuntime_( return std::make_shared(); case config::MOTOR_BUS_MUJOCO: return std::make_shared(); - case config::MOTOR_BUS_ETHERCAT: - return std::make_shared(); + case config::MOTOR_BUS_ETHERCAT: { + auto runtime = std::make_shared(); + if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU && + group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) { + runtime->setPdoMapping(createEyouCia402PdoMapping()); + return runtime; + } + CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor=" + << config::MotorVendor_Name(group_cfg.vendor()) + << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) + << ", group=" << group_cfg.id(); + return nullptr; + } default: CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: " << config::MotorBusType_Name(group_cfg.bus_type()) @@ -534,6 +562,14 @@ std::vector> MotorManager::createEthercatMotors_( CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT config: " << group_cfg.id(); return {}; } + if (group_cfg.vendor() != config::MOTOR_VENDOR_EYOU || + group_cfg.protocol() != config::MOTOR_PROTOCOL_ETHERCAT_CIA402) { + CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor=" + << config::MotorVendor_Name(group_cfg.vendor()) + << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) + << ", group=" << group_cfg.id(); + return {}; + } for (const auto& motor_cfg : motor_cfgs) { if (!ethercat_bus_runtime->slaveForMotor(motor_cfg.id())) { CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT slave config for motor id " @@ -542,11 +578,22 @@ std::vector> MotorManager::createEthercatMotors_( } } - CMVR_LOG(ERROR) << "[MotorManager] EtherCAT motor creation is not implemented: vendor=" - << config::MotorVendor_Name(group_cfg.vendor()) - << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) - << ", group=" << group_cfg.id(); - return {}; + auto protocol = std::make_shared( + ethercat_bus_runtime, group_cfg.ethercat().cia402()); + + std::vector> motors; + motors.reserve(motor_cfgs.size()); + for (const auto& cfg : motor_cfgs) { + auto motor = std::make_shared( + cfg, protocol, std::make_unique(ethercat_bus_runtime)); + if (!motor->init()) { + CMVR_LOG(ERROR) << "[MotorManager] failed to init EYOU EtherCAT motor: " + << cfg.joint_name(); + return {}; + } + motors.push_back(std::move(motor)); + } + return motors; } } // namespace cmvr::device diff --git a/cmvr-es/devices/motor/motor_protocol_interface.h b/cmvr-es/devices/motor/motor_protocol_interface.h index 634a715c..4b8f3e34 100644 --- a/cmvr-es/devices/motor/motor_protocol_interface.h +++ b/cmvr-es/devices/motor/motor_protocol_interface.h @@ -15,7 +15,8 @@ namespace cmvr { public: enum class CommProto : uint8_t { CANOPEN = 1, - CUSTOM = 2 + ETHERCAT = 2, + CUSTOM = 3 }; virtual ~MotorProtocolInterface() = default; @@ -26,9 +27,6 @@ namespace cmvr { */ virtual bool initNode(uint8_t node_id) = 0; - virtual void setQ(uint8_t node_id, double angle_rad) = 0; - virtual void setTarget(uint8_t node_id, double angle_rad,double vel) = 0; - virtual void setTarget(uint8_t node_id,double vel) = 0; virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0; virtual msgs::RunMode getMode(uint8_t node_id) = 0; virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0; @@ -36,12 +34,35 @@ namespace cmvr { virtual void setLimitQ(uint8_t node_id, double ub, double lb) = 0; virtual bool calibrateZeroQ(uint8_t node_id) = 0; virtual bool reachedTargetQ(uint8_t node_id) = 0; - virtual void setQd(uint8_t node_id, double qd) = 0; - virtual void setQdd(uint8_t node_id,double qdd) = 0; - // virtual void setVelocity(uint8_t node_id, double velocity) = 0; - // virtual void clearError(uint8_t node_id) = 0; - virtual void brake(uint8_t node_id) = 0; - virtual void torqueOff(uint8_t node_id) = 0; + // target_q: rad, max_qd: rad/s, max_qdd: rad/s^2. + // Profile Position 写入目标位置和轮廓速度/加速度，并触发一次新目标。 + virtual bool commandProfilePosition(uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) = 0; + // target_qd: rad/s, max_qdd: rad/s^2. + // Profile Velocity 写入目标速度和轮廓加速度。 + virtual bool commandProfileVelocity(uint8_t node_id, + double target_qd, + double max_qdd) = 0; + // target_q: rad, target_qd: rad/s. + // Cyclic Position 周期写入目标位置和目标速度。 + virtual bool commandCyclicPosition(uint8_t node_id, + double target_q, + double target_qd) = 0; + // target_qd: rad/s. + // Cyclic Velocity 周期写入目标速度。 + virtual bool commandCyclicVelocity(uint8_t node_id, + double target_qd) = 0; + // target_tau: N*m. + virtual bool commandCyclicTorque(uint8_t node_id, double target_tau) = 0; + virtual void setMotorConversion(uint8_t node_id, + double encoder_counts_per_rev, + double gear_ratio) = 0; + virtual bool torqueOn(uint8_t node_id) = 0; + virtual bool torqueOff(uint8_t node_id) = 0; + virtual bool brakeRelease(uint8_t node_id) = 0; + virtual bool quickStop(uint8_t node_id) = 0; virtual double getQ(uint8_t node_id) = 0; virtual double getQd(uint8_t node_id) = 0; diff --git a/cmvr-es/manager/device_manager/src/device_manager.cpp b/cmvr-es/manager/device_manager/src/device_manager.cpp index d957afb4..88e7a501 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -26,6 +26,9 @@ using namespace cmvr::device; namespace { +using GroupJointSelection = std::unordered_map>; +using MotorJointSelections = std::unordered_map; + void logSection(const char* title) { CMVR_LOG(INFO) << "---------------- " << title << " ----------------"; @@ -71,6 +74,32 @@ bool motorGroupHasJoint(const cmvr::config::MotorGroupConfig& motor_group, return false; } +void addAllMotorJoints(const std::string& motor_system_id, + const cmvr::config::MotorRootConfig& root_cfg, + MotorJointSelections& selections) +{ + auto& group_selection = selections[motor_system_id]; + for (const auto& motor_group : root_cfg.motor().motor_groups()) { + if (motor_group.id().empty()) { + continue; + } + + auto& selected_joints = group_selection[motor_group.id()]; + for (const auto& motor : motor_group.motors().motors()) { + if (!motor.joint_name().empty()) { + selected_joints.insert(motor.joint_name()); + } + } + if (selected_joints.empty()) { + group_selection.erase(motor_group.id()); + } + } + + if (group_selection.empty()) { + selections.erase(motor_system_id); + } +} + } // namespace template std::shared_ptr DeviceManager::getDevice(const std::string& device_id); @@ -256,8 +285,7 @@ void DeviceManager::log_device_plan_() const void DeviceManager::pre_scan_robot_arm_dependencies_() const { - using GroupJointSelection = std::unordered_map>; - std::unordered_map selections; + MotorJointSelections selections; std::unordered_map motor_roots; for (const auto& entry : cfg_.devices()) { @@ -387,6 +415,15 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } } + if (selections.empty() && cfg_.init_all_motors_when_no_active_joints()) { + CMVR_LOG(INFO) << "[DeviceManager]: No active motor joints from RobotArm; " + << "initialize all configured motors because " + << "init_all_motors_when_no_active_joints=true"; + for (const auto& [motor_system_id, root_cfg] : motor_roots) { + addAllMotorJoints(motor_system_id, root_cfg, selections); + } + } + MotorManager::clearActiveJoints(); for (auto& [motor_system_id, group_selection] : selections) { MotorManager::setActiveJoints(motor_system_id, std::move(group_selection)); diff --git a/dependency/x86/third_party/ethercat/v1.7.0/bin/ethercat b/dependency/x86/third_party/ethercat/v1.7.0/bin/ethercat new file mode 100755 index 00000000..50baa256 Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/bin/ethercat differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf new file mode 100644 index 00000000..9ec7a2ce --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf @@ -0,0 +1,3 @@ +MASTER0_DEVICE="a0:ad:9f:c4:c2:2c" +DEVICE_MODULES="generic" +UPDOWN_INTERFACES="eno1" diff --git a/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_140412 b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_140412 new file mode 100644 index 00000000..c1741260 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_140412 @@ -0,0 +1,113 @@ +#------------------------------------------------------------------------------ +# +# EtherCAT master configuration file for use with ethercatctl. +# +# vim: spelllang=en spell tw=78 +# +#------------------------------------------------------------------------------ + +# +# Main Ethernet devices. +# +# The MASTER_DEVICE variable specifies the Ethernet device for a master +# with index 'X'. +# +# Specify the MAC address (hexadecimal with colons) of the Ethernet device to +# use. Example: "00:00:08:44:ab:66" +# +# Alternatively, a network interface name can be specified. The interface +# name will be resolved to a MAC address using the 'ip' command. +# Example: "eth0" +# +# The broadcast address "ff:ff:ff:ff:ff:ff" has a special meaning: It tells +# the master to accept the first device offered by any Ethernet driver. +# +# The MASTER_DEVICE variables also determine, how many masters will be +# created: A non-empty variable MASTER0_DEVICE will create one master, adding +# a non-empty variable MASTER1_DEVICE will create a second master, and so on. +# +# Examples: +# MASTER0_DEVICE="00:00:08:44:ab:66" +# MASTER0_DEVICE="eth0" +# +MASTER0_DEVICE="" +#MASTER1_DEVICE="" + +# +# Backup Ethernet devices +# +# The MASTER_BACKUP variables specify the devices used for redundancy. They +# behaves nearly the same as the MASTER_DEVICE variable, except that it +# does not interpret the ff:ff:ff:ff:ff:ff address. +# +#MASTER0_BACKUP="" + +# +# Ethernet driver modules to use for EtherCAT operation. +# +# Specify a non-empty list of Ethernet drivers, that shall be used for +# EtherCAT operation. +# +# Except for the generic Ethernet driver module, the init script will try to +# unload the usual Ethernet driver modules in the list and replace them with +# the EtherCAT-capable ones. If a certain (EtherCAT-capable) driver is not +# found, a warning will appear. +# +# Possible values: 8139too, e100, e1000, e1000e, r8169, generic, ccat, igb, +# igc, genet, dwmac-intel, stmmac-pci. +# Separate multiple drivers with spaces. +# A list of all matching kernel versions can be found here: +# https://docs.etherlab.org/ethercat/1.6/doxygen/devicedrivers.html +# +# Note: The e100, e1000, e1000e, r8169, ccat, igb and igc drivers are not +# built by default. Enable them with the --enable- configure switches. +# +DEVICE_MODULES="" + +# If you have any issues about network interfaces not being configured +# properly, systemd may need some additional infos about your setup. +# Have a look at the service file, you'll find some details there. +# + +# +# List of interfaces to bring up and down automatically. +# +# Specify a space-separated list of interface names (such as eth0 or +# enp0s1) that shall be brought up on `ethercatctl start` and down on +# `ethercatctl stop`. +# +# When using the generic driver, the corresponding Ethernet device has to be +# activated before the master is started, otherwise all frames will time out. +# This the perfect use-case for `UPDOWN_INTERFACES`. +# +UPDOWN_INTERFACES="" + +# +# Default SII caching method. +# +# Set the start-up caching method for all masters. The integer value +# determines, which fields are used to look up a cached SII page. It is a +# bit-field consisting of the following flags. A value of zero disables SII +# caching (default). A typical value is 7 (use vendor ID, product code and +# revision number for lookup). +# +# - 1: Vendor ID (always used) +# - 2: Product code (always used) +# - 4: Revision number +# - 8: Serial number +# - 16: Alias address +# +# Please keep in mind that in case serial number or alias address is enabled, +# only slaves with a non-zero serial number or alias benefit from caching. +# +SII_CACHING=0 + +# +# Flags for loading kernel modules. +# +# This can usually be left empty. Adjust this variable, if you have problems +# with module loading. +# +#MODPROBE_FLAGS="-b" + +#------------------------------------------------------------------------------ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_155334 b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_155334 new file mode 100644 index 00000000..9ec7a2ce --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf.bak.20260702_155334 @@ -0,0 +1,3 @@ +MASTER0_DEVICE="a0:ad:9f:c4:c2:2c" +DEVICE_MODULES="generic" +UPDOWN_INTERFACES="eno1" diff --git a/dependency/x86/third_party/ethercat/v1.7.0/etc/init.d/ethercat b/dependency/x86/third_party/ethercat/v1.7.0/etc/init.d/ethercat new file mode 100755 index 00000000..63498a9f --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/etc/init.d/ethercat @@ -0,0 +1,123 @@ +#!/bin/sh + +#------------------------------------------------------------------------------ +# +# Init script for EtherCAT +# +# Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH +# +# This file is part of the IgH EtherCAT Master. +# +# The IgH EtherCAT Master is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License version 2, as +# published by the Free Software Foundation. +# +# The IgH EtherCAT Master is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General +# Public License for more details. +# +# You should have received a copy of the GNU General Public License along +# with the IgH EtherCAT Master; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +# +# +# vim: expandtab +# +#------------------------------------------------------------------------------ + +### BEGIN INIT INFO +# Provides: ethercat +# Required-Start: $local_fs $syslog $network +# Should-Start: $time ntp +# Required-Stop: $local_fs $syslog $network +# Should-Stop: $time ntp +# Default-Start: 3 5 +# Default-Stop: 0 1 2 6 +# Short-Description: EtherCAT master +# Description: EtherCAT master 1.7.0 +### END INIT INFO + +#------------------------------------------------------------------------------ + +ETHERCATCTL="/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl -c /home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/etc/sysconfig/ethercat" + +#------------------------------------------------------------------------------ + +exit_success() { + if [ -r /etc/rc.status ]; then + rc_reset + rc_status -v + rc_exit + else + echo " done" + exit 0 + fi +} + +#------------------------------------------------------------------------------ + +exit_fail() { + if [ -r /etc/rc.status ]; then + rc_failed + rc_status -v + rc_exit + else + echo " failed" + exit 1 + fi +} + +#------------------------------------------------------------------------------ + +if [ -r /etc/rc.status ]; then + . /etc/rc.status + rc_reset +fi + +case "${1}" in + +start) + echo -n "Starting EtherCAT master 1.7.0 " + + if $ETHERCATCTL start; then + exit_success + else + exit_fail + fi + ;; + +stop) + echo -n "Shutting down EtherCAT master 1.7.0 " + + if $ETHERCATCTL stop; then + exit_success + else + exit_fail + fi + ;; + +restart) + $0 stop || exit 1 + sleep 1 + $0 start + ;; + +status) + $ETHERCATCTL status + exit $? + ;; + +*) + echo "USAGE: $0 {start|stop|restart|status}" + ;; + +esac + +if [ -r /etc/rc.status ]; then + rc_exit +else + exit 1 +fi + +#------------------------------------------------------------------------------ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/etc/sysconfig/ethercat b/dependency/x86/third_party/ethercat/v1.7.0/etc/sysconfig/ethercat new file mode 100644 index 00000000..4bf8ea93 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/etc/sysconfig/ethercat @@ -0,0 +1,113 @@ +#------------------------------------------------------------------------------ +# +# EtherCAT master configuration file for use with init.d. +# +# vim: spelllang=en spell tw=78 +# +#------------------------------------------------------------------------------ + +# +# Main Ethernet devices. +# +# The MASTER_DEVICE variable specifies the Ethernet device for a master +# with index 'X'. +# +# Specify the MAC address (hexadecimal with colons) of the Ethernet device to +# use. Example: "00:00:08:44:ab:66" +# +# Alternatively, a network interface name can be specified. The interface +# name will be resolved to a MAC address using the 'ip' command. +# Example: "eth0" +# +# The broadcast address "ff:ff:ff:ff:ff:ff" has a special meaning: It tells +# the master to accept the first device offered by any Ethernet driver. +# +# The MASTER_DEVICE variables also determine, how many masters will be +# created: A non-empty variable MASTER0_DEVICE will create one master, adding +# a non-empty variable MASTER1_DEVICE will create a second master, and so on. +# +# Examples: +# MASTER0_DEVICE="00:00:08:44:ab:66" +# MASTER0_DEVICE="eth0" +# +MASTER0_DEVICE="" +#MASTER1_DEVICE="" + +# +# Backup Ethernet devices +# +# The MASTER_BACKUP variables specify the devices used for redundancy. They +# behaves nearly the same as the MASTER_DEVICE variable, except that it +# does not interpret the ff:ff:ff:ff:ff:ff address. +# +#MASTER0_BACKUP="" + +# +# Ethernet driver modules to use for EtherCAT operation. +# +# Specify a non-empty list of Ethernet drivers, that shall be used for +# EtherCAT operation. +# +# Except for the generic Ethernet driver module, the init script will try to +# unload the usual Ethernet driver modules in the list and replace them with +# the EtherCAT-capable ones. If a certain (EtherCAT-capable) driver is not +# found, a warning will appear. +# +# Possible values: 8139too, e100, e1000, e1000e, r8169, generic, ccat, igb, +# igc, genet, dwmac-intel, stmmac-pci. +# Separate multiple drivers with spaces. +# A list of all matching kernel versions can be found here: +# https://docs.etherlab.org/ethercat/1.6/doxygen/devicedrivers.html +# +# Note: The e100, e1000, e1000e, r8169, ccat, igb and igc drivers are not +# built by default. Enable them with the --enable- configure switches. +# +DEVICE_MODULES="" + +# If you have any issues about network interfaces not being configured +# properly, systemd may need some additional infos about your setup. +# Have a look at the service file, you'll find some details there. +# + +# +# List of interfaces to bring up and down automatically. +# +# Specify a space-separated list of interface names (such as eth0 or +# enp0s1) that shall be brought up on `ethercatctl start` and down on +# `ethercatctl stop`. +# +# When using the generic driver, the corresponding Ethernet device has to be +# activated before the master is started, otherwise all frames will time out. +# This the perfect use-case for `UPDOWN_INTERFACES`. +# +UPDOWN_INTERFACES="" + +# +# Default SII caching method. +# +# Set the start-up caching method for all masters. The integer value +# determines, which fields are used to look up a cached SII page. It is a +# bit-field consisting of the following flags. A value of zero disables SII +# caching (default). A typical value is 7 (use vendor ID, product code and +# revision number for lookup). +# +# - 1: Vendor ID (always used) +# - 2: Product code (always used) +# - 4: Revision number +# - 8: Serial number +# - 16: Alias address +# +# Please keep in mind that in case serial number or alias address is enabled, +# only slaves with a non-zero serial number or alias benefit from caching. +# +SII_CACHING=0 + +# +# Flags for loading kernel modules. +# +# This can usually be left empty. Adjust this variable, if you have problems +# with module loading. +# +#MODPROBE_FLAGS="-b" + +#------------------------------------------------------------------------------ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/include/ecrt.h b/dependency/x86/third_party/ethercat/v1.7.0/include/ecrt.h new file mode 100644 index 00000000..64a43ec3 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/include/ecrt.h @@ -0,0 +1,3216 @@ +/***************************************************************************** + * + * Copyright (C) 2006-2026 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT master userspace library. + * + * The IgH EtherCAT master userspace library is free software; you can + * redistribute it and/or modify it under the terms of the GNU Lesser General + * Public License as published by the Free Software Foundation; version 2.1 + * of the License. + * + * The IgH EtherCAT master userspace library is distributed in the hope that + * it will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with the IgH EtherCAT master userspace library. If not, see + * . + * + ****************************************************************************/ + +/** \file + * + * EtherCAT master application interface. + * + * \defgroup ApplicationInterface EtherCAT Application Interface + * + * EtherCAT interface for realtime applications. This interface is designed + * for realtime modules that want to use EtherCAT. There are functions to + * request a master, to map process data, to communicate with slaves via CoE + * and to configure and activate the bus. + * + * Changes in version 1.7.0: + * + * - Added ecrt_master_sii_caching() to set the SII caching method and added + * the feature flag EC_HAVE_SII_CACHING and the enum type + * ec_sii_caching_fields_t. + * + * Changes in version 1.6.0: + * + * - Added the ecrt_master_scan_progress() method, the + * ec_master_scan_progress_t structure and the EC_HAVE_SCAN_PROGRESS + * definition to check for its existence. + * - Added the EoE configuration methods ecrt_slave_config_eoe_mac_address(), + * ecrt_slave_config_eoe_ip_address(), ecrt_slave_config_eoe_subnet_mask(), + * ecrt_slave_config_eoe_default_gateway(), + * ecrt_slave_config_eoe_dns_address(), + * ecrt_slave_config_eoe_hostname() and the EC_HAVE_SET_IP + * definition to check for its existence. + * - Added ecrt_slave_config_state_timeout() to set the application-layer + * state change timeout and EC_HAVE_STATE_TIMEOUT to check for its + * existence. + * + * Changes since version 1.5.2: + * + * - Added the ecrt_slave_config_flag() method and the EC_HAVE_FLAGS + * definition to check for its existence. + * - Added SoE IDN requests, including the datatype ec_soe_request_t and the + * methods ecrt_slave_config_create_soe_request(), + * ecrt_soe_request_object(), ecrt_soe_request_timeout(), + * ecrt_soe_request_data(), ecrt_soe_request_data_size(), + * ecrt_soe_request_state(), ecrt_soe_request_write() and + * ecrt_soe_request_read(). Use the EC_HAVE_SOE_REQUESTS to check, if the + * functionality is available. + * + * Changes in version 1.5.2: + * + * - Added redundancy_active flag to ec_domain_state_t. + * - Added ecrt_master_link_state() method and ec_master_link_state_t to query + * the state of a redundant link. + * - Added the EC_HAVE_REDUNDANCY define, to check, if the interface contains + * redundancy features. + * - Added ecrt_sdo_request_index() to change SDO index and subindex after + * request creation. + * - Added interface for retrieving CoE emergency messages, i. e. + * ecrt_slave_config_emerg_size(), ecrt_slave_config_emerg_pop(), + * ecrt_slave_config_emerg_clear(), ecrt_slave_config_emerg_overruns() and + * the defines EC_HAVE_EMERGENCY and EC_COE_EMERGENCY_MSG_SIZE. + * - Added interface for direct EtherCAT register access: Added data type + * ec_reg_request_t and methods ecrt_slave_config_create_reg_request(), + * ecrt_reg_request_data(), ecrt_reg_request_state(), + * ecrt_reg_request_write(), ecrt_reg_request_read() and the feature flag + * EC_HAVE_REG_ACCESS. + * - Added method to select the reference clock, + * ecrt_master_select_reference_clock() and the feature flag + * EC_HAVE_SELECT_REF_CLOCK to check, if the method is available. + * - Added method to get the reference clock time, + * ecrt_master_reference_clock_time() and the feature flag + * EC_HAVE_REF_CLOCK_TIME to have the possibility to synchronize the master + * clock to the reference clock. + * - Changed the data types of the shift times in ecrt_slave_config_dc() to + * int32_t to correctly display negative shift times. + * - Added ecrt_slave_config_reg_pdo_entry_pos() and the feature flag + * EC_HAVE_REG_BY_POS for registering PDO entries with non-unique indices + * via their positions in the mapping. + * + * Changes in version 1.5: + * + * - Added the distributed clocks feature and the respective method + * ecrt_slave_config_dc() to configure a slave for cyclic operation, and + * ecrt_master_application_time(), ecrt_master_sync_reference_clock() and + * ecrt_master_sync_slave_clocks() for offset and drift compensation. The + * EC_TIMEVAL2NANO() macro can be used for epoch time conversion, while the + * ecrt_master_sync_monitor_queue() and ecrt_master_sync_monitor_process() + * methods can be used to monitor the synchrony. + * - Improved the callback mechanism. ecrt_master_callbacks() now takes two + * callback functions for sending and receiving datagrams. + * ecrt_master_send_ext() is used to execute the sending of non-application + * datagrams. + * - Added watchdog configuration (method ecrt_slave_config_watchdog(), + * #ec_watchdog_mode_t, \a watchdog_mode parameter in ec_sync_info_t and + * ecrt_slave_config_sync_manager()). + * - Added ecrt_slave_config_complete_sdo() method to download an SDO during + * configuration via CompleteAccess. + * - Added ecrt_master_deactivate() to remove the master configuration. + * - Added ecrt_open_master() and ecrt_master_reserve() separation for + * userspace. + * - Added master information interface (methods ecrt_master(), + * ecrt_master_get_slave(), ecrt_master_get_sync_manager(), + * ecrt_master_get_pdo() and ecrt_master_get_pdo_entry()) to get information + * about the currently connected slaves and the PDO entries provided. + * - Added ecrt_master_sdo_download(), ecrt_master_sdo_download_complete() and + * ecrt_master_sdo_upload() methods to let an application transfer SDOs + * before activating the master. + * - Changed the meaning of the negative return values of + * ecrt_slave_config_reg_pdo_entry() and ecrt_slave_config_sdo*(). + * - Implemented the Vendor-specific over EtherCAT mailbox protocol. See + * ecrt_slave_config_create_voe_handler(). + * - Renamed ec_sdo_request_state_t to #ec_request_state_t, because it is also + * used by VoE handlers. + * - Removed 'const' from argument of ecrt_sdo_request_state(), because the + * userspace library has to modify object internals. + * - Added 64-bit data access macros. + * - Added ecrt_slave_config_idn() method for storing SoE IDN configurations, + * and ecrt_master_read_idn() and ecrt_master_write_idn() to read/write IDNs + * ad-hoc via the user-space library. + * - Added ecrt_master_reset() to initiate retrying to configure slaves. + * + * @{ + */ + +/****************************************************************************/ + +#ifndef __ECRT_H__ +#define __ECRT_H__ + +#ifdef __KERNEL__ +#include +#include +#include +#include // struct in_addr +#else +#include // for size_t +#include +#include // for struct timeval +#include // struct in_addr +#endif + +/***************************************************************************** + * Global definitions + ****************************************************************************/ + +/** EtherCAT realtime interface major version number. + */ +#define ECRT_VER_MAJOR 1 + +/** EtherCAT realtime interface minor version number. + */ +#define ECRT_VER_MINOR 6 + +/** EtherCAT realtime interface version word generator. + */ +#define ECRT_VERSION(a, b) (((a) << 8) + (b)) + +/** EtherCAT realtime interface version word. + */ +#define ECRT_VERSION_MAGIC ECRT_VERSION(ECRT_VER_MAJOR, ECRT_VER_MINOR) + +/***************************************************************************** + * Feature flags + ****************************************************************************/ + +/** Defined, if the redundancy features are available. + * + * I. e. if the \a redundancy_active flag in ec_domain_state_t and the + * ecrt_master_link_state() method are available. + */ +#define EC_HAVE_REDUNDANCY + +/** Defined, if the CoE emergency ring feature is available. + * + * I. e. if the ecrt_slave_config_emerg_*() methods are available. + */ +#define EC_HAVE_EMERGENCY + +/** Defined, if the register access interface is available. + * + * I. e. if the methods ecrt_slave_config_create_reg_request(), + * ecrt_reg_request_data(), ecrt_reg_request_state(), ecrt_reg_request_write() + * and ecrt_reg_request_read() are available. + */ +#define EC_HAVE_REG_ACCESS + +/** Defined if the method ecrt_master_select_reference_clock() is available. + */ +#define EC_HAVE_SELECT_REF_CLOCK + +/** Defined if the method ecrt_master_reference_clock_time() is available. + */ +#define EC_HAVE_REF_CLOCK_TIME + +/** Defined if the method ecrt_slave_config_reg_pdo_entry_pos() is available. + */ +#define EC_HAVE_REG_BY_POS + +/** Defined if the method ecrt_master_sync_reference_clock_to() is available. + */ +#define EC_HAVE_SYNC_TO + +/** Defined if the method ecrt_slave_config_flag() is available. + */ +#define EC_HAVE_FLAGS + +/** Defined if the methods ecrt_slave_config_create_soe_request(), + * ecrt_soe_request_object(), ecrt_soe_request_timeout(), + * ecrt_soe_request_data(), ecrt_soe_request_data_size(), + * ecrt_soe_request_state(), ecrt_soe_request_write() and + * ecrt_soe_request_read() and the datatype ec_soe_request_t are available. + */ +#define EC_HAVE_SOE_REQUESTS + +/** Defined, if the method ecrt_master_scan_progress() and the + * ec_master_scan_progress_t structure are available. + */ +#define EC_HAVE_SCAN_PROGRESS + +/** Defined, if the methods ecrt_slave_config_eoe_mac_address(), + * ecrt_slave_config_eoe_ip_address(), ecrt_slave_config_eoe_subnet_mask(), + * ecrt_slave_config_eoe_default_gateway(), + * ecrt_slave_config_eoe_dns_address(), ecrt_slave_config_eoe_hostname() are + * available. + */ +#define EC_HAVE_SET_IP + +/** Defined, if the method ecrt_slave_config_state_timeout() is available. + */ +#define EC_HAVE_STATE_TIMEOUT + +/** Defined, if the method ecrt_master_sii_caching() and the enum type + * ec_sii_caching_fields_t and its values are available. + */ +#define EC_HAVE_SII_CACHING + +/****************************************************************************/ + +/** Symbol visibility control macro. + */ +#ifndef EC_PUBLIC_API +# if defined(ethercat_EXPORTS) && !defined(__KERNEL__) +# define EC_PUBLIC_API __attribute__ ((visibility ("default"))) +# else +# define EC_PUBLIC_API +# endif +#endif + +/****************************************************************************/ + +/** End of list marker. + * + * This can be used with ecrt_slave_config_pdos(). + */ +#define EC_END ~0U + +/** Maximum number of sync managers per slave. + */ +#define EC_MAX_SYNC_MANAGERS 16 + +/** Maximum string length. + * + * Used in ec_slave_info_t. + */ +#define EC_MAX_STRING_LENGTH 64 + +/** Maximum number of slave ports. */ +#define EC_MAX_PORTS 4 + +/** Timeval to nanoseconds conversion. + * + * This macro converts a Unix epoch time to EtherCAT DC time. + * + * \see void ecrt_master_application_time() + * + * \param TV struct timeval containing epoch time. + */ +#define EC_TIMEVAL2NANO(TV) \ + (((TV).tv_sec - 946684800ULL) * 1000000000ULL + (TV).tv_usec * 1000ULL) + +/** Size of a CoE emergency message in byte. + * + * \see ecrt_slave_config_emerg_pop(). + */ +#define EC_COE_EMERGENCY_MSG_SIZE 8 + +/***************************************************************************** + * Data types + ****************************************************************************/ + +struct ec_master; +typedef struct ec_master ec_master_t; /**< \see ec_master */ + +struct ec_slave_config; +typedef struct ec_slave_config ec_slave_config_t; /**< \see ec_slave_config */ + +struct ec_domain; +typedef struct ec_domain ec_domain_t; /**< \see ec_domain */ + +struct ec_sdo_request; +typedef struct ec_sdo_request ec_sdo_request_t; /**< \see ec_sdo_request. */ + +struct ec_soe_request; +typedef struct ec_soe_request ec_soe_request_t; /**< \see ec_soe_request. */ + +struct ec_voe_handler; +typedef struct ec_voe_handler ec_voe_handler_t; /**< \see ec_voe_handler. */ + +struct ec_reg_request; +typedef struct ec_reg_request ec_reg_request_t; /**< \see ec_reg_request. */ + +/****************************************************************************/ + +/** Master state. + * + * This is used for the output parameter of ecrt_master_state(). + * + * \see ecrt_master_state(). + */ +typedef struct { + unsigned int slaves_responding; /**< Sum of responding slaves on all + Ethernet devices. */ + unsigned int al_states : 4; /**< Application-layer states of all slaves. + The states are coded in the lower 4 bits. + If a bit is set, it means that at least one + slave in the network is in the corresponding + state: + - Bit 0: \a INIT + - Bit 1: \a PREOP + - Bit 2: \a SAFEOP + - Bit 3: \a OP */ + unsigned int link_up : 1; /**< \a true, if at least one Ethernet link is + up. */ +} ec_master_state_t; + +/****************************************************************************/ + +/** Redundant link state. + * + * This is used for the output parameter of ecrt_master_link_state(). + * + * \see ecrt_master_link_state(). + */ +typedef struct { + unsigned int slaves_responding; /**< Sum of responding slaves on the given + link. */ + unsigned int al_states : 4; /**< Application-layer states of the slaves on + the given link. The states are coded in the + lower 4 bits. If a bit is set, it means + that at least one slave in the network is in + the corresponding state: + - Bit 0: \a INIT + - Bit 1: \a PREOP + - Bit 2: \a SAFEOP + - Bit 3: \a OP */ + unsigned int link_up : 1; /**< \a true, if the given Ethernet link is up. + */ +} ec_master_link_state_t; + +/****************************************************************************/ + +/** Slave configuration state. + * + * This is used as an output parameter of ecrt_slave_config_state(). + * + * \see ecrt_slave_config_state(). + */ +typedef struct { + unsigned int online : 1; /**< The slave is online. */ + unsigned int operational : 1; /**< The slave was brought into \a OP state + using the specified configuration. */ + unsigned int al_state : 4; /**< The application-layer state of the slave. + - 1: \a INIT + - 2: \a PREOP + - 4: \a SAFEOP + - 8: \a OP + + Note that each state is coded in a different + bit! */ +} ec_slave_config_state_t; + +/****************************************************************************/ + +/** Master information. + * + * This is used as an output parameter of ecrt_master(). + * + * \see ecrt_master(). + */ +typedef struct { + unsigned int slave_count; /**< Number of slaves in the network. */ + unsigned int link_up : 1; /**< \a true, if the network link is up. */ + uint8_t scan_busy; /**< \a true, while the master is scanning the network. + */ + uint64_t app_time; /**< Application time. */ +} ec_master_info_t; + +/****************************************************************************/ + +/** Master scan progress information. + * + * This is used as an output parameter of ecrt_master_scan_progress(). + * + * \see ecrt_master_scan_progress(). + */ +typedef struct { + unsigned int slave_count; /**< Number of slaves detected. */ + unsigned int scan_index; /**< Index of the slave that is currently + scanned. If it is less than the \a + slave_count, the network scan is in progress. + */ +} ec_master_scan_progress_t; + +/****************************************************************************/ + +/** EtherCAT slave port descriptor. + */ +typedef enum { + EC_PORT_NOT_IMPLEMENTED, /**< Port is not implemented. */ + EC_PORT_NOT_CONFIGURED, /**< Port is not configured. */ + EC_PORT_EBUS, /**< Port is an E-Bus. */ + EC_PORT_MII /**< Port is a MII. */ +} ec_slave_port_desc_t; + +/****************************************************************************/ + +/** EtherCAT slave port information. + */ +typedef struct { + uint8_t link_up; /**< Link detected. */ + uint8_t loop_closed; /**< Loop closed. */ + uint8_t signal_detected; /**< Detected signal on RX port. */ +} ec_slave_port_link_t; + +/****************************************************************************/ + +/** Slave information. + * + * This is used as an output parameter of ecrt_master_get_slave(). + * + * \see ecrt_master_get_slave(). + */ +typedef struct { + uint16_t position; /**< Offset of the slave in the ring. */ + uint32_t vendor_id; /**< Vendor-ID stored on the slave. */ + uint32_t product_code; /**< Product-Code stored on the slave. */ + uint32_t revision_number; /**< Revision-Number stored on the slave. */ + uint32_t serial_number; /**< Serial-Number stored on the slave. */ + uint16_t alias; /**< The slaves alias if not equal to 0. */ + int16_t current_on_ebus; /**< Used current in mA. */ + struct { + ec_slave_port_desc_t desc; /**< Physical port type. */ + ec_slave_port_link_t link; /**< Port link state. */ + uint32_t receive_time; /**< Receive time on DC transmission delay + measurement. */ + uint16_t next_slave; /**< Ring position of next DC slave on that + port. */ + uint32_t delay_to_next_dc; /**< Delay [ns] to next DC slave. */ + } ports[EC_MAX_PORTS]; /**< Port information. */ + uint8_t al_state; /**< Current state of the slave. */ + uint8_t error_flag; /**< Error flag for that slave. */ + uint8_t sync_count; /**< Number of sync managers. */ + uint16_t sdo_count; /**< Number of SDOs. */ + char name[EC_MAX_STRING_LENGTH]; /**< Name of the slave. */ +} ec_slave_info_t; + +/****************************************************************************/ + +/** Domain working counter interpretation. + * + * This is used in ec_domain_state_t. + */ +typedef enum { + EC_WC_ZERO = 0, /**< No registered process data were exchanged. */ + EC_WC_INCOMPLETE, /**< Some of the registered process data were + exchanged. */ + EC_WC_COMPLETE /**< All registered process data were exchanged. */ +} ec_wc_state_t; + +/****************************************************************************/ + +/** Domain state. + * + * This is used for the output parameter of ecrt_domain_state(). + */ +typedef struct { + unsigned int working_counter; /**< Value of the last working counter. */ + ec_wc_state_t wc_state; /**< Working counter interpretation. */ + unsigned int redundancy_active; /**< Redundant link is in use. */ +} ec_domain_state_t; + +/****************************************************************************/ + +/** Direction type for PDO assignment functions. + */ +typedef enum { + EC_DIR_INVALID, /**< Invalid direction. Do not use this value. */ + EC_DIR_OUTPUT, /**< Values written by the master. */ + EC_DIR_INPUT, /**< Values read by the master. */ + EC_DIR_COUNT /**< Number of directions. For internal use only. */ +} ec_direction_t; + +/****************************************************************************/ + +/** Watchdog mode for sync manager configuration. + * + * Used to specify, if a sync manager's watchdog is to be enabled. + */ +typedef enum { + EC_WD_DEFAULT, /**< Use the default setting of the sync manager. */ + EC_WD_ENABLE, /**< Enable the watchdog. */ + EC_WD_DISABLE, /**< Disable the watchdog. */ +} ec_watchdog_mode_t; + +/****************************************************************************/ + +/** PDO entry configuration information. + * + * This is the data type of the \a entries field in ec_pdo_info_t. + * + * \see ecrt_slave_config_pdos(). + */ +typedef struct { + uint16_t index; /**< PDO entry index. */ + uint8_t subindex; /**< PDO entry subindex. */ + uint8_t bit_length; /**< Size of the PDO entry in bit. */ +} ec_pdo_entry_info_t; + +/****************************************************************************/ + +/** PDO configuration information. + * + * This is the data type of the \a pdos field in ec_sync_info_t. + * + * \see ecrt_slave_config_pdos(). + */ +typedef struct { + uint16_t index; /**< PDO index. */ + unsigned int n_entries; /**< Number of PDO entries in \a entries to map. + Zero means, that the default mapping shall be + used (this can only be done if the slave is + present at configuration time). */ + ec_pdo_entry_info_t const *entries; /**< Array of PDO entries to map. Can + either be \a NULL, or must contain + at least \a n_entries values. */ +} ec_pdo_info_t; + +/****************************************************************************/ + +/** Sync manager configuration information. + * + * This can be use to configure multiple sync managers including the PDO + * assignment and PDO mapping. It is used as an input parameter type in + * ecrt_slave_config_pdos(). + */ +typedef struct { + uint8_t index; /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS for a valid sync manager, + but can also be \a 0xff to mark the end of the list. */ + ec_direction_t dir; /**< Sync manager direction. */ + unsigned int n_pdos; /**< Number of PDOs in \a pdos. */ + ec_pdo_info_t const *pdos; /**< Array with PDOs to assign. This must + contain at least \a n_pdos PDOs. */ + ec_watchdog_mode_t watchdog_mode; /**< Watchdog mode. */ +} ec_sync_info_t; + +/****************************************************************************/ + +/** List record type for PDO entry mass-registration. + * + * This type is used for the array parameter of the + * ecrt_domain_reg_pdo_entry_list() + */ +typedef struct { + uint16_t alias; /**< Slave alias address. */ + uint16_t position; /**< Slave position. */ + uint32_t vendor_id; /**< Slave vendor ID. */ + uint32_t product_code; /**< Slave product code. */ + uint16_t index; /**< PDO entry index. */ + uint8_t subindex; /**< PDO entry subindex. */ + unsigned int *offset; /**< Pointer to a variable to store the PDO entry's + (byte-)offset in the process data. */ + unsigned int *bit_position; /**< Pointer to a variable to store a bit + position (0-7) within the \a offset. Can be + NULL, in which case an error is raised if + the PDO entry does not byte-align. */ +} ec_pdo_entry_reg_t; + +/****************************************************************************/ + +/** Request state. + * + * This is used as return type for ecrt_sdo_request_state() and + * ecrt_voe_handler_state(). + */ +typedef enum { + EC_REQUEST_UNUSED, /**< Not requested. */ + EC_REQUEST_BUSY, /**< Request is being processed. */ + EC_REQUEST_SUCCESS, /**< Request was processed successfully. */ + EC_REQUEST_ERROR, /**< Request processing failed. */ +} ec_request_state_t; + +/****************************************************************************/ + +/** Application-layer state. + */ +typedef enum { + EC_AL_STATE_INIT = 1, /**< Init. */ + EC_AL_STATE_PREOP = 2, /**< Pre-operational. */ + EC_AL_STATE_SAFEOP = 4, /**< Safe-operational. */ + EC_AL_STATE_OP = 8, /**< Operational. */ +} ec_al_state_t; + +/****************************************************************************/ + +/** Fields for SII caching. + * + * For use in the method ecrt_master_sii_caching(). + */ +typedef enum { + EC_SII_DISABLE_CACHING = 0, /** Disable SII caching. */ + EC_SII_VENDOR = 1, /** Use vendor ID. */ + EC_SII_PRODUCT = 2, /** Use product code. */ + EC_SII_REVISION = 4, /** Use revision number. */ + EC_SII_SERIAL = 8, /** Use serial number. */ + EC_SII_ALIAS = 16, /** Use alias address. */ +} ec_sii_caching_fields_t; + +/***************************************************************************** + * Global functions + ****************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + +/** Returns the version magic of the realtime interface. + * + * \apiusage{master_any,rt_safe} + * + * \return Value of ECRT_VERSION_MAGIC() at EtherCAT master compile time. + */ +EC_PUBLIC_API unsigned int ecrt_version_magic(void); + +/** Requests an EtherCAT master for realtime operation. + * + * Before an application can access an EtherCAT master, it has to reserve one + * for exclusive use. + * + * In userspace, this is a convenience function for ecrt_open_master() and + * ecrt_master_reserve(). + * + * This function has to be the first function an application has to call to + * use EtherCAT. The function takes the index of the master as its argument. + * The first master has index 0, the n-th master has index n - 1. The number + * of masters has to be specified when loading the master module. + * + * \apiusage{master_idle,blocking} + * + * \return Pointer to the reserved master, otherwise \a NULL. + */ +EC_PUBLIC_API ec_master_t *ecrt_request_master( + unsigned int master_index /**< Index of the master to request. */ + ); + +#ifndef __KERNEL__ + +/** Opens an EtherCAT master for userspace access. + * + * This function has to be the first function an application has to call to + * use EtherCAT. The function takes the index of the master as its argument. + * The first master has index 0, the n-th master has index n - 1. The number + * of masters has to be specified when loading the master module. + * + * For convenience, the function ecrt_request_master() can be used. + * + * \apiusage{master_idle,blocking} + * + * \return Pointer to the opened master, otherwise \a NULL. + */ +EC_PUBLIC_API ec_master_t *ecrt_open_master( + unsigned int master_index /**< Index of the master to request. */ + ); + +#endif // #ifndef __KERNEL__ + +/** Releases a requested EtherCAT master. + * + * After use, a master it has to be released to make it available for other + * applications. + * + * This method frees all created data structures. It should not be called in + * realtime context. + * + * If the master was activated, ecrt_master_deactivate() is called internally. + * + * \apiusage{master_any,blocking} + */ +EC_PUBLIC_API void ecrt_release_master( + ec_master_t *master /**< EtherCAT master */ + ); + +/***************************************************************************** + * Master methods + ****************************************************************************/ + +#ifndef __KERNEL__ + +/** Reserves an EtherCAT master for realtime operation. + * + * Before an application can use PDO/domain registration functions or SDO + * request functions on the master, it has to reserve one for exclusive use. + * + * \apiusage{master_idle,blocking} + * + * \return 0 in case of success, else < 0 + */ +EC_PUBLIC_API int ecrt_master_reserve( + ec_master_t *master /**< EtherCAT master */ + ); + +#endif // #ifndef __KERNEL__ + +#ifdef __KERNEL__ + +/** Sets the locking callbacks. + * + * For concurrent master access, i. e. if other instances than the application + * want to send and receive datagrams on the network, the application has to + * provide a callback mechanism. This method takes two function pointers as + * its parameters. Asynchronous master access (like EoE processing) is only + * possible if the callbacks have been set. + * + * The task of the send callback (\a send_cb) is to decide, if the network + * hardware is currently accessible and whether or not to call the + * ecrt_master_send_ext() method. + * + * The task of the receive callback (\a receive_cb) is to decide, if a call to + * ecrt_master_receive() is allowed and to execute it respectively. + * + * \apiusage{master_idle,blocking} + * + * \attention This method has to be called before ecrt_master_activate(). + */ +void ecrt_master_callbacks( + ec_master_t *master, /**< EtherCAT master */ + void (*send_cb)(void *), /**< Datagram sending callback. */ + void (*receive_cb)(void *), /**< Receive callback. */ + void *cb_data /**< Arbitrary pointer passed to the callback functions. + */ + ); + +#endif /* __KERNEL__ */ + +/** Creates a new process data domain. + * + * For process data exchange, at least one process data domain is needed. + * This method creates a new process data domain and returns a pointer to the + * new domain object. This object can be used for registering PDOs and + * exchanging them in cyclic operation. + * + * This method allocates memory and should be called in non-realtime context + * before ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return Pointer to the new domain on success, else NULL. + */ +EC_PUBLIC_API ec_domain_t *ecrt_master_create_domain( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Obtains a slave configuration. + * + * Creates a slave configuration object for the given \a alias and \a position + * tuple and returns it. If a configuration with the same \a alias and \a + * position already exists, it will be re-used. In the latter case, the given + * vendor ID and product code are compared to the stored ones. On mismatch, an + * error message is raised and the function returns \a NULL. + * + * Slaves are addressed with the \a alias and \a position parameters. + * - If \a alias is zero, \a position is interpreted as the desired slave's + * ring position. + * - If \a alias is non-zero, it matches a slave with the given alias. In this + * case, \a position is interpreted as ring offset, starting from the + * aliased slave, so a position of zero means the aliased slave itself and a + * positive value matches the n-th slave behind the aliased one. + * + * If the slave with the given address is found during the configuration, + * its vendor ID and product code are matched against the given value. On + * mismatch, the slave is not configured and an error message is raised. + * + * If different slave configurations are pointing to the same slave during + * configuration, a warning is raised and only the first configuration is + * applied. + * + * This method allocates memory and should be called in non-realtime context + * before ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval >0 Pointer to the slave configuration structure. + * \retval NULL in the error case. + */ +EC_PUBLIC_API ec_slave_config_t *ecrt_master_slave_config( + ec_master_t *master, /**< EtherCAT master */ + uint16_t alias, /**< Slave alias. */ + uint16_t position, /**< Slave position. */ + uint32_t vendor_id, /**< Expected vendor ID. */ + uint32_t product_code /**< Expected product code. */ + ); + +/** Selects the reference clock for distributed clocks. + * + * If this method is not called for a certain master, or if the slave + * configuration pointer is NULL, then the first slave with DC functionality + * will provide the reference clock. + * + * \apiusage{master_idle,blocking} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_select_reference_clock( + ec_master_t *master, /**< EtherCAT master. */ + ec_slave_config_t *sc /**< Slave config of the slave to use as the + * reference slave (or NULL). */ + ); + +/** Obtains master information. + * + * No memory is allocated on the heap in this function. + * + * \apiusage{master_any,rt_safe} + * + * \attention The pointer to this structure must point to a valid variable. + * + * \return 0 in case of success, else < 0 + */ +EC_PUBLIC_API int ecrt_master( + ec_master_t *master, /**< EtherCAT master */ + ec_master_info_t *master_info /**< Structure that will output the + information */ + ); + +/** Obtains network scan progress information. + * + * No memory is allocated on the heap in this function. + * + * \apiusage{master_any,rt_safe} + * + * \attention The pointer to this structure must point to a valid variable. + * + * \return 0 in case of success, else < 0 + */ +EC_PUBLIC_API int ecrt_master_scan_progress( + ec_master_t *master, /**< EtherCAT master */ + ec_master_scan_progress_t *progress /**< Structure that will output + the progress information. */ + ); + +/** Obtains slave information. + * + * Tries to find the slave with the given ring position. The obtained + * information is stored in a structure. No memory is allocated on the heap in + * this function. + * + * \apiusage{master_any,blocking} + * + * \attention The pointer to this structure must point to a valid variable. + * + * \return 0 in case of success, else < 0 + */ +EC_PUBLIC_API int ecrt_master_get_slave( + ec_master_t *master, /**< EtherCAT master */ + uint16_t slave_position, /**< Slave position. */ + ec_slave_info_t *slave_info /**< Structure that will output the + information */ + ); + +#ifndef __KERNEL__ + +/** Returns the proposed configuration of a slave's sync manager. + * + * Fills a given ec_sync_info_t structure with the attributes of a sync + * manager. The \a pdos field of the return value is left empty. Use + * ecrt_master_get_pdo() to get the PDO information. + * + * \apiusage{master_any,blocking} + * + * \return zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_master_get_sync_manager( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + ec_sync_info_t *sync /**< Pointer to output structure. */ + ); + +/** Returns information about a currently assigned PDO. + * + * Fills a given ec_pdo_info_t structure with the attributes of a currently + * assigned PDO of the given sync manager. The \a entries field of the return + * value is left empty. Use ecrt_master_get_pdo_entry() to get the PDO + * entry information. + * + * \apiusage{master_any,blocking} + * + * \retval zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_master_get_pdo( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + uint16_t pos, /**< Zero-based PDO position. */ + ec_pdo_info_t *pdo /**< Pointer to output structure. */ + ); + +/** Returns information about a currently mapped PDO entry. + * + * Fills a given ec_pdo_entry_info_t structure with the attributes of a + * currently mapped PDO entry of the given PDO. + * + * \apiusage{master_any,blocking} + * + * \retval zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_master_get_pdo_entry( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + uint16_t pdo_pos, /**< Zero-based PDO position. */ + uint16_t entry_pos, /**< Zero-based PDO entry position. */ + ec_pdo_entry_info_t *entry /**< Pointer to output structure. */ + ); + +#endif /* #ifndef __KERNEL__ */ + +/** Executes an SDO download request to write data to a slave. + * + * This request is processed by the master state machine. This method blocks, + * until the request has been processed and may not be called in realtime + * context. + * + * \apiusage{master_any,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_master_sdo_download( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint16_t index, /**< Index of the SDO. */ + uint8_t subindex, /**< Subindex of the SDO. */ + const uint8_t *data, /**< Data buffer to download. */ + size_t data_size, /**< Size of the data buffer. */ + uint32_t *abort_code /**< Abort code of the SDO download. */ + ); + +/** Executes an SDO download request to write data to a slave via complete + * access. + * + * This request is processed by the master state machine. This method blocks, + * until the request has been processed and may not be called in realtime + * context. + * + * \apiusage{master_any,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_master_sdo_download_complete( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint16_t index, /**< Index of the SDO. */ + const uint8_t *data, /**< Data buffer to download. */ + size_t data_size, /**< Size of the data buffer. */ + uint32_t *abort_code /**< Abort code of the SDO download. */ + ); + +/** Executes an SDO upload request to read data from a slave. + * + * This request is processed by the master state machine. This method blocks, + * until the request has been processed and may not be called in realtime + * context. + * + * \apiusage{master_any,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_master_sdo_upload( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint16_t index, /**< Index of the SDO. */ + uint8_t subindex, /**< Subindex of the SDO. */ + uint8_t *target, /**< Target buffer for the upload. */ + size_t target_size, /**< Size of the target buffer. */ + size_t *result_size, /**< Uploaded data size. */ + uint32_t *abort_code /**< Abort code of the SDO upload. */ + ); + +/** Executes an SoE write request. + * + * Starts writing an IDN and blocks until the request was processed, or an + * error occurred. + * + * \apiusage{master_any,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_master_write_idn( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t drive_no, /**< Drive number. */ + uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */ + const uint8_t *data, /**< Pointer to data to write. */ + size_t data_size, /**< Size of data to write. */ + uint16_t *error_code /**< Pointer to variable, where an SoE error code + can be stored. */ + ); + +/** Executes an SoE read request. + * + * Starts reading an IDN and blocks until the request was processed, or an + * error occurred. + * + * \apiusage{master_any,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_master_read_idn( + ec_master_t *master, /**< EtherCAT master. */ + uint16_t slave_position, /**< Slave position. */ + uint8_t drive_no, /**< Drive number. */ + uint16_t idn, /**< SoE IDN (see ecrt_slave_config_idn()). */ + uint8_t *target, /**< Pointer to memory where the read data can be + stored. */ + size_t target_size, /**< Size of the memory \a target points to. */ + size_t *result_size, /**< Actual size of the received data. */ + uint16_t *error_code /**< Pointer to variable, where an SoE error code + can be stored. */ + ); + +/** Finishes the configuration phase and prepares for cyclic operation. + * + * This function tells the master that the configuration phase is finished and + * the realtime operation will begin. The function allocates internal memory + * for the domains and calculates the logical FMMU addresses for domain + * members. It tells the master state machine that the configuration is + * now to be applied to the network. + * + * \apiusage{master_idle,blocking} + * + * \attention After this function has been called, the realtime application is + * in charge of cyclically calling ecrt_master_send() and + * ecrt_master_receive() to ensure network communication. Before calling this + * function, the master thread is responsible for that, so these functions may + * not be called! The method itself allocates memory and should not be called + * in realtime context. + * + * \return 0 in case of success, else < 0 + */ +EC_PUBLIC_API int ecrt_master_activate( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Deactivates the master. + * + * Removes the master configuration. All objects created by + * ecrt_master_create_domain(), ecrt_master_slave_config(), ecrt_domain_data() + * ecrt_slave_config_create_sdo_request() and + * ecrt_slave_config_create_voe_handler() are freed, so pointers to them + * become invalid. + * + * \apiusage{master_op,blocking} + * + * This method should not be called in realtime context. + * \return 0 on success, otherwise negative error code. + * \retval 0 Success. + * \retval -EINVAL Master has not been activated before. + */ +EC_PUBLIC_API int ecrt_master_deactivate( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Set interval between calls to ecrt_master_send(). + * + * This information helps the master to decide, how much data can be appended + * to a frame by the master state machine. When the master is configured with + * --enable-hrtimers, this is used to calculate the scheduling of the master + * thread. + * + * \apiusage{master_idle,blocking} + * + * \retval 0 on success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_master_set_send_interval( + ec_master_t *master, /**< EtherCAT master. */ + size_t send_interval /**< Send interval in us */ + ); + +/** Sends all datagrams in the queue. + * + * This method takes all datagrams, that have been queued for transmission, + * puts them into frames, and passes them to the Ethernet device for sending. + * + * Has to be called cyclically by the application after ecrt_master_activate() + * has returned. + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_send( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Fetches received frames from the hardware and processes the datagrams. + * + * Queries the network device for received frames by calling the interrupt + * service routine. Extracts received datagrams and dispatches the results to + * the datagram objects in the queue. Received datagrams, and the ones that + * timed out, will be marked, and dequeued. + * + * Has to be called cyclically by the realtime application after + * ecrt_master_activate() has returned. + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_receive( + ec_master_t *master /**< EtherCAT master. */ + ); + +#ifdef __KERNEL__ +/** Sends non-application datagrams. + * + * This method has to be called in the send callback function passed via + * ecrt_master_callbacks() to allow the sending of non-application datagrams. + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise negative error code. + * \retval -EAGAIN Lock could not be acquired, try again later. + */ +int ecrt_master_send_ext( + ec_master_t *master /**< EtherCAT master. */ + ); +#endif + +/** Reads the current master state. + * + * Stores the master state information in the given \a state structure. + * + * This method returns a global state. For the link-specific states in a + * redundant network topology, use the ecrt_master_link_state() method. + * + * \apiusage{master_any,rt_safe} + * + * \return Zero on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_state( + const ec_master_t *master, /**< EtherCAT master. */ + ec_master_state_t *state /**< Structure to store the information. */ + ); + +/** Reads the current state of a redundant link. + * + * Stores the link state information in the given \a state structure. + * + * \apiusage{master_any,rt_safe} + * + * \return Zero on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_link_state( + const ec_master_t *master, /**< EtherCAT master. */ + unsigned int dev_idx, /**< Index of the device (0 = main device, 1 = + first backup device, ...). */ + ec_master_link_state_t *state /**< Structure to store the information. + */ + ); + +/** Sets the application time. + * + * The master has to know the application's time when operating slaves with + * distributed clocks. The time is not incremented by the master itself, so + * this method has to be called cyclically. + * + * \attention The time passed to this method is used to calculate the phase of + * the slaves' SYNC0/1 interrupts. It should be called constantly at the same + * point of the realtime cycle. So it is recommended to call it at the start + * of the calculations to avoid deviancies due to changing execution times. + * Avoid calling this method before the realtime cycle is established. + * + * The time is used when setting the slaves' System Time Offset and + * Cyclic Operation Start Time registers and when synchronizing the + * DC reference clock to the application time via + * ecrt_master_sync_reference_clock(). + * + * The time is defined as nanoseconds from 2000-01-01 00:00. Converting an + * epoch time can be done with the EC_TIMEVAL2NANO() macro, but is not + * necessary, since the absolute value is not of any interest. + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_application_time( + ec_master_t *master, /**< EtherCAT master. */ + uint64_t app_time /**< Application time. */ + ); + +/** Queues the DC reference clock drift compensation datagram for sending. + * + * The reference clock will by synchronized to the application time provided + * by the last call off ecrt_master_application_time(). + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise negative error code. + * \retval 0 Success. + * \retval -ENXIO No reference clock found. + */ +EC_PUBLIC_API int ecrt_master_sync_reference_clock( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Queues the DC reference clock drift compensation datagram for sending. + * + * The reference clock will by synchronized to the time passed in the + * sync_time parameter. + * + * Has to be called by the application after ecrt_master_activate() + * has returned. + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise negative error code. + * \retval 0 Success. + * \retval -ENXIO No reference clock found. + */ +EC_PUBLIC_API int ecrt_master_sync_reference_clock_to( + ec_master_t *master, /**< EtherCAT master. */ + uint64_t sync_time /**< Sync reference clock to this time. */ + ); + +/** Queues the DC clock drift compensation datagram for sending. + * + * All slave clocks synchronized to the reference clock. + * + * Has to be called by the application after ecrt_master_activate() + * has returned. + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + * \retval 0 Success. + * \retval -ENXIO No reference clock found. + */ +EC_PUBLIC_API int ecrt_master_sync_slave_clocks( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Get the lower 32 bit of the reference clock system time. + * + * This method can be used to synchronize the master to the reference clock. + * + * The reference clock system time is queried via the + * ecrt_master_sync_slave_clocks() method, that reads the system time of the + * reference clock and writes it to the slave clocks (so be sure to call it + * cyclically to get valid data). + * + * \attention The returned time is the system time of the reference clock + * minus the transmission delay of the reference clock. + * + * Calling this method makes only sense in realtime context (after master + * activation), when the ecrt_master_sync_slave_clocks() method is called + * cyclically. + * + * \apiusage{master_op,rt_safe} + * + * \retval 0 success, system time was written into \a time. + * \retval -ENXIO No reference clock found. + * \retval -EIO Slave synchronization datagram was not received. + */ +EC_PUBLIC_API int ecrt_master_reference_clock_time( + const ec_master_t *master, /**< EtherCAT master. */ + uint32_t *time /**< Pointer to store the queried system time. */ + ); + +/** Queues the DC synchrony monitoring datagram for sending. + * + * The datagram broadcast-reads all "System time difference" registers (\a + * 0x092c) to get an upper estimation of the DC synchrony. The result can be + * checked with the ecrt_master_sync_monitor_process() method. + * + * \apiusage{master_op,rt_safe} + * + * \return Zero on success, otherwise a negative error code. + */ +EC_PUBLIC_API int ecrt_master_sync_monitor_queue( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Processes the DC synchrony monitoring datagram. + * + * If the sync monitoring datagram was sent before with + * ecrt_master_sync_monitor_queue(), the result can be queried with this + * method. + * + * \apiusage{master_op,rt_safe} + * + * \return Upper estimation of the maximum time difference in ns, -1 on error. + * \retval (uint32_t)-1 Error. + */ +EC_PUBLIC_API uint32_t ecrt_master_sync_monitor_process( + const ec_master_t *master /**< EtherCAT master. */ + ); + +/** Retry configuring slaves. + * + * Via this method, the application can tell the master to bring all slaves to + * OP state. In general, this is not necessary, because it is automatically + * done by the master. But with special slaves, that can be reconfigured by + * the vendor during runtime, it can be useful. + * + * Calling this method only makes sense in realtime context (after + * activation), because slaves will not be configured before. + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_reset( + ec_master_t *master /**< EtherCAT master. */ + ); + +/** Set the SII caching method. + * + * Via this method, the application can tell the master to either which fields + * to use for looking up cached SII content pages or to disable SII caching at + * all. + * + * The default when starting up is defined in the master configuration file. + * The caching method stays valid as long as the master is existing, so it + * could be set by a prior application. + * + * The allowed fields are defined in ec_sii_caching_fields_t. A typical setup + * could be: + * + * \code + * if (ecrt_master_sii_caching(master, + * EC_SII_VENDOR | EC_SII_PRODUCT | EC_SII_REVISION)) { + * fprintf(stderr, "Failed to set up SII caching method.\n"); + * } + * \endcode + * + * A value of zero disables SII caching completely, thus the SII contents are + * completely loaded from every slave during scanning: + * + * \code + * if (ecrt_master_sii_caching(master, EC_SII_DISABLE_CACHING)) { + * fprintf(stderr, "Failed to disable SII caching.\n"); + * } + * \endcode + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_master_sii_caching( + ec_master_t *master, /**< EtherCAT master. */ + ec_sii_caching_fields_t fields /** Fields to use for cache lookup. */ + ); + +/***************************************************************************** + * Slave configuration methods + ****************************************************************************/ + +/** Configure a sync manager. + * + * Sets the direction of a sync manager. This overrides the direction bits + * from the default control register from SII. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_slave_config_sync_manager( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + ec_direction_t direction, /**< Input/Output. */ + ec_watchdog_mode_t watchdog_mode /** Watchdog mode. */ + ); + +/** Configure a slave's watchdog times. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_slave_config_watchdog( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t watchdog_divider, /**< Number of 40 ns intervals (register + 0x0400). Used as a base unit for all + slave watchdogs^. If set to zero, the + value is not written, so the default is + used. */ + uint16_t watchdog_intervals /**< Number of base intervals for sync + manager watchdog (register 0x0420). If + set to zero, the value is not written, + so the default is used. */ + ); + +/** Add a PDO to a sync manager's PDO assignment. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \see ecrt_slave_config_pdos() + * \return zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_slave_config_pdo_assign_add( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t sync_index, /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + uint16_t index /**< Index of the PDO to assign. */ + ); + +/** Clear a sync manager's PDO assignment. + * + * This can be called before assigning PDOs via + * ecrt_slave_config_pdo_assign_add(), to clear the default assignment of a + * sync manager. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \see ecrt_slave_config_pdos() + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_slave_config_pdo_assign_clear( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t sync_index /**< Sync manager index. Must be less + than #EC_MAX_SYNC_MANAGERS. */ + ); + +/** Add a PDO entry to the given PDO's mapping. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \see ecrt_slave_config_pdos() + * \return zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_slave_config_pdo_mapping_add( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t pdo_index, /**< Index of the PDO. */ + uint16_t entry_index, /**< Index of the PDO entry to add to the PDO's + mapping. */ + uint8_t entry_subindex, /**< Subindex of the PDO entry to add to the + PDO's mapping. */ + uint8_t entry_bit_length /**< Size of the PDO entry in bit. */ + ); + +/** Clear the mapping of a given PDO. + * + * This can be called before mapping PDO entries via + * ecrt_slave_config_pdo_mapping_add(), to clear the default mapping. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \see ecrt_slave_config_pdos() + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_slave_config_pdo_mapping_clear( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t pdo_index /**< Index of the PDO. */ + ); + +/** Specify a complete PDO configuration. + * + * This function is a convenience wrapper for the functions + * ecrt_slave_config_sync_manager(), ecrt_slave_config_pdo_assign_clear(), + * ecrt_slave_config_pdo_assign_add(), ecrt_slave_config_pdo_mapping_clear() + * and ecrt_slave_config_pdo_mapping_add(), that are better suitable for + * automatic code generation. + * + * The following example shows, how to specify a complete configuration, + * including the PDO mappings. With this information, the master is able to + * reserve the complete process data, even if the slave is not present at + * configuration time: + * + * \code + * ec_pdo_entry_info_t el3162_channel1[] = { + * {0x3101, 1, 8}, // status + * {0x3101, 2, 16} // value + * }; + * + * ec_pdo_entry_info_t el3162_channel2[] = { + * {0x3102, 1, 8}, // status + * {0x3102, 2, 16} // value + * }; + * + * ec_pdo_info_t el3162_pdos[] = { + * {0x1A00, 2, el3162_channel1}, + * {0x1A01, 2, el3162_channel2} + * }; + * + * ec_sync_info_t el3162_syncs[] = { + * {2, EC_DIR_OUTPUT}, + * {3, EC_DIR_INPUT, 2, el3162_pdos}, + * {0xff} + * }; + * + * if (ecrt_slave_config_pdos(sc_ana_in, EC_END, el3162_syncs)) { + * // handle error + * } + * \endcode + * + * The next example shows, how to configure the PDO assignment only. The + * entries for each assigned PDO are taken from the PDO's default mapping. + * Please note, that PDO entry registration will fail, if the PDO + * configuration is left empty and the slave is offline. + * + * \code + * ec_pdo_info_t pdos[] = { + * {0x1600}, // Channel 1 + * {0x1601} // Channel 2 + * }; + * + * ec_sync_info_t syncs[] = { + * {3, EC_DIR_INPUT, 2, pdos}, + * }; + * + * if (ecrt_slave_config_pdos(slave_config_ana_in, 1, syncs)) { + * // handle error + * } + * \endcode + * + * Processing of \a syncs will stop, if + * - the number of processed items reaches \a n_syncs, or + * - the \a index member of an ec_sync_info_t item is 0xff. In this case, + * \a n_syncs should set to a number greater than the number of list items; + * using EC_END is recommended. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return zero on success, else non-zero + */ +EC_PUBLIC_API int ecrt_slave_config_pdos( + ec_slave_config_t *sc, /**< Slave configuration. */ + unsigned int n_syncs, /**< Number of sync manager configurations in + \a syncs. */ + const ec_sync_info_t syncs[] /**< Array of sync manager + configurations. */ + ); + +/** Registers a PDO entry for process data exchange in a domain. + * + * Searches the assigned PDOs for the given PDO entry. An error is raised, if + * the given entry is not mapped. Otherwise, the corresponding sync manager + * and FMMU configurations are provided for slave configuration and the + * respective sync manager's assigned PDOs are appended to the given domain, + * if not already done. The offset of the requested PDO entry's data inside + * the domain's process data is returned. Optionally, the PDO entry bit + * position (0-7) can be retrieved via the \a bit_position output parameter. + * This pointer may be \a NULL, in this case an error is raised if the PDO + * entry does not byte-align. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval >=0 Success: Offset of the PDO entry's process data. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_reg_pdo_entry( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t entry_index, /**< Index of the PDO entry to register. */ + uint8_t entry_subindex, /**< Subindex of the PDO entry to register. */ + ec_domain_t *domain, /**< Domain. */ + unsigned int *bit_position /**< Optional address if bit addressing + is desired */ + ); + +/** Registers a PDO entry using its position. + * + * Similar to ecrt_slave_config_reg_pdo_entry(), but not using PDO indices but + * offsets in the PDO mapping, because PDO entry indices may not be unique + * inside a slave's PDO mapping. An error is raised, if + * one of the given positions is out of range. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval >=0 Success: Offset of the PDO entry's process data. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_reg_pdo_entry_pos( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t sync_index, /**< Sync manager index. */ + unsigned int pdo_pos, /**< Position of the PDO inside the SM. */ + unsigned int entry_pos, /**< Position of the entry inside the PDO. */ + ec_domain_t *domain, /**< Domain. */ + unsigned int *bit_position /**< Optional address if bit addressing + is desired */ + ); + +/** Configure distributed clocks. + * + * Sets the AssignActivate word and the cycle and shift times for the sync + * signals. + * + * The AssignActivate word is vendor-specific and can be taken from the XML + * device description file (Device -> Dc -> AssignActivate). Set this to zero, + * if the slave shall be operated without distributed clocks (default). + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \attention The \a sync1_shift time is ignored. + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_slave_config_dc( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t assign_activate, /**< AssignActivate word. */ + uint32_t sync0_cycle, /**< SYNC0 cycle time [ns]. */ + int32_t sync0_shift, /**< SYNC0 shift time [ns]. */ + uint32_t sync1_cycle, /**< SYNC1 cycle time [ns]. */ + int32_t sync1_shift /**< SYNC1 shift time [ns]. */ + ); + +/** Add an SDO configuration. + * + * An SDO configuration is stored in the slave configuration object and is + * downloaded to the slave whenever the slave is being configured by the + * master. This usually happens once on master activation, but can be repeated + * subsequently, for example after the slave's power supply failed. + * + * \attention The SDOs for PDO assignment (\p 0x1C10 - \p 0x1C2F) and PDO + * mapping (\p 0x1600 - \p 0x17FF and \p 0x1A00 - \p 0x1BFF) should not be + * configured with this function, because they are part of the slave + * configuration done by the master. Please use ecrt_slave_config_pdos() and + * friends instead. + * + * This is the generic function for adding an SDO configuration. Please note + * that the this function does not do any endianness correction. If + * datatype-specific functions are needed (that automatically correct the + * endianness), have a look at ecrt_slave_config_sdo8(), + * ecrt_slave_config_sdo16() and ecrt_slave_config_sdo32(). + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_sdo( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t index, /**< Index of the SDO to configure. */ + uint8_t subindex, /**< Subindex of the SDO to configure. */ + const uint8_t *data, /**< Pointer to the data. */ + size_t size /**< Size of the \a data. */ + ); + +/** Add a configuration value for an 8-bit SDO. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \see ecrt_slave_config_sdo(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_sdo8( + ec_slave_config_t *sc, /**< Slave configuration */ + uint16_t sdo_index, /**< Index of the SDO to configure. */ + uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ + uint8_t value /**< Value to set. */ + ); + +/** Add a configuration value for a 16-bit SDO. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \see ecrt_slave_config_sdo(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_sdo16( + ec_slave_config_t *sc, /**< Slave configuration */ + uint16_t sdo_index, /**< Index of the SDO to configure. */ + uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ + uint16_t value /**< Value to set. */ + ); + +/** Add a configuration value for a 32-bit SDO. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \see ecrt_slave_config_sdo(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_sdo32( + ec_slave_config_t *sc, /**< Slave configuration */ + uint16_t sdo_index, /**< Index of the SDO to configure. */ + uint8_t sdo_subindex, /**< Subindex of the SDO to configure. */ + uint32_t value /**< Value to set. */ + ); + +/** Add configuration data for a complete SDO. + * + * The SDO data are transferred via CompleteAccess. Data for the first + * subindex (0) have to be included. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \see ecrt_slave_config_sdo(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_complete_sdo( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t index, /**< Index of the SDO to configure. */ + const uint8_t *data, /**< Pointer to the data. */ + size_t size /**< Size of the \a data. */ + ); + +/** Set the size of the CoE emergency ring buffer. + * + * The initial size is zero, so all messages will be dropped. This method can + * be called even after master activation, but it will clear the ring buffer! + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return 0 on success, or negative error code. + */ +EC_PUBLIC_API int ecrt_slave_config_emerg_size( + ec_slave_config_t *sc, /**< Slave configuration. */ + size_t elements /**< Number of records of the CoE emergency ring. */ + ); + +/** Read and remove one record from the CoE emergency ring buffer. + * + * A record consists of 8 bytes: + * + * Byte 0-1: Error code (little endian) + * Byte 2: Error register + * Byte 3-7: Data + * + * Calling this method makes only sense in realtime context (after master + * activation). + * + * \return 0 on success (record popped), or negative error code (i. e. + * -ENOENT, if ring is empty). + * + * \apiusage{master_op,any_context} + */ +EC_PUBLIC_API int ecrt_slave_config_emerg_pop( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t *target /**< Pointer to target memory (at least + EC_COE_EMERGENCY_MSG_SIZE bytes). */ + ); + +/** Clears CoE emergency ring buffer and the overrun counter. + * + * Calling this method makes only sense in realtime context (after master + * activation). + * + * \apiusage{master_op,any_context} + * + * \return 0 on success, or negative error code. + * + */ +EC_PUBLIC_API int ecrt_slave_config_emerg_clear( + ec_slave_config_t *sc /**< Slave configuration. */ + ); + +/** Read the number of CoE emergency overruns. + * + * The overrun counter will be incremented when a CoE emergency message could + * not be stored in the ring buffer and had to be dropped. Call + * ecrt_slave_config_emerg_clear() to reset the counter. + * + * Calling this method makes only sense in realtime context (after master + * activation). + * + * \apiusage{master_op,any_context} + * + * \return Number of overruns since last clear, or negative error code. + * + */ +EC_PUBLIC_API int ecrt_slave_config_emerg_overruns( + const ec_slave_config_t *sc /**< Slave configuration. */ + ); + +/** Create an SDO request to exchange SDOs during realtime operation. + * + * The created SDO request object is freed automatically when the master is + * released. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return New SDO request, or NULL on error. + */ +EC_PUBLIC_API ec_sdo_request_t *ecrt_slave_config_create_sdo_request( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint16_t index, /**< SDO index. */ + uint8_t subindex, /**< SDO subindex. */ + size_t size /**< Data size to reserve. */ + ); + +/** Create an SoE request to exchange SoE IDNs during realtime operation. + * + * The created SoE request object is freed automatically when the master is + * released. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return New SoE request, or NULL on error. + */ +EC_PUBLIC_API ec_soe_request_t *ecrt_slave_config_create_soe_request( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t drive_no, /**< Drive number. */ + uint16_t idn, /**< Sercos ID-Number. */ + size_t size /**< Data size to reserve. */ + ); + +/** Create an VoE handler to exchange vendor-specific data during realtime + * operation. + * + * The number of VoE handlers per slave configuration is not limited, but + * usually it is enough to create one for sending and one for receiving, if + * both can be done simultaneously. + * + * The created VoE handler object is freed automatically when the master is + * released. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return New VoE handler, or NULL on error. + */ +EC_PUBLIC_API ec_voe_handler_t *ecrt_slave_config_create_voe_handler( + ec_slave_config_t *sc, /**< Slave configuration. */ + size_t size /**< Data size to reserve. */ + ); + +/** Create a register request to exchange EtherCAT register contents during + * realtime operation. + * + * This interface should not be used to take over master functionality, + * instead it is intended for debugging and monitoring reasons. + * + * The created register request object is freed automatically when the master + * is released. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \return New register request, or NULL on error. + */ +EC_PUBLIC_API ec_reg_request_t *ecrt_slave_config_create_reg_request( + ec_slave_config_t *sc, /**< Slave configuration. */ + size_t size /**< Data size to reserve. */ + ); + +/** Outputs the state of the slave configuration. + * + * Stores the state information in the given \a state structure. The state + * information is updated by the master state machine, so it may take a few + * cycles, until it changes. + * + * \attention If the state of process data exchange shall be monitored in + * realtime, ecrt_domain_state() should be used. + * + * \apiusage{master_op,rt_safe} + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_state( + const ec_slave_config_t *sc, /**< Slave configuration */ + ec_slave_config_state_t *state /**< State object to write to. */ + ); + +/** Add an SoE IDN configuration. + * + * A configuration for a Sercos-over-EtherCAT IDN is stored in the slave + * configuration object and is written to the slave whenever the slave is + * being configured by the master. This usually happens once on master + * activation, but can be repeated subsequently, for example after the slave's + * power supply failed. + * + * The \a idn parameter can be separated into several sections: + * - Bit 15: Standard data (0) or Product data (1) + * - Bit 14 - 12: Parameter set (0 - 7) + * - Bit 11 - 0: Data block number (0 - 4095) + * + * Please note that the this function does not do any endianness correction. + * Multi-byte data have to be passed in EtherCAT endianness (little-endian). + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_idn( + ec_slave_config_t *sc, /**< Slave configuration. */ + uint8_t drive_no, /**< Drive number. */ + uint16_t idn, /**< SoE IDN. */ + ec_al_state_t state, /**< AL state in which to write the IDN (PREOP or + SAFEOP). */ + const uint8_t *data, /**< Pointer to the data. */ + size_t size /**< Size of the \a data. */ + ); + +/** Adds a feature flag to a slave configuration. + * + * Feature flags are a generic way to configure slave-specific behavior. + * + * Multiple calls with the same slave configuration and key will overwrite the + * configuration. + * + * The following flags may be available: + * - AssignToPdi: Zero (default) keeps the slave information interface (SII) + * assigned to EtherCAT (except during transition to PREOP). Non-zero + * assigns the SII to the slave controller side before going to PREOP and + * leaves it there until a write command happens. + * - WaitBeforeSAFEOPms: Number of milliseconds to wait before commanding the + * transition from PREOP to SAFEOP. This can be used as a workaround for + * slaves that need a little time to initialize. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_flag( + ec_slave_config_t *sc, /**< Slave configuration. */ + const char *key, /**< Key as null-terminated ASCII string. */ + int32_t value /**< Value to store. */ + ); + +/** Sets the link/MAC address for Ethernet-over-EtherCAT (EoE) operation. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * The MAC address is stored in the slave configuration object and will be + * written to the slave during the configuration process. + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_eoe_mac_address( + ec_slave_config_t *sc, /**< Slave configuration. */ + const unsigned char *mac_address /**< MAC address. */ + ); + +/** Sets the IP address for Ethernet-over-EtherCAT (EoE) operation. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * The IP address is stored in the slave configuration object and will be + * written to the slave during the configuration process. + * + * The IP address is passed by-value as a `struct in_addr`. This structure + * contains the 32-bit IPv4 address in network byte order (big endian). + * + * A string-represented IPv4 address can be converted to a `struct in_addr` + * for example via the POSIX function `inet_pton()` (see man 3 inet_pton): + * + * \code{.c} + * #include + * struct in_addr addr; + * if (inet_aton("192.168.0.1", &addr) == 0) { + * fprintf(stderr, "Failed to convert IP address.\n"); + * return -1; + * } + * if (ecrt_slave_config_eoe_ip_address(sc, addr)) { + * fprintf(stderr, "Failed to set IP address.\n"); + * return -1; + * } + * \endcode + * + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_eoe_ip_address( + ec_slave_config_t *sc, /**< Slave configuration. */ + struct in_addr ip_address /**< IPv4 address. */ + ); + +/** Sets the subnet mask for Ethernet-over-EtherCAT (EoE) operation. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * The subnet mask is stored in the slave configuration object and will be + * written to the slave during the configuration process. + * + * The subnet mask is passed by-value as a `struct in_addr`. This structure + * contains the 32-bit mask in network byte order (big endian). + * + * See ecrt_slave_config_eoe_ip_address() on how to convert string-coded masks + * to `struct in_addr`. + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_eoe_subnet_mask( + ec_slave_config_t *sc, /**< Slave configuration. */ + struct in_addr subnet_mask /**< IPv4 subnet mask. */ + ); + +/** Sets the gateway address for Ethernet-over-EtherCAT (EoE) operation. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * The gateway address is stored in the slave configuration object and will be + * written to the slave during the configuration process. + * + * The address is passed by-value as a `struct in_addr`. This structure + * contains the 32-bit IPv4 address in network byte order (big endian). + * + * See ecrt_slave_config_eoe_ip_address() on how to convert string-coded IPv4 + * addresses to `struct in_addr`. + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_eoe_default_gateway( + ec_slave_config_t *sc, /**< Slave configuration. */ + struct in_addr gateway_address /**< Gateway's IPv4 address. */ + ); + +/** Sets the IPv4 address of the DNS server for Ethernet-over-EtherCAT (EoE) + * operation. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * The DNS server address is stored in the slave configuration object and will + * be written to the slave during the configuration process. + * + * The address is passed by-value as a `struct in_addr`. This structure + * contains the 32-bit IPv4 address in network byte order (big endian). + * + * See ecrt_slave_config_eoe_ip_address() on how to convert string-coded IPv4 + * addresses to `struct in_addr`. + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_eoe_dns_address( + ec_slave_config_t *sc, /**< Slave configuration. */ + struct in_addr dns_address /**< IPv4 address of the DNS server. */ + ); + +/** Sets the host name for Ethernet-over-EtherCAT (EoE) operation. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * The host name is stored in the slave configuration object and will + * be written to the slave during the configuration process. + * + * The maximum size of the host name is 32 bytes (including the zero + * terminator). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_eoe_hostname( + ec_slave_config_t *sc, /**< Slave configuration. */ + const char *name /**< Zero-terminated host name. */ + ); + +/** Sets the application-layer state transition timeout in ms. + * + * Change the maximum allowed time for a slave to make an application-layer + * state transition for the given state transition (for example from PREOP to + * SAFEOP). The default values are defined in ETG.2000. + * + * A timeout value of zero ms will restore the default value. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + * + * \retval 0 Success. + * \retval <0 Error code. + */ +EC_PUBLIC_API int ecrt_slave_config_state_timeout( + ec_slave_config_t *sc, /**< Slave configuration. */ + ec_al_state_t from_state, /**< Initial state. */ + ec_al_state_t to_state, /**< Target state. */ + unsigned int timeout_ms /**< Timeout in [ms]. */ + ); + +/***************************************************************************** + * Domain methods + ****************************************************************************/ + +/** Registers a bunch of PDO entries for a domain. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \see ecrt_slave_config_reg_pdo_entry() + * + * \attention The registration array has to be terminated with an empty + * structure, or one with the \a index field set to zero! + * + * \apiusage{master_idle,blocking} + * + * \return 0 on success, else non-zero. + */ +EC_PUBLIC_API int ecrt_domain_reg_pdo_entry_list( + ec_domain_t *domain, /**< Domain. */ + const ec_pdo_entry_reg_t *pdo_entry_regs /**< Array of PDO + registrations. */ + ); + +/** Returns the current size of the domain's process data. + * + * The domain size is calculated after master activation. + * + * \apiusage{master_op,rt_safe} + * + * \return Size of the process data image, or a negative error code. + */ +EC_PUBLIC_API size_t ecrt_domain_size( + const ec_domain_t *domain /**< Domain. */ + ); + +#ifdef __KERNEL__ + +/** Provide external memory to store the domain's process data. + * + * Call this after all PDO entries have been registered and before activating + * the master. + * + * The size of the allocated memory must be at least ecrt_domain_size(), after + * all PDO entries have been registered. + * + * This method has to be called in non-realtime context before + * ecrt_master_activate(). + * + * \apiusage{master_idle,blocking} + */ +void ecrt_domain_external_memory( + ec_domain_t *domain, /**< Domain. */ + uint8_t *memory /**< Address of the memory to store the process + data in. */ + ); + +#endif /* __KERNEL__ */ + +/** Returns the domain's process data. + * + * - In kernel context: If external memory was provided with + * ecrt_domain_external_memory(), the returned pointer will contain the + * address of that memory. Otherwise it will point to the internally allocated + * memory. In the latter case, this method may not be called before + * ecrt_master_activate(). + * + * - In userspace context: This method has to be called after + * ecrt_master_activate() to get the mapped domain process data memory. + * + * \apiusage{master_op,rt_safe} + * + * \return Pointer to the process data memory. + */ +EC_PUBLIC_API uint8_t *ecrt_domain_data( + const ec_domain_t *domain /**< Domain. */ + ); + +/** Determines the states of the domain's datagrams. + * + * Evaluates the working counters of the received datagrams and outputs + * statistics, if necessary. This must be called after ecrt_master_receive() + * is expected to receive the domain datagrams in order to make + * ecrt_domain_state() return the result of the last process data exchange. + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_domain_process( + ec_domain_t *domain /**< Domain. */ + ); + +/** (Re-)queues all domain datagrams in the master's datagram queue. + * + * Call this function to mark the domain's datagrams for exchanging at the + * next call of ecrt_master_send(). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_domain_queue( + ec_domain_t *domain /**< Domain. */ + ); + +/** Reads the state of a domain. + * + * Stores the domain state in the given \a state structure. + * + * Using this method, the process data exchange can be monitored in realtime. + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_domain_state( + const ec_domain_t *domain, /**< Domain. */ + ec_domain_state_t *state /**< Pointer to a state object to store the + information. */ + ); + +/***************************************************************************** + * SDO request methods. + ****************************************************************************/ + +/** Set the SDO index and subindex. + * + * \attention If the SDO index and/or subindex is changed while + * ecrt_sdo_request_state() returns EC_REQUEST_BUSY, this may lead to + * unexpected results. + * + * This method is meant to be called in realtime context (after master + * activation). To initialize the SDO request, the index and subindex can be + * set via ecrt_slave_config_create_sdo_request(). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_sdo_request_index( + ec_sdo_request_t *req, /**< SDO request. */ + uint16_t index, /**< SDO index. */ + uint8_t subindex /**< SDO subindex. */ + ); + +/** Set the timeout for an SDO request. + * + * If the request cannot be processed in the specified time, if will be marked + * as failed. + * + * The timeout is permanently stored in the request object and is valid until + * the next call of this method. + * + * The timeout should be defined in non-realtime context, but can also be + * changed afterwards. + * + * \apiusage{master_any,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_sdo_request_timeout( + ec_sdo_request_t *req, /**< SDO request. */ + uint32_t timeout /**< Timeout in milliseconds. Zero means no + timeout. */ + ); + +/** Access to the SDO request's data. + * + * This function returns a pointer to the request's internal SDO data memory. + * + * - After a read operation was successful, integer data can be evaluated + * using the EC_READ_*() macros as usual. Example: + * \code + * uint16_t value = EC_READ_U16(ecrt_sdo_request_data(sdo))); + * \endcode + * - If a write operation shall be triggered, the data have to be written to + * the internal memory. Use the EC_WRITE_*() macros, if you are writing + * integer data. Be sure, that the data fit into the memory. The memory size + * is a parameter of ecrt_slave_config_create_sdo_request(). + * \code + * EC_WRITE_U16(ecrt_sdo_request_data(sdo), 0xFFFF); + * \endcode + * + * \attention The return value can be invalid during a read operation, because + * the internal SDO data memory could be re-allocated if the read SDO data do + * not fit inside. + * + * This method is meant to be called in realtime context (after master + * activation), but can also be used to initialize data before. + * + * \apiusage{master_any,rt_safe} + * + * \return Pointer to the internal SDO data memory. + * + */ +EC_PUBLIC_API uint8_t *ecrt_sdo_request_data( + const ec_sdo_request_t *req /**< SDO request. */ + ); + +/** Returns the current SDO data size. + * + * When the SDO request is created, the data size is set to the size of the + * reserved memory. After a read operation the size is set to the size of the + * read data. The size is not modified in any other situation. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_any,rt_safe} + * + * \return SDO data size in bytes. + * + */ +EC_PUBLIC_API size_t ecrt_sdo_request_data_size( + const ec_sdo_request_t *req /**< SDO request. */ + ); + +/** Get the current state of the SDO request. + * + * The user-space implementation fetches incoming data and stores the received + * data size in the request object, so the request is not const. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return Request state. + * + */ +EC_PUBLIC_API ec_request_state_t ecrt_sdo_request_state( +#ifdef __KERNEL__ + const +#endif + ec_sdo_request_t *req /**< SDO request. */ + ); + +/** Schedule an SDO write operation. + * + * \attention This method may not be called while ecrt_sdo_request_state() + * returns EC_REQUEST_BUSY. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + * \retval -EINVAL Invalid input data, e.g. data size == 0. + * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_sdo_request() + * too small. + */ +EC_PUBLIC_API int ecrt_sdo_request_write( + ec_sdo_request_t *req /**< SDO request. */ + ); + +/** Schedule an SDO read operation. + * + * \attention This method may not be called while ecrt_sdo_request_state() + * returns EC_REQUEST_BUSY. + * + * \attention After calling this function, the return value of + * ecrt_sdo_request_data() must be considered as invalid while + * ecrt_sdo_request_state() returns EC_REQUEST_BUSY. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_sdo_request_read( + ec_sdo_request_t *req /**< SDO request. */ + ); + +/***************************************************************************** + * SoE request methods. + ****************************************************************************/ + +/** Set the request's drive and Sercos ID numbers. + * + * \attention If the drive number and/or IDN is changed while + * ecrt_soe_request_state() returns EC_REQUEST_BUSY, this may lead to + * unexpected results. + * + * This method is meant to be called in realtime context (after master + * activation). To initialize the SoE request, the drive_no and IDN can be + * set via ecrt_slave_config_create_soe_request(). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_soe_request_idn( + ec_soe_request_t *req, /**< IDN request. */ + uint8_t drive_no, /**< SDO index. */ + uint16_t idn /**< SoE IDN. */ + ); + +/** Set the timeout for an SoE request. + * + * If the request cannot be processed in the specified time, if will be marked + * as failed. + * + * The timeout is permanently stored in the request object and is valid until + * the next call of this method. + * + * The timeout should be defined in non-realtime context, but can also be + * changed afterwards. + * + * \apiusage{master_any,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_soe_request_timeout( + ec_soe_request_t *req, /**< SoE request. */ + uint32_t timeout /**< Timeout in milliseconds. Zero means no + timeout. */ + ); + +/** Access to the SoE request's data. + * + * This function returns a pointer to the request's internal IDN data memory. + * + * - After a read operation was successful, integer data can be evaluated + * using the EC_READ_*() macros as usual. Example: + * \code + * uint16_t value = EC_READ_U16(ecrt_soe_request_data(idn_req))); + * \endcode + * - If a write operation shall be triggered, the data have to be written to + * the internal memory. Use the EC_WRITE_*() macros, if you are writing + * integer data. Be sure, that the data fit into the memory. The memory size + * is a parameter of ecrt_slave_config_create_soe_request(). + * \code + * EC_WRITE_U16(ecrt_soe_request_data(idn_req), 0xFFFF); + * \endcode + * + * \attention The return value can be invalidated during a read operation, + * because the internal IDN data memory could be re-allocated if the read IDN + * data do not fit inside. + * + * This method is meant to be called in realtime context (after master + * activation), but can also be used to initialize data before. + * + * \apiusage{master_any,rt_safe} + * + * \return Pointer to the internal IDN data memory. + * + */ +EC_PUBLIC_API uint8_t *ecrt_soe_request_data( + const ec_soe_request_t *req /**< SoE request. */ + ); + +/** Returns the current IDN data size. + * + * When the SoE request is created, the data size is set to the size of the + * reserved memory. After a read operation the size is set to the size of the + * read data. The size is not modified in any other situation. + * + * \apiusage{master_any,rt_safe} + * + * \return IDN data size in bytes. + */ +EC_PUBLIC_API size_t ecrt_soe_request_data_size( + const ec_soe_request_t *req /**< SoE request. */ + ); + +/** Get the current state of the SoE request. + * + * \return Request state. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * In the user-space implementation, the method fetches the size of the + * incoming data, so the request object is not const. + * + * \apiusage{master_op,rt_safe} + */ +EC_PUBLIC_API ec_request_state_t ecrt_soe_request_state( +#ifdef __KERNEL__ + const +#endif + ec_soe_request_t *req /**< SoE request. */ + ); + +/** Schedule an SoE IDN write operation. + * + * \attention This method may not be called while ecrt_soe_request_state() + * returns EC_REQUEST_BUSY. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + * \retval -EINVAL Invalid input data, e.g. data size == 0. + * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_soe_request() + * too small. + */ +EC_PUBLIC_API int ecrt_soe_request_write( + ec_soe_request_t *req /**< SoE request. */ + ); + +/** Schedule an SoE IDN read operation. + * + * \attention This method may not be called while ecrt_soe_request_state() + * returns EC_REQUEST_BUSY. + * + * \attention After calling this function, the return value of + * ecrt_soe_request_data() must be considered as invalid while + * ecrt_soe_request_state() returns EC_REQUEST_BUSY. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_soe_request_read( + ec_soe_request_t *req /**< SoE request. */ + ); + +/***************************************************************************** + * VoE handler methods. + ****************************************************************************/ + +/** Sets the VoE header for future send operations. + * + * A VoE message shall contain a 4-byte vendor ID, followed by a 2-byte vendor + * type at as header. These numbers can be set with this function. The values + * are valid and will be used for future send operations until the next call + * of this method. + * + * This method is meant to be called in non-realtime context (before master + * activation) to initialize the header data, but it is also safe to + * change the header later on in realtime context. + * + * \apiusage{master_any,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_voe_handler_send_header( + ec_voe_handler_t *voe, /**< VoE handler. */ + uint32_t vendor_id, /**< Vendor ID. */ + uint16_t vendor_type /**< Vendor-specific type. */ + ); + +/** Reads the header data of a received VoE message. + * + * This method can be used to get the received VoE header information after a + * read operation has succeeded. + * + * The header information is stored at the memory given by the pointer + * parameters. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_voe_handler_received_header( + const ec_voe_handler_t *voe, /**< VoE handler. */ + uint32_t *vendor_id, /**< Vendor ID. */ + uint16_t *vendor_type /**< Vendor-specific type. */ + ); + +/** Access to the VoE handler's data. + * + * This function returns a pointer to the VoE handler's internal memory, that + * points to the actual VoE data right after the VoE header (see + * ecrt_voe_handler_send_header()). + * + * - After a read operation was successful, the memory contains the received + * data. The size of the received data can be determined via + * ecrt_voe_handler_data_size(). + * - Before a write operation is triggered, the data have to be written to the + * internal memory. Be sure, that the data fit into the memory. The reserved + * memory size is a parameter of ecrt_slave_config_create_voe_handler(). + * + * \attention The returned pointer is not necessarily persistent: After a read + * operation, the internal memory may have been reallocated. This can be + * avoided by reserving enough memory via the \a size parameter of + * ecrt_slave_config_create_voe_handler(). + * + * \apiusage{master_any,rt_safe} + * + * \return Pointer to the internal memory. + */ +EC_PUBLIC_API uint8_t *ecrt_voe_handler_data( + const ec_voe_handler_t *voe /**< VoE handler. */ + ); + +/** Returns the current data size. + * + * The data size is the size of the VoE data without the header (see + * ecrt_voe_handler_send_header()). + * + * When the VoE handler is created, the data size is set to the size of the + * reserved memory. At a write operation, the data size is set to the number + * of bytes to write. After a read operation the size is set to the size of + * the read data. The size is not modified in any other situation. + * + * \apiusage{master_any,rt_safe} + * + * \return Data size in bytes. + */ +EC_PUBLIC_API size_t ecrt_voe_handler_data_size( + const ec_voe_handler_t *voe /**< VoE handler. */ + ); + +/** Start a VoE write operation. + * + * After this function has been called, the ecrt_voe_handler_execute() method + * must be called in every realtime cycle as long as it returns + * EC_REQUEST_BUSY. No other operation may be started while the handler is + * busy. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_voe_handler + * too small. + */ +EC_PUBLIC_API int ecrt_voe_handler_write( + ec_voe_handler_t *voe, /**< VoE handler. */ + size_t size /**< Number of bytes to write (without the VoE header). */ + ); + +/** Start a VoE read operation. + * + * After this function has been called, the ecrt_voe_handler_execute() method + * must be called in every realtime cycle as long as it returns + * EC_REQUEST_BUSY. No other operation may be started while the handler is + * busy. + * + * The state machine queries the slave's send mailbox for new data to be send + * to the master. If no data appear within the EC_VOE_RESPONSE_TIMEOUT + * (defined in master/voe_handler.c), the operation fails. + * + * On success, the size of the read data can be determined via + * ecrt_voe_handler_data_size(), while the VoE header of the received data + * can be retrieved with ecrt_voe_handler_received_header(). + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_voe_handler_read( + ec_voe_handler_t *voe /**< VoE handler. */ + ); + +/** Start a VoE read operation without querying the sync manager status. + * + * After this function has been called, the ecrt_voe_handler_execute() method + * must be called in every realtime cycle as long as it returns + * EC_REQUEST_BUSY. No other operation may be started while the handler is + * busy. + * + * The state machine queries the slave by sending an empty mailbox. The slave + * fills its data to the master in this mailbox. If no data appear within the + * EC_VOE_RESPONSE_TIMEOUT (defined in master/voe_handler.c), the operation + * fails. + * + * On success, the size of the read data can be determined via + * ecrt_voe_handler_data_size(), while the VoE header of the received data + * can be retrieved with ecrt_voe_handler_received_header(). + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + */ +EC_PUBLIC_API int ecrt_voe_handler_read_nosync( + ec_voe_handler_t *voe /**< VoE handler. */ + ); + +/** Execute the handler. + * + * This method executes the VoE handler. It has to be called in every realtime + * cycle as long as it returns EC_REQUEST_BUSY. + * + * \return Handler state. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + */ +EC_PUBLIC_API ec_request_state_t ecrt_voe_handler_execute( + ec_voe_handler_t *voe /**< VoE handler. */ + ); + +/***************************************************************************** + * Register request methods. + ****************************************************************************/ + +/** Access to the register request's data. + * + * This function returns a pointer to the request's internal memory. + * + * - After a read operation was successful, integer data can be evaluated + * using the EC_READ_*() macros as usual. Example: + * \code + * uint16_t value = EC_READ_U16(ecrt_reg_request_data(reg_request))); + * \endcode + * - If a write operation shall be triggered, the data have to be written to + * the internal memory. Use the EC_WRITE_*() macros, if you are writing + * integer data. Be sure, that the data fit into the memory. The memory size + * is a parameter of ecrt_slave_config_create_reg_request(). + * \code + * EC_WRITE_U16(ecrt_reg_request_data(reg_request), 0xFFFF); + * \endcode + * + * This method is meant to be called in realtime context (after master + * activation), but can also be used to initialize data before. + * + * \apiusage{master_any,rt_safe} + * + * \return Pointer to the internal memory. + * + */ +EC_PUBLIC_API uint8_t *ecrt_reg_request_data( + const ec_reg_request_t *req /**< Register request. */ + ); + +/** Get the current state of the register request. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return Request state. + * + */ +EC_PUBLIC_API ec_request_state_t ecrt_reg_request_state( + const ec_reg_request_t *req /**< Register request. */ + ); + +/** Schedule an register write operation. + * + * \attention This method may not be called while ecrt_reg_request_state() + * returns EC_REQUEST_BUSY. + * + * \attention The \a size parameter is truncated to the size given at request + * creation. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_reg_request + * too small. + */ +EC_PUBLIC_API int ecrt_reg_request_write( + ec_reg_request_t *req, /**< Register request. */ + uint16_t address, /**< Register address. */ + size_t size /**< Size to write. */ + ); + +/** Schedule a register read operation. + * + * \attention This method may not be called while ecrt_reg_request_state() + * returns EC_REQUEST_BUSY. + * + * \attention The \a size parameter is truncated to the size given at request + * creation. + * + * This method is meant to be called in realtime context (after master + * activation). + * + * \apiusage{master_op,rt_safe} + * + * \return 0 on success, otherwise negative error code. + * \retval -ENOBUFS Reserved memory in ecrt_slave_config_create_reg_request + * too small. + */ +EC_PUBLIC_API int ecrt_reg_request_read( + ec_reg_request_t *req, /**< Register request. */ + uint16_t address, /**< Register address. */ + size_t size /**< Size to write. */ + ); + +/***************************************************************************** + * Bitwise read/write macros + ****************************************************************************/ + +/** Read a certain bit of an EtherCAT data byte. + * + * \param DATA EtherCAT data pointer + * \param POS bit position + */ +#define EC_READ_BIT(DATA, POS) ((*((uint8_t *) (DATA)) >> (POS)) & 0x01) + +/** Write a certain bit of an EtherCAT data byte. + * + * \param DATA EtherCAT data pointer + * \param POS bit position + * \param VAL new bit value + */ +#define EC_WRITE_BIT(DATA, POS, VAL) \ + do { \ + if (VAL) *((uint8_t *) (DATA)) |= (1 << (POS)); \ + else *((uint8_t *) (DATA)) &= ~(1 << (POS)); \ + } while (0) + +/***************************************************************************** + * Byte-swapping functions for user space + ****************************************************************************/ + +#ifndef __KERNEL__ + +#if __BYTE_ORDER == __LITTLE_ENDIAN + +#define le16_to_cpu(x) x +#define le32_to_cpu(x) x +#define le64_to_cpu(x) x + +#define cpu_to_le16(x) x +#define cpu_to_le32(x) x +#define cpu_to_le64(x) x + +#elif __BYTE_ORDER == __BIG_ENDIAN + +#define swap16(x) \ + ((uint16_t)( \ + (((uint16_t)(x) & 0x00ffU) << 8) | \ + (((uint16_t)(x) & 0xff00U) >> 8) )) +#define swap32(x) \ + ((uint32_t)( \ + (((uint32_t)(x) & 0x000000ffUL) << 24) | \ + (((uint32_t)(x) & 0x0000ff00UL) << 8) | \ + (((uint32_t)(x) & 0x00ff0000UL) >> 8) | \ + (((uint32_t)(x) & 0xff000000UL) >> 24) )) +#define swap64(x) \ + ((uint64_t)( \ + (((uint64_t)(x) & 0x00000000000000ffULL) << 56) | \ + (((uint64_t)(x) & 0x000000000000ff00ULL) << 40) | \ + (((uint64_t)(x) & 0x0000000000ff0000ULL) << 24) | \ + (((uint64_t)(x) & 0x00000000ff000000ULL) << 8) | \ + (((uint64_t)(x) & 0x000000ff00000000ULL) >> 8) | \ + (((uint64_t)(x) & 0x0000ff0000000000ULL) >> 24) | \ + (((uint64_t)(x) & 0x00ff000000000000ULL) >> 40) | \ + (((uint64_t)(x) & 0xff00000000000000ULL) >> 56) )) + +#define le16_to_cpu(x) swap16(x) +#define le32_to_cpu(x) swap32(x) +#define le64_to_cpu(x) swap64(x) + +#define cpu_to_le16(x) swap16(x) +#define cpu_to_le32(x) swap32(x) +#define cpu_to_le64(x) swap64(x) + +#endif + +#define le16_to_cpup(x) le16_to_cpu(*((uint16_t *)(x))) +#define le32_to_cpup(x) le32_to_cpu(*((uint32_t *)(x))) +#define le64_to_cpup(x) le64_to_cpu(*((uint64_t *)(x))) + +#endif /* ifndef __KERNEL__ */ + +/***************************************************************************** + * Read macros + ****************************************************************************/ + +/** Read an 8-bit unsigned value from EtherCAT data. + * + * \return EtherCAT data value + */ +#define EC_READ_U8(DATA) \ + ((uint8_t) *((uint8_t *) (DATA))) + +/** Read an 8-bit signed value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_S8(DATA) \ + ((int8_t) *((uint8_t *) (DATA))) + +/** Read a 16-bit unsigned value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_U16(DATA) \ + ((uint16_t) le16_to_cpup((void *) (DATA))) + +/** Read a 16-bit signed value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_S16(DATA) \ + ((int16_t) le16_to_cpup((void *) (DATA))) + +/** Read a 32-bit unsigned value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_U32(DATA) \ + ((uint32_t) le32_to_cpup((void *) (DATA))) + +/** Read a 32-bit signed value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_S32(DATA) \ + ((int32_t) le32_to_cpup((void *) (DATA))) + +/** Read a 64-bit unsigned value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_U64(DATA) \ + ((uint64_t) le64_to_cpup((void *) (DATA))) + +/** Read a 64-bit signed value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_S64(DATA) \ + ((int64_t) le64_to_cpup((void *) (DATA))) + +/***************************************************************************** + * Floating-point read functions and macros (userspace only) + ****************************************************************************/ + +#ifndef __KERNEL__ + +/** Read a 32-bit floating-point value from EtherCAT data. + * + * \apiusage{master_any,rt_safe} + * + * \param data EtherCAT data pointer + * \return EtherCAT data value + */ +EC_PUBLIC_API float ecrt_read_real(const void *data); + +/** Read a 32-bit floating-point value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_REAL(DATA) ecrt_read_real(DATA) + +/** Read a 64-bit floating-point value from EtherCAT data. + * + * \apiusage{master_any,rt_safe} + * + * \param data EtherCAT data pointer + * \return EtherCAT data value + */ +EC_PUBLIC_API double ecrt_read_lreal(const void *data); + +/** Read a 64-bit floating-point value from EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \return EtherCAT data value + */ +#define EC_READ_LREAL(DATA) ecrt_read_lreal(DATA) + +#endif // ifndef __KERNEL__ + +/***************************************************************************** + * Write macros + ****************************************************************************/ + +/** Write an 8-bit unsigned value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_U8(DATA, VAL) \ + do { \ + *((uint8_t *)(DATA)) = ((uint8_t) (VAL)); \ + } while (0) + +/** Write an 8-bit signed value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_S8(DATA, VAL) EC_WRITE_U8(DATA, VAL) + +/** Write a 16-bit unsigned value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_U16(DATA, VAL) \ + do { \ + *((uint16_t *) (DATA)) = cpu_to_le16((uint16_t) (VAL)); \ + } while (0) + +/** Write a 16-bit signed value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_S16(DATA, VAL) EC_WRITE_U16(DATA, VAL) + +/** Write a 32-bit unsigned value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_U32(DATA, VAL) \ + do { \ + *((uint32_t *) (DATA)) = cpu_to_le32((uint32_t) (VAL)); \ + } while (0) + +/** Write a 32-bit signed value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_S32(DATA, VAL) EC_WRITE_U32(DATA, VAL) + +/** Write a 64-bit unsigned value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_U64(DATA, VAL) \ + do { \ + *((uint64_t *) (DATA)) = cpu_to_le64((uint64_t) (VAL)); \ + } while (0) + +/** Write a 64-bit signed value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_S64(DATA, VAL) EC_WRITE_U64(DATA, VAL) + +/***************************************************************************** + * Floating-point write functions and macros (userspace only) + ****************************************************************************/ + +#ifndef __KERNEL__ + +/** Write a 32-bit floating-point value to EtherCAT data. + * + * \apiusage{master_any,rt_safe} + * + * \param data EtherCAT data pointer + * \param value new value + */ +EC_PUBLIC_API void ecrt_write_real(void *data, float value); + +/** Write a 32-bit floating-point value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_REAL(DATA, VAL) ecrt_write_real(DATA, VAL) + +/** Write a 64-bit floating-point value to EtherCAT data. + * + * \apiusage{master_any,rt_safe} + * + * \param data EtherCAT data pointer + * \param value new value + */ +EC_PUBLIC_API void ecrt_write_lreal(void *data, double value); + +/** Write a 64-bit floating-point value to EtherCAT data. + * + * \param DATA EtherCAT data pointer + * \param VAL new value + */ +#define EC_WRITE_LREAL(DATA, VAL) ecrt_write_lreal(DATA, VAL) + +#endif // ifndef __KERNEL__ + +/****************************************************************************/ + +#ifdef __cplusplus +} +#endif + +/****************************************************************************/ + +/** @} */ + +#endif diff --git a/dependency/x86/third_party/ethercat/v1.7.0/include/ectty.h b/dependency/x86/third_party/ethercat/v1.7.0/include/ectty.h new file mode 100644 index 00000000..3178d3ce --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/include/ectty.h @@ -0,0 +1,106 @@ +/***************************************************************************** + * + * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH + * + * This file is part of the IgH EtherCAT master userspace library. + * + * The IgH EtherCAT master userspace library is free software; you can + * redistribute it and/or modify it under the terms of the GNU Lesser General + * Public License as published by the Free Software Foundation; version 2.1 + * of the License. + * + * The IgH EtherCAT master userspace library is distributed in the hope that + * it will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with the IgH EtherCAT master userspace library. If not, see + * . + * + ****************************************************************************/ + +/** \file + * + * EtherCAT virtual TTY interface. + * + * \defgroup TTYInterface EtherCAT Virtual TTY Interface + * + * @{ + */ + +/****************************************************************************/ + +#ifndef __ECTTY_H__ +#define __ECTTY_H__ + +#include + +/***************************************************************************** + * Data types + ****************************************************************************/ + +struct ec_tty; +typedef struct ec_tty ec_tty_t; /**< \see ec_tty */ + +/** Operations on the virtual TTY interface. + */ +typedef struct { + int (*cflag_changed)(void *, tcflag_t); /**< Called when the serial + * settings shall be changed. The + * \a cflag argument contains the + * new settings. */ +} ec_tty_operations_t; + +/***************************************************************************** + * Global functions + ****************************************************************************/ + +/** Create a virtual TTY interface. + * + * \param ops Set of callbacks. + * \param cb_data Arbitrary data, that is passed to any callback. + * + * \return Pointer to the interface object, otherwise an ERR_PTR value. + */ +ec_tty_t *ectty_create( + const ec_tty_operations_t *ops, + void *cb_data + ); + +/***************************************************************************** + * TTY interface methods + ****************************************************************************/ + +/** Releases a virtual TTY interface. + */ +void ectty_free( + ec_tty_t *tty /**< TTY interface. */ + ); + +/** Reads data to send from the TTY interface. + * + * If there are data to send, they are copied into the \a buffer. At maximum, + * \a size bytes are copied. The actual number of bytes copied is returned. + * + * \return Number of bytes copied. + */ +unsigned int ectty_tx_data( + ec_tty_t *tty, /**< TTY interface. */ + uint8_t *buffer, /**< Buffer for data to transmit. */ + size_t size /**< Available space in \a buffer. */ + ); + +/** Pushes received data to the TTY interface. + */ +void ectty_rx_data( + ec_tty_t *tty, /**< TTY interface. */ + const uint8_t *buffer, /**< Buffer with received data. */ + size_t size /**< Number of bytes in \a buffer. */ + ); + +/****************************************************************************/ + +/** @} */ + +#endif diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/cmake/ethercat/ethercat-config.cmake b/dependency/x86/third_party/ethercat/v1.7.0/lib/cmake/ethercat/ethercat-config.cmake new file mode 100644 index 00000000..49f8e81c --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/lib/cmake/ethercat/ethercat-config.cmake @@ -0,0 +1,43 @@ +#---------------------------------------------------------------------------- +# +# Copyright (C) 2021 Bjarne von Horn, Ingenieurgemeinschaft IgH +# +# This file is part of the IgH EtherCAT Master. +# +# The IgH EtherCAT Master is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License version 2, as +# published by the Free Software Foundation. +# +# The IgH EtherCAT Master is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General +# Public License for more details. +# +# You should have received a copy of the GNU General Public License along +# with the IgH EtherCAT Master; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +# +# vim: tw=78 +# +#---------------------------------------------------------------------------- + + +find_library(EtherCAT_LIBRARY + NAMES ethercat + PATHS /home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/lib +) + +find_path(EtherCAT_INCLUDE_DIR + NAMES ecrt.h + PATHS /home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/include +) + +mark_as_advanced(EtherCAT_LIBRARY EtherCAT_INCLUDE_DIR) + +if(NOT TARGET EtherLab::ethercat) + add_library(EtherLab::ethercat SHARED IMPORTED) + set_target_properties(EtherLab::ethercat PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${EtherCAT_INCLUDE_DIR}" + IMPORTED_LOCATION "${EtherCAT_LIBRARY}" + ) +endif() diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.a b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.a new file mode 100644 index 00000000..5a47c1d7 Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.a differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.la b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.la new file mode 100755 index 00000000..056b3b88 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.la @@ -0,0 +1,41 @@ +# libethercat.la - a libtool library file +# Generated by libtool (GNU libtool) 2.4.6 Debian-2.4.6-15build2 +# +# Please DO NOT delete this file! +# It is necessary for linking the library. + +# The name that we can dlopen(3). +dlname='libethercat.so.1' + +# Names of this library. +library_names='libethercat.so.1.2.0 libethercat.so.1 libethercat.so' + +# The name of the static archive. +old_library='libethercat.a' + +# Linker flags that cannot go in dependency_libs. +inherited_linker_flags='' + +# Libraries that this one depends upon. +dependency_libs='' + +# Names of additional weak libraries provided by this library +weak_library_names='' + +# Version information for libethercat. +current=3 +age=2 +revision=0 + +# Is this an already installed library? +installed=yes + +# Should we warn about portability when linking against -modules? +shouldnotlink=no + +# Files to dlopen/dlpreopen +dlopen='' +dlpreopen='' + +# Directory that this library needs to be installed in: +libdir='/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/lib' diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so new file mode 120000 index 00000000..d85df380 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so @@ -0,0 +1 @@ +libethercat.so.1.2.0 \ No newline at end of file diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1 b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1 new file mode 120000 index 00000000..d85df380 --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1 @@ -0,0 +1 @@ +libethercat.so.1.2.0 \ No newline at end of file diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1.2.0 b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1.2.0 new file mode 100755 index 00000000..5cdc92f2 Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/libethercat.so.1.2.0 differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/devices/ec_generic.ko b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/devices/ec_generic.ko new file mode 100644 index 00000000..8af80416 Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/devices/ec_generic.ko differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/examples/mini/ec_mini.ko b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/examples/mini/ec_mini.ko new file mode 100644 index 00000000..0c4e304e Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/examples/mini/ec_mini.ko differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/master/ec_master.ko b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/master/ec_master.ko new file mode 100644 index 00000000..a5bc2c41 Binary files /dev/null and b/dependency/x86/third_party/ethercat/v1.7.0/lib/modules/6.8.0-124-generic/ethercat/master/ec_master.ko differ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/pkgconfig/libethercat.pc b/dependency/x86/third_party/ethercat/v1.7.0/lib/pkgconfig/libethercat.pc new file mode 100644 index 00000000..de20cf9f --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/lib/pkgconfig/libethercat.pc @@ -0,0 +1,34 @@ +# +# pkgconfig file for ethercat library +# +# Copyright 2021 Bjarne von Horn (vh at igh dot de) +# +# This file is part of the ethercat library. +# +# The ethercat library is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or (at your +# option) any later version. +# +# The ethercat library is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY +# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public +# License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with the ethercat library. If not, see . +# +# vim: tw=78 noexpandtab +# + +prefix=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0 +exec_prefix=${prefix} +libdir=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/lib +includedir=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/include + +Name: libethercat +Description: Client support library for the EtherCAT Master +URL: http://www.etherlab.org +Version: 1.7.0 +Libs: -L${libdir} -lethercat +Cflags: -I${includedir} diff --git a/dependency/x86/third_party/ethercat/v1.7.0/lib/systemd/system/ethercat.service b/dependency/x86/third_party/ethercat/v1.7.0/lib/systemd/system/ethercat.service new file mode 100644 index 00000000..116b36cf --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/lib/systemd/system/ethercat.service @@ -0,0 +1,38 @@ +# +# EtherCAT master kernel modules +# + +[Unit] +Description=EtherCAT Master Kernel Modules + +# Fine tuning of the startup dependencies below are recommended +# to provide a reliable startup routine. +# The dependencies below can be either uncommented after copying +# this file to /etc/systemd/system or by creating overrides: +# Copy the needed dependencies into +# /etc/systemd/system/ethercat.service.d/50-dependencies.conf +# in a [Unit] section. + +# +# Uncomment this, if the generic Ethernet driver is used. It assures, that the +# network interfaces are configured, before the master starts. +# +#Requires=network.target # Stop master, if network is stopped +#After=network.target # Start master, after network is ready + +# +# Uncomment this, if a native Ethernet driver is used. It assures, that the +# network interfaces are configured, after the Ethernet drivers have been +# replaced. Otherwise, the networking configuration tools could be confused. +# +#Before=network-pre.target +#Wants=network-pre.target + +[Service] +Type=oneshot +RemainAfterExit=yes +ExecStart=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl start +ExecStop=/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl stop + +[Install] +WantedBy=multi-user.target diff --git a/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl b/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl new file mode 100755 index 00000000..b455d52b --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/sbin/ethercatctl @@ -0,0 +1,253 @@ +#!/bin/bash + +#------------------------------------------------------------------------------ +# +# Start script for EtherCAT to use with systemd +# +# Copyright (C) 2006-2021 Florian Pose, Ingenieurgemeinschaft IgH +# +# This file is part of the IgH EtherCAT Master. +# +# The IgH EtherCAT Master is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License version 2, as +# published by the Free Software Foundation. +# +# The IgH EtherCAT Master is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General +# Public License for more details. +# +# You should have received a copy of the GNU General Public License along +# with the IgH EtherCAT Master; if not, write to the Free Software +# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +# +# vim: expandtab sw=4 tw=78 +# +#------------------------------------------------------------------------------ + +LSMOD="/sbin/lsmod" +MODPROBE="/sbin/modprobe" +RMMOD="/sbin/rmmod" +MODINFO="/sbin/modinfo" +IP="/sbin/ip" + +ETHERCAT="/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/bin/ethercat" + +#------------------------------------------------------------------------------ + +if [ "$1" = "-c" ]; then + ETHERCAT_CONFIG="$2" + COMMAND="$3" +else + ETHERCAT_CONFIG="/home/lgv/cmvr/0-workspace/cmvr-es/dependency/x86/third_party/ethercat/v1.7.0/etc/ethercat.conf" + COMMAND="$1" +fi + +#------------------------------------------------------------------------------ + +if [ ! -r ${ETHERCAT_CONFIG} ]; then + echo ${ETHERCAT_CONFIG} not existing; + exit 6 +fi + +# shellcheck source=/etc/ethercat.conf +. ${ETHERCAT_CONFIG} + +#------------------------------------------------------------------------------ + +is_mac_address() { + local x='[0-9a-fA-F]' + echo "$1" | grep -qE "^($x$x:){5}$x$x\$" - +} + +#------------------------------------------------------------------------------ + +parse_mac_address() { + local DEVICENAMETOMAC + if [ -z "${1}" ] || is_mac_address "${1}"; then + MAC="${1}" + else + DEVICENAMETOMAC=$("${IP}" address show dev "${1}" | + awk '/link\/ether/ { print $2; }') + if is_mac_address "${DEVICENAMETOMAC}"; then + MAC="${DEVICENAMETOMAC}" + else + echo Invalid MAC address or interface name \""${1}"\" \ + in ${ETHERCAT_CONFIG} + exit 1 + fi + fi +} + +#------------------------------------------------------------------------------ + +case "$COMMAND" in + +start) + # bring up all updown interfaces before anything else + for interface in $UPDOWN_INTERFACES; do + $IP link set dev $interface up + done + + # construct DEVICES and BACKUPS from configuration variables + DEVICES="" + BACKUPS="" + MASTER_INDEX=0 + + while true; do + DEVICE=$(eval echo "\${MASTER${MASTER_INDEX}_DEVICE}") + BACKUP=$(eval echo "\${MASTER${MASTER_INDEX}_BACKUP}") + if [ -z "${DEVICE}" ]; then break; fi + + if [ ${MASTER_INDEX} -gt 0 ]; then + DEVICES=${DEVICES}, + BACKUPS=${BACKUPS}, + fi + + parse_mac_address "${DEVICE}" + DEVICES=${DEVICES}${MAC} + + parse_mac_address "${BACKUP}" + BACKUPS=${BACKUPS}${MAC} + + MASTER_INDEX=$((${MASTER_INDEX} + 1)) + done + + if [ -z "${DEVICES}" ]; then + echo "ERROR: No network cards for EtherCAT specified." + echo -n "Please edit ${ETHERCAT_CONFIG} with root permissions" + echo -n " and set MASTER0_DEVICE variable to either a " + echo "network interface name (like eth0) or to a MAC address." + exit 1 + fi + + MODULE_PARAMS=( + main_devices="${DEVICES}" + backup_devices="${BACKUPS}" + ) + + if [ -n "$SII_CACHING" ]; then + MODULE_PARAMS+=(sii_caching="$SII_CACHING") + fi + + # load master module + if ! ${MODPROBE} ${MODPROBE_FLAGS} ec_master "${MODULE_PARAMS[@]}"; then + exit 1 + fi + + LOADED_MODULES=ec_master + + # check for modules to replace + for MODULE in ${DEVICE_MODULES}; do + ECMODULE=ec_${MODULE} + if ! ${MODINFO} "${ECMODULE}" > /dev/null; then + continue # ec_* module not found + fi + + if [ "${MODULE}" != "generic" ] && [ "${MODULE}" != "ccat" ]; then + # unload standard module and check if unloading was successful + ${RMMOD} "${MODULE}" 2> /dev/null || true + if ${LSMOD} | grep "^${MODULE//-/_} " > /dev/null; then + # could not unload module + ${RMMOD} ${LOADED_MODULES} + exit 1 + fi + fi + + if ! ${MODPROBE} ${MODPROBE_FLAGS} "${ECMODULE}"; then + if [ "${MODULE}" != "generic" ] && [ "${MODULE}" != "ccat" ]; then + ${MODPROBE} ${MODPROBE_FLAGS} "${MODULE}" # try to restore + fi + ${RMMOD} ${LOADED_MODULES} + exit 1 + fi + + LOADED_MODULES="${ECMODULE} ${LOADED_MODULES}" + done + + exit 0 + ;; + +#------------------------------------------------------------------------------ + +stop) + # unload EtherCAT device modules + for MODULE in ${DEVICE_MODULES} master; do + ECMODULE=ec_${MODULE} + if ! ${LSMOD} | grep -q "^${ECMODULE//-/_} "; then + continue # ec_* module not loaded + fi + if ! ${RMMOD} "${ECMODULE}"; then + exit 1 + fi; + done + + sleep 1 + + # load standard modules again + for MODULE in ${DEVICE_MODULES}; do + if [ "${MODULE}" == "generic" ] || [ "${MODULE}" == "ccat" ]; then + continue + fi + ${MODPROBE} ${MODPROBE_FLAGS} "${MODULE}" + done + + # bring down all updown interfaces + for interface in $UPDOWN_INTERFACES; do + $IP link set dev $interface down + done + + exit 0 + ;; + +#------------------------------------------------------------------------------ + +restart) + $0 stop || exit 1 + sleep 1 + $0 start + ;; + +#------------------------------------------------------------------------------ + +status) + echo "Checking for EtherCAT master 1.7.0 " + + # count masters in configuration file + MASTER_COUNT=0 + while true; do + DEVICE=$(eval echo "\${MASTER${MASTER_COUNT}_DEVICE}") + if [ -z "${DEVICE}" ]; then break; fi + MASTER_COUNT=$((${MASTER_COUNT} + 1)) + done + + RESULT=0 + + for i in $(seq 0 "$((${MASTER_COUNT} - 1))"); do + echo -n "Master${i} " + + # Check if the master is in idle or operation phase + ${ETHERCAT} master --master "${i}" 2>/dev/null | \ + grep -qE 'Phase:[[:space:]]*Idle|Phase:[[:space:]]*Operation' + EXITCODE=$? + + if [ ${EXITCODE} -eq 0 ]; then + echo " running" + else + echo " dead" + RESULT=1 + fi + done + + exit ${RESULT} + ;; + +#------------------------------------------------------------------------------ + +*) + echo "USAGE: $0 [-c path/to/ethercat.conf] {start|stop|restart|status}" + exit 1 + ;; +esac + +#------------------------------------------------------------------------------ diff --git a/dependency/x86/third_party/ethercat/v1.7.0/share/bash-completion/completions/ethercat b/dependency/x86/third_party/ethercat/v1.7.0/share/bash-completion/completions/ethercat new file mode 100644 index 00000000..3734b55b --- /dev/null +++ b/dependency/x86/third_party/ethercat/v1.7.0/share/bash-completion/completions/ethercat @@ -0,0 +1,80 @@ +# Copyright (C) 2022 Bjarne von Horn, Ingenieurgemeinschaft IgH +# +# This file is part of the IgH EtherCAT master userspace library. +# +# The IgH EtherCAT master userspace library is free software; you can +# redistribute it and/or modify it under the terms of the GNU Lesser General +# Public License as published by the Free Software Foundation; version 2.1 +# of the License. +# +# The IgH EtherCAT master userspace library is distributed in the hope that +# it will be useful, but WITHOUT ANY WARRANTY; without even the implied +# warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with the IgH EtherCAT master userspace library. If not, see +# . +# + +_ethercat_completions() +{ + local ethercat_commands="alias config crc cstruct data debug domains download eoe foe_read foe_write graph master pdos reg_read reg_write rescan sdos sii_read sii_write slaves soe_read soe_write states upload version xml" + local options="--help --force --quiet --verbose --master " + if [ "$COMP_CWORD" -eq 1 ] ; then + COMPREPLY=($(compgen -W "$ethercat_commands --help" -- "${COMP_WORDS[1]}")) + elif [[ "${COMP_WORDS[1]}" != "--help" && ! "${COMP_WORDS[COMP_CWORD-1]}" =~ ^-a|-p|--alias|--position$ ]] ; then + case "${COMP_WORDS[1]}" in + "alias" | "config" | "cstruct" | "slaves" | "sdos" | "sii_read" | "upload" | "xml") + options+="--alias --position" + ;; + "crc") + options+="reset" + ;; + "debug") + options+="0 1 2" + ;; + "domains") + options+="--domain" + ;; + "download" | "reg_read" | "soe_read" | "soe_write") + if [[ "${COMP_WORDS[COMP_CWORD-1]}" =~ ^-t|--type$ ]] ; then + options="bool int8 int16 int32 int64 uint8 uint16 uint32 uint64 float double string octet_string unicode_string sm8 sm16 sm32 sm64" + else + options+="--alias --position --type" + fi + ;; + "foe_read" | "foe_write") + if [[ "${COMP_WORDS[COMP_CWORD-1]}" =~ ^-o|--output-file$ ]] ; then + COMPREPLY=($(compgen -o filenames -A file -- "${COMP_WORDS[$COMP_CWORD]}")) + else + options+="--alias --position --output-file" + COMPREPLY=($(compgen -o filenames -A file -W "$options" -- "${COMP_WORDS[$COMP_CWORD]}")) + fi + return + ;; + "graph") + options+="DC CRC" + ;; + "pdos") + if [[ "${COMP_WORDS[COMP_CWORD-1]}" =~ ^-s|--skin$ ]] ; then + options="default etherlab" + else + options+="--alias --position --skin" + fi + ;; + "sii_write") + options+="--alias --position" + COMPREPLY=($(compgen -o filenames -A file -W "$options" -- "${COMP_WORDS[$COMP_CWORD]}")) + return + ;; + "states") + options+="--alias --position INIT PREOP BOOT SAFEOP OP" + ;; + + esac + COMPREPLY+=($(compgen -W "$options" -- "${COMP_WORDS[$COMP_CWORD]}")) + fi +} + +complete -F _ethercat_completions ethercat diff --git a/docs/EYOU_ServoModule_ECAT_V145.xml b/docs/EYOU_ServoModule_ECAT_V145.xml new file mode 100644 index 00000000..92f89763 --- /dev/null +++ b/docs/EYOU_ServoModule_ECAT_V145.xml @@ -0,0 +1,5560 @@ + + + + #x1097 + Jiangsu Yiyou Robot Technology Co., Ltd. + 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 + + + + + ServoDrive + Servo Drives + 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 + + + + + EYOU_ServoModule_V145 + EYOU_ServoModule_ECAT_V145 + + + + 2000 + 9000 + 5000 + 200 + + + + + 100 + 2000 + + + + ServoDrive + + + 402 + + + + + + BIT2 + 2 + + + + BOOL + 1 + + + + DINT + 32 + + + + INT + 16 + + + + SINT + 8 + + + + UDINT + 32 + + + + UINT + 16 + + + + USINT + 8 + + + + REAL + 32 + + + + ARRAY [0..3] OF BYTE + USINT + 32 + + 0 + 4 + + + + + STRING(12) + 96 + + + + STRING(10) + 80 + + + + DT1010 + 112 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Store All Parameters + UDINT + 32 + 16 + + rw + o + + + + 2 + Store Communication Parameters + UDINT + 32 + 48 + + rw + o + + + + 3 + Store Application Parametesr + UDINT + 32 + 80 + + rw + o + + + + + + DT1011 + 112 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Restore All Parameters + UDINT + 32 + 16 + + rw + o + + + + 2 + Restore Communication Parameters + UDINT + 32 + 48 + + rw + o + + + + 3 + Restore Application Parametesr + UDINT + 32 + 80 + + rw + o + + + + + + DT1018 + 144 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Vendor ID + UDINT + 32 + 16 + + ro + o + + + + 2 + Product code + UDINT + 32 + 48 + + ro + o + + + + 3 + Revision + UDINT + 32 + 80 + + ro + o + + + + 4 + Serial number + UDINT + 32 + 112 + + ro + o + + + + + DT1C00ARR + USINT + 32 + + 1 + 4 + + + + DT1C00 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DT1C00ARR + 32 + 16 + + ro + o + + + + + + DT10F1 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Local Error Reaction + UDINT + 32 + 16 + + rw + o + + + + 2 + Sync Error Counter Limit + UDINT + 32 + 48 + + rw + o + + + + + + DT1C32 + 488 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + m + + + + 1 + Synchronization Type + UINT + 16 + 16 + + rw + o + + + + 2 + Cycle Time + UDINT + 32 + 32 + + ro + o + + + + 4 + Synchronization Types supported + UINT + 16 + 96 + + ro + o + + + + 5 + Minimum Cycle Time + UDINT + 32 + 112 + + ro + o + + + + 6 + Calc and Copy Time + UDINT + 32 + 144 + + ro + o + + + + 8 + Get Cycle Time + UINT + 16 + 208 + + rw + c + + + + 9 + Delay Time + UDINT + 32 + 224 + + ro + c + + + + 10 + Sync0 Cycle Time + UDINT + 32 + 256 + + rw + o + + + + 11 + SM-Event Missed + UINT + 16 + 288 + + ro + c + + + + 12 + Cycle Time Too Small + UINT + 16 + 304 + + ro + c + + + + 32 + Sync Error + BOOL + 1 + 480 + + ro + c + + + + + + DT1C33 + 488 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + m + + + + 1 + Synchronization Type + UINT + 16 + 16 + + rw + o + + + + 2 + Cycle Time + UDINT + 32 + 32 + + ro + o + + + + 4 + Synchronization Types supported + UINT + 16 + 96 + + ro + o + + + + 5 + Minimum Cycle Time + UDINT + 32 + 112 + + ro + o + + + + 6 + Calc and Copy Time + UDINT + 32 + 144 + + ro + o + + + + 8 + Get Cycle Time + UINT + 16 + 208 + + rw + c + + + + 9 + Delay Time + UDINT + 32 + 224 + + ro + c + + + + 10 + Sync0 Cycle Time + UDINT + 32 + 256 + + rw + o + + + + 11 + SM-Event Missed + UINT + 16 + 288 + + ro + c + + + + 12 + Cycle Time Too Small + UINT + 16 + 304 + + ro + c + + + + 32 + Sync Error + BOOL + 1 + 480 + + ro + c + + + + + DT1600 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + + + + 1 + 1st Output Object to be mapped + UDINT + 32 + 16 + + rw + o + + + + 2 + 2nd Output Object to be mapped + UDINT + 32 + 48 + + rw + o + + + + 3 + 3rd Output Object to be mapped + UDINT + 32 + 80 + + rw + o + + + + 4 + 4th Output Object to be mapped + UDINT + 32 + 112 + + rw + o + + + + 5 + 5th Output Object to be mapped + UDINT + 32 + 144 + + rw + o + + + + 6 + 6th Output Object to be mapped + UDINT + 32 + 176 + + rw + o + + + + 7 + 7th Output Object to be mapped + UDINT + 32 + 208 + + rw + o + + + + 8 + 8th Output Object to be mapped + UDINT + 32 + 240 + + rw + o + + + + 9 + 9th Output Object to be mapped + UDINT + 32 + 272 + + rw + o + + + + 10 + 10th Output Object to be mapped + UDINT + 32 + 304 + + rw + o + + + + + DT1601 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + + + + 1 + 1st Output Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Output Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Output Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Output Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Output Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Output Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Output Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Output Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Output Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Output Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1602 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + 1st Output Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Output Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Output Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Output Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Output Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Output Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Output Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Output Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Output Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Output Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1A00 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + + + + 1 + 1st Input Object to be mapped + UDINT + 32 + 16 + + rw + o + + + + 2 + 2nd Input Object to be mapped + UDINT + 32 + 48 + + rw + o + + + + 3 + 3rd Input Object to be mapped + UDINT + 32 + 80 + + rw + o + + + + 4 + 4th Input Object to be mapped + UDINT + 32 + 112 + + rw + o + + + + 5 + 5th Input Object to be mapped + UDINT + 32 + 144 + + rw + o + + + + 6 + 6th Input Object to be mapped + UDINT + 32 + 176 + + rw + o + + + + 7 + 7th Input Object to be mapped + UDINT + 32 + 208 + + rw + o + + + + 8 + 8th Input Object to be mapped + UDINT + 32 + 240 + + rw + o + + + + 9 + 9th Input Object to be mapped + UDINT + 32 + 272 + + rw + o + + + + 10 + 10th Input Object to be mapped + UDINT + 32 + 304 + + rw + o + + + + + DT1A01 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + 1st Input Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Input Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Input Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Input Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Input Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Input Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Input Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Input Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Input Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Input Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1A02 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + 1st Input Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Input Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Input Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Input Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Input Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Input Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Input Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Input Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Input Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Input Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1C12ARR + UINT + 32 + + 1 + 2 + + + + DT1C12 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + o + + + + Elements + DT1C12ARR + 32 + 16 + + rw + o + + + + + DT1C13ARR + UINT + 32 + + 1 + 2 + + + + DT1C13 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + o + + + + Elements + DT1C13ARR + 32 + 16 + + rw + o + + + + + DT2001 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Eu Node Id + USINT + 8 + 16 + + rw + o + + + + 2 + Eu Can BitRate + UINT + 16 + 32 + + rw + o + + + + + DT2010 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Current Loop Kp Default + UINT + 16 + 16 + + ro + o + + + + 2 + Current Loop Ki Default + UINT + 16 + 32 + + ro + o + + + + 3 + Current Loop Kp + UINT + 16 + 48 + + rw + o + + + + 4 + Current Loop Ki + UINT + 16 + 64 + + rw + o + + + + + DT2012 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Velocity Loop Kp Default + UINT + 16 + 16 + + ro + o + + + + 2 + Velocity Loop Ki Default + UINT + 16 + 32 + + ro + o + + + + 3 + Velocity Loop Kp + UINT + 16 + 48 + + rw + o + + + + 4 + Velocity Loop Ki + UINT + 16 + 64 + + rw + o + + + + + DT2013 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Position Loop Kp Default + UINT + 16 + 16 + + ro + o + + + + 2 + Position Loop Ki Default + UINT + 16 + 32 + + ro + o + + + + 3 + Position Loop Kp + UINT + 16 + 48 + + rw + o + + + + 4 + Position Loop Ki + UINT + 16 + 64 + + rw + o + + + + + DT2014 + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuBrakeControl + USINT + 8 + 16 + + rw + o + + + + 2 + EuBrakeState + USINT + 8 + 32 + + ro + o + + + + 3 + EuBrakeAutoState + USINT + 8 + 48 + + rw + o + + + + + DT2016 + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Servo Temperture + SINT + 8 + 16 + + ro + o + + + + 2 + EuHighTemperatureLimit + SINT + 8 + 32 + + rw + o + + + + 3 + EuHighTemperatureWindowsTime + UINT + 16 + 48 + + rw + o + + + + + DT2020 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuMotorBlockTorque + UINT + 16 + 16 + + ro + o + + + + 2 + EuMotorBlockTime + UINT + 16 + 32 + + rw + o + + + + 3 + EuMotorBlockVelocity + UDINT + 32 + 48 + + rw + o + + + + + DT2021 + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuVolecityFollowingErrorWindows + UDINT + 32 + 16 + + rw + o + + + + 2 + EuVolecityFollowingErrorTime + UINT + 16 + 48 + + rw + o + + + + + DT202D + 144 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuUnderVoltageThreshold + UDINT + 32 + 16 + + rw + o + + + + 2 + EuUnderVoltageTime + UDINT + 32 + 48 + + rw + o + + + + 3 + EuOverVoltageThreshold + UDINT + 32 + 80 + + rw + o + + + + 4 + EuOverVoltageTime + UDINT + 32 + 112 + + rw + o + + + + + DT202E + 128 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuVelocityFeedforwardControlSelect + INT + 16 + 16 + + rw + o + + + + 2 + EuVelocityFeedforwardFilterTimeConst + INT + 16 + 32 + + rw + o + + + + 3 + EuVelocityFeedforwardGain + INT + 16 + 48 + + rw + o + + + + 4 + EuVelocityFeedforwardOriginalValue + DINT + 32 + 64 + + ro + o + + + + 4 + EuVelocityFeedforwardFilteredValue + DINT + 32 + 96 + + ro + o + + + + + DT210F + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + FocCurBiasA + UINT + 16 + 16 + + ro + o + + + + 2 + FocCurBiasB + UINT + 16 + 32 + + ro + o + + + + 3 + FocCurBiasC + UINT + 16 + 48 + + ro + o + + + + + DT607D + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Min position limit + DINT + 32 + 16 + + rw + o + + + + 2 + Max position limit + DINT + 32 + 48 + + rw + o + + + + + DT6091 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Motor revolutions + UDINT + 32 + 16 + + ro + o + + + + 2 + Shaft revolutions + UDINT + 32 + 48 + + rw + o + + + + + DT60C1 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Interpolation data record + DINT + 32 + 16 + + rw + o + + + + + DT60C2 + 32 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Interpolation period + USINT + 8 + 16 + + rw + o + + + + 2 + Interpolation Index + SINT + 8 + 24 + + rw + o + + + + + DT60FF + 32 + + 0 + Target Velocity + UDINT + 32 + 0 + + rw + o + R + + + + + DT6502 + 32 + + 0 + Supported Drive Modes + UDINT + 32 + 0 + + ro + o + + + + + DTF000 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Module index distance + UINT + 16 + 16 + + ro + o + + + + 2 + Maximum number of modules + UINT + 16 + 32 + + ro + o + + + + + DTF010ARR + UDINT + 64 + + 1 + 2 + + + + DTF010 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DTF010ARR + 64 + 16 + + ro + o + + + + + + DTF030ARR + UDINT + 64 + + 1 + 2 + + + + DTF030 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DTF030ARR + 64 + 16 + + ro + o + + + + + + DTF050ARR + UDINT + 64 + + 1 + 2 + + + + DTF050 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DTF050ARR + 64 + 16 + + ro + o + + + + + + + + #x1000 + Device type + UDINT + 32 + + 92010200 + + + ro + m + + + + #x1001 + Error register + USINT + 8 + + 00 + + + ro + o + + + + #x1008 + Device name + STRING(12) + 96 + + 4575504878782d787878 + + + ro + o + + + + #x1009 + Hardware version + STRING(10) + 80 + + 56332e3046343035 + + + ro + o + + + + #x100a + Software version + STRING(10) + 80 + + 56313433 + + + ro + o + + + + #x1c00 + Sync manager type + DT1C00 + 48 + + + SubIndex 000 + + 04 + + + + SubIndex 001 + + 01 + + + + SubIndex 002 + + 02 + + + + SubIndex 003 + + 03 + + + + SubIndex 004 + + 04 + + + + + ro + o + + + + #x1010 + Store Parameters + DT1010 + 112 + + + SubIndex 000 + + 03 + + + + Store All Parameters + + 00 + + + + Store Communication Parameters + + 00 + + + + Store Application Parameters + + 00 + + + + + rw + o + + + + #x1011 + Restore Default Parameters + DT1011 + 112 + + + SubIndex 000 + + 03 + + + + Store All Parameters + + 00 + + + + Store Communication Parameters + + 00 + + + + Store Application Parameters + + 00 + + + + + rw + o + + + + #x1018 + Identity + DT1018 + 144 + + + SubIndex 000 + + 04 + + + + Vendor ID + + 9710 + + + + Product code + + 0624 + + + + Revision + + 0002 + + + + Serial number + + 00000000 + + + + + ro + o + + + + #x10F1 + Error Settings + DT10F1 + 80 + + + SubIndex 000 + + 04 + + + + Local Error Reaction + + 01 + + + + Sync Error Counter Limit + + 04 + + + + + ro + o + + + + #x1c32 + SM output parameter + DT1C32 + 488 + + + SubIndex 000 + + 20 + + + + Synchronization Type + + 0100 + + + + Cycle Time + + 00000000 + + + + Synchronization Types supported + + 1E40 + + + + Minimum Cycle Time + + 50C30000 + + + + Calc and Copy Time + + 00000000 + + + + Get Cycle Time + + 0000 + + + + Delay Time + + 00000000 + + + + Sync0 Cycle Time + + 00000000 + + + + SM-Event Missed + + 0000 + + + + Cycle Time Too Small + + 0000 + + + + Sync Error + + 00 + + + + + ro + o + + + + #x1c33 + SM input parameter + DT1C33 + 488 + + + SubIndex 000 + + 20 + + + + Synchronization Type + + 2200 + + + + Cycle Time + + 00000000 + + + + Synchronization Types supported + + 1E40 + + + + Minimum Cycle Time + + 50C30000 + + + + Calc and Copy Time + + 00000000 + + + + Get Cycle Time + + 0000 + + + + Delay Time + + 00000000 + + + + Sync0 Cycle Time + + 00000000 + + + + SM-Event Missed + + 0000 + + + + Cycle Time Too Small + + 0000 + + + + Sync Error + + 00 + + + + + ro + o + + + + #x1c12 + RxPDO assign + DT1C12 + 48 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 0016 + + + + SubIndex 002 + + 0000 + + + + + ro + o + + + + #x1c13 + TxPDO assign + DT1C13 + 48 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 001a + + + + SubIndex 002 + + 0000 + + + + + ro + o + + + + #x1600 + csp/csv/cst RxPDO + DT1600 + 336 + + + SubIndex 000 + + 00 + 10 + #x06 + + + + 1st Output Object to be mapped + + #x60400010 + + + + 2st Output Object to be mapped + + #x607A0020 + + + + 3st Output Object to be mapped + + #x60FF0020 + + + + 4st Output Object to be mapped + + #x60710010 + + + + 5st Output Object to be mapped + + #x60600008 + + + + 6st Output Object to be mapped + + #x00000000 + + + + 7st Output Object to be mapped + + #x00000000 + + + + 8st Output Object to be mapped + + #x00000000 + + + + 9st Output Object to be mapped + + #x00000000 + + + + 10st Output Object to be mapped + + #x00000000 + + + + + rw + o + + + + #x1601 + pp/pv/pt RxPDO + DT1601 + 336 + + + SubIndex 000 + + #x0A + + + + 1st Output Object to be mapped + + #x60400010 + + + + 2st Output Object to be mapped + + #x607A0020 + + + + 3st Output Object to be mapped + + #x60FF0020 + + + + 4st Output Object to be mapped + + #x60710010 + + + + 5st Output Object to be mapped + + #x60830020 + + + + 6st Output Object to be mapped + + #x60840020 + + + + 7st Output Object to be mapped + + #x60810020 + + + + 8st Output Object to be mapped + + #x60870010 + + + + 9st Output Object to be mapped + + #x60600008 + + + + 10st Output Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x1602 + pv RxPDO + DT1602 + 336 + + + SubIndex 000 + + #x06 + + + + 1st Output Object to be mapped + + #x60400010 + + + + 2st Output Object to be mapped + + #x60FF0020 + + + + 3st Output Object to be mapped + + #x60830020 + + + + 4st Output Object to be mapped + + #x60840020 + + + + 5st Output Object to be mapped + + #x60600008 + + + + 6st Output Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x1a00 + csp/csv/cst TxPDO + DT1A00 + 336 + + + SubIndex 000 + + 00 + 10 + #x07 + + + + 1st Input Object to be mapped + + #x60410010 + + + + 2st Input Object to be mapped + + #x60640020 + + + + 3st Input Object to be mapped + + #x606C0020 + + + + 4st Input Object to be mapped + + #x60770010 + + + + 5st Input Object to be mapped + + #x60610008 + + + + 6st Input Object to be mapped + + #x603F0010 + + + + 7st Input Object to be mapped + + #x00000000 + + + + 8st Input Object to be mapped + + #x00000000 + + + + 9st Input Object to be mapped + + #x00000000 + + + + 10st Input Object to be mapped + + #x00000000 + + + + + rw + o + + + + #x1a01 + pp/pv/pt TxPDO + DT1A01 + 336 + + + SubIndex 000 + + #x07 + + + + 1st Input Object to be mapped + + #x60410010 + + + + 2st Input Object to be mapped + + #x60640020 + + + + 3st Input Object to be mapped + + #x606C0020 + + + + 4st Input Object to be mapped + + #x60770020 + + + + 5st Input Object to be mapped + + #x60610008 + + + + 6st Input Object to be mapped + + #x603F0010 + + + + 7st Input Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x1a02 + pv TxPDO + DT1A02 + 336 + + + SubIndex 000 + + #x05 + + + + 1st Input Object to be mapped + + #x60410010 + + + + 2st Input Object to be mapped + + #x606C0020 + + + + 3st Input Object to be mapped + + #x60610008 + + + + 4st Input Object to be mapped + + #x603F0010 + + + + 5st Input Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x2000 + Eu Motor SN + UDINT + 32 + + + Eu Motor SN + + 00 + + + + + rw + o + + + + #x2001 + Servo Config + DT2001 + 48 + + + SubIndex 000 + + 02 + + + + Eu Node Id + + 01 + + + + Eu Can BitRate + + 1000 + + + + + ro + o + + + + #x2002 + Motor Para + UDINT + 32 + + + Motor Para + + 00 + + + + + rw + o + + + + #x2003 + Soft Limit State + UDINT + 32 + + + Soft Limit State + + 00 + + + + + rw + o + + + + #x2004 + EuCommMode + USINT + 8 + + + EuCommMode + + 00 + + + + + rw + o + + + + #x2010 + Current Loop Pi + DT2010 + 80 + + + SubIndex 000 + + 04 + + + + Current Loop Kp Default + + 0040 + + + + Current Loop Ki Default + + 0001 + + + + Current Loop Kp + + 0004 + + + + Current Loop Ki + + 0001 + + + + + ro + o + + + + #x2012 + Velocity Loop Pi + DT2012 + 80 + + + SubIndex 000 + + 04 + + + + Velocity Loop Kp Default + + D007 + + + + Velocity Loop Ki Default + + 64 + + + + Velocity Loop Kp + + D007 + + + + Velocity Loop Ki + + 64 + + + + + ro + o + + + + #x2013 + Position Loop Pi + DT2013 + 80 + + + SubIndex 000 + + 04 + + + + Position Loop Kp Default + + D007 + + + + Position Loop Ki Default + + 64 + + + + Position Loop Kp + + D007 + + + + Position Loop Ki + + 64 + + + + + ro + o + + + + #x2014 + brake control + DT2014 + 64 + + + SubIndex 000 + + 03 + + + + EuBrakeControl + + 00 + + + + EuBrakeState + + 00 + + + + EuBrakeAutoState + + 01 + + + + + ro + o + + + + #x2015 + Eu Pwm Inv + USINT + 8 + + + Eu Pwm Inv + + 00 + + + + + ro + o + + + + #x2016 + Servo Temperture + DT2016 + 64 + + + SubIndex 000 + + 03 + + + + EuTemperature + + 19 + + + + EuHighTemperatureLimit + + 55 + + + + EuHighTemperatureWindowsTime + + B80B + + + + + ro + o + + + + #x2017 + EuVelocityIntLimit + UINT + 16 + + + EuVelocityIntLimit + + D007 + + + + + rw + o + + + + #x2020 + Motor Block + DT2020 + 80 + + + SubIndex 000 + + 03 + + + + EuMotorBlockTorque + + E803 + + + + EuMotorBlockTime + + B80B + + + + EuMotorBlockVelocity + + 1027 + + + + + ro + o + + + + #x2021 + velocity floowing error + DT2021 + 64 + + + SubIndex 000 + + 02 + + + + EuVolecityFollowingErrorWindows + + A08601 + + + + EuVolecityFollowingErrorTime + + 0BB8 + + + + + ro + o + + + + #x2022 + torque window + UINT + 16 + + + torque window + + 0A + + + + + rw + o + + + + #x2023 + Torque window time + UINT + 16 + + + Torque window time + + 01 + + + + + rw + o + + + + #x2024 + EuOverSpeedThreshold + UDINT + 32 + + + EuOverSpeedThreshold + + AAAA42 + + + + + rw + o + + + + #x2025 + EuOverSpeedTime + UINT + 16 + + + EuOverSpeedTime + + 64 + + + + + rw + o + + + + #x2026 + EuBrakeDelayTime + USINT + 8 + + + EuBrakeDelayTime + + 64 + + + + + rw + o + + + + #x2027 + EuAutoMagnetAngleAlignmentFlag + UDINT + 32 + + + EuAutoMagnetAngleAlignmentFlag + + 00 + + + + + rw + o + + + + #x2028 + EuI2tLimit + UINT + 16 + + + EuI2tLimit + + 64 + + + + + rw + o + + + + #x2029 + EuI2tValue + UINT + 16 + + + EuI2tValue + + 00 + + + + + ro + o + + + + #x202A + EuFirstEncoderValue + DINT + 32 + + + EuFirstEncoderValue + + 00 + + + + + ro + o + + + + #x202B + EuSecondEncoderValue + DINT + 32 + + + EuSecondEncoderValue + + 00 + + + + + ro + o + + + + #x202C + EuThetaBias + DINT + 32 + + + EuThetaBias + + 00 + + + + + ro + o + + + + #x202D + Eu Voltage Threshold + DT202D + 144 + + + SubIndex 000 + + 04 + + + + EuUnderVoltageThreshold + + 283707 + + + + EuUnderVoltageTime + + 05 + + + + EuOverVoltageThreshold + + A00901 + + + + EuOverVoltageTime + + 05 + + + + + ro + o + + + + #x202E + Eu Velocity Feedforward + DT202E + 128 + + + SubIndex 000 + + 05 + + + + EuVelocityFeedforwardControlSelect + + 01 + + + + EuVelocityFeedforwardFilterTimeConst + + 00 + + + + EuVelocityFeedforwardGain + + 00 + + + + EuVelocityFeedforwardOriginalValue + + 00 + + + + EuVelocityFeedforwardFilteredValue + + 00 + + + + + ro + o + + + + #x2100 + EuDisableFpgaMu150Spi + DINT + 32 + + + EuDisableFpgaMu150Spi + + 00 + + + + + rw + o + + + + #x2104 + ServoMagnetAngleAlignment + UDINT + 32 + + + ServoMagnetAngleAlignment + + 00 + + + + + rw + o + + + + #x2109 + EuGoToBoot + DINT + 32 + + + EuGoToBoot + + 00 + + + + + rw + o + + + + #x210A + EuCompStart + DINT + 32 + + + EuCompStart + + 00 + + + + + rw + o + + + + #x210B + EuCompEn + DINT + 32 + + + EuCompEn + + 00 + + + + + rw + o + + + + #x210C + EuCompBias + DINT + 32 + + + EuCompBias + + 00 + + + + + rw + o + + + + #x210D + EuCompState + DINT + 32 + + + EuCompState + + 00 + + + + + rw + o + + + + #x210E + EuFocCurBiasSet + USINT + 8 + + + EuFocCurBiasSet + + 00 + + + + + rw + o + + + + #x210F + Foc Cur Bias + DT210F + 144 + + + SubIndex 000 + + 03 + + + + FocCurBiasA + + 00 + + + + FocCurBiasB + + 00 + + + + FocCurBiasC + + 00 + + + + + ro + o + + + + #x2110 + EuTorqueFactor + UINT + 16 + + + EuTorqueFactor + + E803 + + + + + rw + o + + + + #x2201 + EuSysOutPulseStep + UDINT + 32 + + + EuSysOutPulseStep + + 00 + + + + + rw + o + + + + #x2301 + Position Velocity Mix Position Kp + UINT + 16 + + + Position Velocity Mix Position Kp + + 00 + + + + + rw + o + + + + #x2302 + Position Velocity Mix Velocity Kd + UINT + 16 + + + Position Velocity Mix Velocity Kd + + 00 + + + + + rw + o + + + + #x603F + Error Code + UINT + 16 + + + Error Code + + 00 + + + + + ro + o + T + + + + #x6040 + Control Word + UINT + 16 + + + Control Word + + 00 + + + + + rw + o + R + + + + #x6041 + Status Word + UINT + 16 + + + Status Word + + 00 + + + + + ro + o + T + + + + #x605A + Quickstop Option Code + INT + 16 + + + Quickstop Option Code + + 02 + + + + + rw + o + + + + #x605B + Shutdown Option Code + INT + 16 + + + Shutdown Option Code + + 00 + + + + + rw + o + + + + #x605C + Disable Operation Option Code + INT + 16 + + + Disable Operation Option Code + + 01 + + + + + rw + o + + + + #x605D + Halt option code + INT + 16 + + + Halt option code + + 01 + + + + + rw + o + + + + #x605E + Fault Reaction Code + INT + 16 + + + Fault Reaction Code + + 02 + + + + + rw + o + + + + #x6060 + Modes of Operation + SINT + 8 + + + Modes of Operation + + 00 + + + + + rw + o + R + + + + #x6061 + Modes of Operation Display + SINT + 8 + + + Modes of Operation Display + + 00 + + + + + ro + o + T + + + + #x6062 + Position demannd value + DINT + 32 + + + Position demannd value + + 00 + + + + + ro + o + T + + + + #x6064 + Position Actual Value + DINT + 32 + + + Position Actual Value + + 00 + + + + + ro + o + T + + + + #x6065 + Maximal following error + UDINT + 32 + + + Maximal following error + + E09304 + + + + + rw + o + + + + #x6067 + Position window + UDINT + 32 + + + Position window + + 64 + + + + + rw + o + + + + #x6068 + Position window time + UINT + 16 + + + Position window time + + 01 + + + + + rw + o + + + + #x606B + Velocity demand value + DINT + 32 + + + Velocity demand value + + 00 + + + + + ro + o + T + + + + #x606C + Velocity Actual Value + DINT + 32 + + + Velocity Actual Value + + 00 + + + + + ro + o + T + + + + #x606D + Velocity window + UINT + 16 + + + Velocity window + + 1027 + + + + + ro + o + + + + #x606E + Velocity window time + UINT + 16 + + + Velocity window time + + 01 + + + + + ro + o + + + + #x606F + Velocity threshold + UINT + 16 + + + Velocity threshold + + E803 + + + + + ro + o + + + + #x6070 + Velocity threshold time + UINT + 16 + + + Velocity threshold time + + 01 + + + + + ro + o + + + + #x6071 + Target Torque + INT + 16 + + + Target Torque + + 00 + + + + + rw + o + + + + #x6072 + Max Torque + UINT + 16 + + + Max Torque + + D007 + + + + + rw + o + + + + #x6074 + Torque demand value + INT + 16 + + + Torque Actual Value + + 00 + + + + + ro + o + T + + + + #x6076 + Motor rated torque + UDINT + 32 + + + Motor rated torque + + 00 + + + + + rw + o + + + + #x6077 + Torque Actual Value + INT + 16 + + + Torque Actual Value + + 00 + + + + + ro + o + T + + + + #x6078 + Current actual value + INT + 16 + + + Current actual value + + 00 + + + + + ro + o + T + + + + #x6079 + DC link circuit voltage + UDINT + 32 + + + DC link circuit voltage + + 00 + + + + + rw + o + + + + #x607A + Target Position + DINT + 32 + + + Target Position + + 00 + + + + + rw + o + R + + + + #x607C + Home Offset + DINT + 32 + + + Home Offset + + 00 + + + + + rw + o + + + + #x607D + Software Position Limit + DT607D + 80 + + + SubIndex 000 + + 02 + + + + Min position limit + + 006CCA88 + + + + Max position limit + + 00943577 + + + + + ro + o + + + + #x607F + Max Profile Velocity + UDINT + 32 + + + Max Profile Velocity + + 00 + + + + + rw + o + + + + #x6081 + Profile Velocity + UDINT + 32 + + + Profile Velocity + + 00 + + + + + rw + o + R + + + + #x6083 + Profile Acceleration + UDINT + 32 + + + Profile Acceleration + + 00 + + + + + rw + o + R + + + + #x6084 + Profile Deceleration + UDINT + 32 + + + Profile Deceleration + + 00 + + + + + rw + o + R + + + + #x6085 + Quickstop Declaration + DINT + 32 + + + Quickstop Declaration + + 00 + + + + + rw + o + + + + #x6087 + Torque slope + UDINT + 32 + + + Torque slope + + E803 + + + + + rw + o + + + + #x6091 + Gear ratio + DT6091 + 80 + + + SubIndex 000 + + 02 + + + + Motor revolutions + + 000051 + + + + Shaft revolutions + + 000051 + + + + + ro + o + + + + #x60B1 + Velocity Offset + DINT + 32 + + + Velocity Offset + + 00 + + + + + rw + o + + + + #x60B2 + Torque Offset + INT + 16 + + + Torque Offset + + 00 + + + + + rw + o + + + + #x60C1 + Interpolation Data Record + DT60C1 + 48 + + + SubIndex 000 + + 01 + + + + Interpolation data record + + 00 + + + + + ro + o + + + + #x60C2 + Interpolation Time Period + DT60C2 + 32 + + + SubIndex 000 + + 02 + + + + Interpolation period + + 01 + + + + Interpolation Index + + -3 + + + + + ro + o + + + + #x60F4 + Following error actual value + DINT + 32 + + + Following error actual value + + 00 + + + + + ro + o + T + + + + #x60FF + Target Velocity + DINT + 32 + + + Target Velocity + + 00 + + + + + rw + o + R + + + + #x6502 + Supported Drive Modes + UDINT + 32 + + + Supported Drive Modes + + 0001 + + + + + ro + o + + + + #xf000 + Modular device profile + DTF000 + 48 + + + SubIndex 000 + + 02 + + + + Module index distance + + 2003 + + + + Maximum number of modules + + 02 + + + + + ro + o + + + + #xf010 + Module profile list + DTF010 + 80 + + + SubIndex 000 + + 02 + + + + SubIndex 001 + + 92010200 + + + + SubIndex 002 + + 00000000 + + + + + ro + o + + + + #xf030 + Configured module Ident list + DTF030 + 80 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 00983100 + + + + SubIndex 002 + + 00000000 + + + + + ro + o + + + + #xf050 + Module detected list + DTF050 + 80 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 00983100 + + + + SubIndex 002 + + 00000000 + + + + + ro + o + + + + + + Outputs + Inputs + MBoxState + MBoxOut + MBoxIn + Outputs + Inputs + + + + + + Synchron + SM-Synchron + #x0 + 0 + 0 + 0 + + + DC + DC-Synchron + #x300 + 0 + 0 + 0 + + + + + Axis 0 + #x119800 + #x219800 + #x319800 + + + + 2048 + 800E00CC8813f000000000800000 + + 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 + + + + + Axis (csv,csp,cst) + dynamic switchbewteen csp/csv + + #x1600 + #x1601 + #x1602 + Outputs + + #x6040 + 0 + 16 + Control Word + object 0x6040:0 + UINT + + + #x607A + 0 + 32 + Target Position + object 0x607A:0 + DINT + + + #x60FF + 0 + 32 + Target Velocity + object 0x60FF:0 + DINT + + + #x6071 + 0 + 16 + Target Torque + object 0x6071:0 + INT + + + #x6060 + 0 + 8 + Mode Of Operation + object 0x6060:0 + SINT + + + #x0 + 0 + 8 + + + + #x1a00 + #x1a01 + #x1a02 + Inputs + + #x6041 + 0 + 16 + Status Word + object 0x6041:0 + UINT + + + #x6064 + 0 + 32 + Actual Position + object 0x6064:0 + DINT + + + #x606C + 0 + 32 + Actual Velocity + object 0x606C:0 + DINT + + + #x6077 + 0 + 16 + Actual Torque + object 0x6077:0 + INT + + + #x6061 + 0 + 8 + Mode Of OperationDisplay + object 0x6061:0 + SINT + + + #x603F + 0 + 16 + Error Code + object 0x603F:0 + UINT + + + #x0 + 0 + 8 + + + + 402 + + + + Axis (pp,pv,pt) + Axis only supports pp + + #x1601 + #x1600 + #x1602 + Outputs + + #x6040 + 0 + 16 + Control Word + object 0x6040:0 + UINT + + + #x607A + 0 + 32 + Target Position + object 0x607A:0 + DINT + + + #x60FF + 0 + 32 + Target Velocity + object 0x60FF:0 + DINT + + + #x6071 + 0 + 16 + Target Torque + object 0x6071:0 + INT + + + #x6083 + 0 + 32 + Profile Acceleration + object 0x6083:0 + UDINT + + + #x6084 + 0 + 32 + Profile Deceleration + object 0x6084:0 + UDINT + + + #x6081 + 0 + 32 + Profile Velocity + object 0x6081:0 + UDINT + + + #x6087 + 0 + 32 + Torque Slope + object 0x6087:0 + UDINT + + + #x6060 + 0 + 8 + Modes Of Operation + object 0x6060:0 + SINT + + + #x0 + 0 + 8 + + + + #x1a01 + #x1a00 + #x1a02 + Inputs + + #x6041 + 0 + 16 + Status Word + object 0x6041:0 + UINT + + + #x6064 + 0 + 32 + Actual Position + object 0x6064:0 + DINT + + + #x606C + 0 + 32 + Actual Velocity + object 0x606C:0 + DINT + + + #x6077 + 0 + 16 + Actual Torque + object 0x6077:0 + INT + + + #x6061 + 0 + 8 + Mode Of OperationDisplay + object 0x6061:0 + SINT + + + #x603F + 0 + 16 + Error Code + object 0x603F:0 + UINT + + + #x0 + 0 + 8 + + + + + + SO + #x6060 + 0 + 08 + Modes of operation + + + + + 402 + + + + pv - axis + Axis only supports pv + + #x1602 + #x1600 + #x1601 + Outputs + + #x6040 + 0 + 16 + Control Word + object 0x6040:0 + UINT + + + #x60FF + 0 + 32 + Target Velocity + object 0x60FF:0 + DINT + + + #x6083 + 0 + 32 + Profile Acceleration + object 0x6083:0 + UDINT + + + #x6084 + 0 + 32 + Profile Deceleration + object 0x6084:0 + UDINT + + + #x6060 + 0 + 8 + Modes Of Operation + object 0x6060:0 + SINT + + + #x0 + 0 + 8 + + + + #x1a02 + #x1a00 + #x1a01 + Inputs + + #x6041 + 0 + 16 + Status Word + object 0x6041:0 + UINT + + + #x606C + 0 + 32 + Actual Velocity + object 0x606C:0 + DINT + + + #x6061 + 0 + 8 + Mode Of OperationDisplay + object 0x6061:0 + SINT + + + #x603F + 0 + 16 + Error Code + object 0x603F:0 + UINT + + + #x0 + 0 + 8 + + + + + + SO + #x6060 + 0 + 08 + Modes of operation + + + + + 402 + + + + + diff --git a/docs/EYOU_ServoModule_ECAT_V145_no_slot.xml b/docs/EYOU_ServoModule_ECAT_V145_no_slot.xml new file mode 100644 index 00000000..d8204e8f --- /dev/null +++ b/docs/EYOU_ServoModule_ECAT_V145_no_slot.xml @@ -0,0 +1,5495 @@ + + + + #x1097 + Jiangsu Yiyou Robot Technology Co., Ltd. + 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 + + + + + ServoDrive + Servo Drives + 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 + + + + + EYOU_ServoModule_V145 + EYOU_ServoModule_ECAT_V145 + + + + 2000 + 9000 + 5000 + 200 + + + + + 100 + 2000 + + + + ServoDrive + + + 402 + + + + + + BIT2 + 2 + + + + BOOL + 1 + + + + DINT + 32 + + + + INT + 16 + + + + SINT + 8 + + + + UDINT + 32 + + + + UINT + 16 + + + + USINT + 8 + + + + REAL + 32 + + + + ARRAY [0..3] OF BYTE + USINT + 32 + + 0 + 4 + + + + + STRING(12) + 96 + + + + STRING(10) + 80 + + + + DT1010 + 112 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Store All Parameters + UDINT + 32 + 16 + + rw + o + + + + 2 + Store Communication Parameters + UDINT + 32 + 48 + + rw + o + + + + 3 + Store Application Parametesr + UDINT + 32 + 80 + + rw + o + + + + + + DT1011 + 112 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Restore All Parameters + UDINT + 32 + 16 + + rw + o + + + + 2 + Restore Communication Parameters + UDINT + 32 + 48 + + rw + o + + + + 3 + Restore Application Parametesr + UDINT + 32 + 80 + + rw + o + + + + + + DT1018 + 144 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Vendor ID + UDINT + 32 + 16 + + ro + o + + + + 2 + Product code + UDINT + 32 + 48 + + ro + o + + + + 3 + Revision + UDINT + 32 + 80 + + ro + o + + + + 4 + Serial number + UDINT + 32 + 112 + + ro + o + + + + + DT1C00ARR + USINT + 32 + + 1 + 4 + + + + DT1C00 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DT1C00ARR + 32 + 16 + + ro + o + + + + + + DT10F1 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Local Error Reaction + UDINT + 32 + 16 + + rw + o + + + + 2 + Sync Error Counter Limit + UDINT + 32 + 48 + + rw + o + + + + + + DT1C32 + 488 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + m + + + + 1 + Synchronization Type + UINT + 16 + 16 + + rw + o + + + + 2 + Cycle Time + UDINT + 32 + 32 + + ro + o + + + + 4 + Synchronization Types supported + UINT + 16 + 96 + + ro + o + + + + 5 + Minimum Cycle Time + UDINT + 32 + 112 + + ro + o + + + + 6 + Calc and Copy Time + UDINT + 32 + 144 + + ro + o + + + + 8 + Get Cycle Time + UINT + 16 + 208 + + rw + c + + + + 9 + Delay Time + UDINT + 32 + 224 + + ro + c + + + + 10 + Sync0 Cycle Time + UDINT + 32 + 256 + + rw + o + + + + 11 + SM-Event Missed + UINT + 16 + 288 + + ro + c + + + + 12 + Cycle Time Too Small + UINT + 16 + 304 + + ro + c + + + + 32 + Sync Error + BOOL + 1 + 480 + + ro + c + + + + + + DT1C33 + 488 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + m + + + + 1 + Synchronization Type + UINT + 16 + 16 + + rw + o + + + + 2 + Cycle Time + UDINT + 32 + 32 + + ro + o + + + + 4 + Synchronization Types supported + UINT + 16 + 96 + + ro + o + + + + 5 + Minimum Cycle Time + UDINT + 32 + 112 + + ro + o + + + + 6 + Calc and Copy Time + UDINT + 32 + 144 + + ro + o + + + + 8 + Get Cycle Time + UINT + 16 + 208 + + rw + c + + + + 9 + Delay Time + UDINT + 32 + 224 + + ro + c + + + + 10 + Sync0 Cycle Time + UDINT + 32 + 256 + + rw + o + + + + 11 + SM-Event Missed + UINT + 16 + 288 + + ro + c + + + + 12 + Cycle Time Too Small + UINT + 16 + 304 + + ro + c + + + + 32 + Sync Error + BOOL + 1 + 480 + + ro + c + + + + + DT1600 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + + + + 1 + 1st Output Object to be mapped + UDINT + 32 + 16 + + rw + o + + + + 2 + 2nd Output Object to be mapped + UDINT + 32 + 48 + + rw + o + + + + 3 + 3rd Output Object to be mapped + UDINT + 32 + 80 + + rw + o + + + + 4 + 4th Output Object to be mapped + UDINT + 32 + 112 + + rw + o + + + + 5 + 5th Output Object to be mapped + UDINT + 32 + 144 + + rw + o + + + + 6 + 6th Output Object to be mapped + UDINT + 32 + 176 + + rw + o + + + + 7 + 7th Output Object to be mapped + UDINT + 32 + 208 + + rw + o + + + + 8 + 8th Output Object to be mapped + UDINT + 32 + 240 + + rw + o + + + + 9 + 9th Output Object to be mapped + UDINT + 32 + 272 + + rw + o + + + + 10 + 10th Output Object to be mapped + UDINT + 32 + 304 + + rw + o + + + + + DT1601 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + + + + 1 + 1st Output Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Output Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Output Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Output Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Output Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Output Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Output Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Output Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Output Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Output Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1602 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + 1st Output Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Output Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Output Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Output Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Output Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Output Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Output Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Output Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Output Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Output Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1A00 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + + + + 1 + 1st Input Object to be mapped + UDINT + 32 + 16 + + rw + o + + + + 2 + 2nd Input Object to be mapped + UDINT + 32 + 48 + + rw + o + + + + 3 + 3rd Input Object to be mapped + UDINT + 32 + 80 + + rw + o + + + + 4 + 4th Input Object to be mapped + UDINT + 32 + 112 + + rw + o + + + + 5 + 5th Input Object to be mapped + UDINT + 32 + 144 + + rw + o + + + + 6 + 6th Input Object to be mapped + UDINT + 32 + 176 + + rw + o + + + + 7 + 7th Input Object to be mapped + UDINT + 32 + 208 + + rw + o + + + + 8 + 8th Input Object to be mapped + UDINT + 32 + 240 + + rw + o + + + + 9 + 9th Input Object to be mapped + UDINT + 32 + 272 + + rw + o + + + + 10 + 10th Input Object to be mapped + UDINT + 32 + 304 + + rw + o + + + + + DT1A01 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + 1st Input Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Input Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Input Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Input Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Input Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Input Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Input Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Input Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Input Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Input Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1A02 + 336 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + 1st Input Object to be mapped + UDINT + 32 + 16 + + ro + o + + + + 2 + 2nd Input Object to be mapped + UDINT + 32 + 48 + + ro + o + + + + 3 + 3rd Input Object to be mapped + UDINT + 32 + 80 + + ro + o + + + + 4 + 4th Input Object to be mapped + UDINT + 32 + 112 + + ro + o + + + + 5 + 5th Input Object to be mapped + UDINT + 32 + 144 + + ro + o + + + + 6 + 6th Input Object to be mapped + UDINT + 32 + 176 + + ro + o + + + + 7 + 7th Input Object to be mapped + UDINT + 32 + 208 + + ro + o + + + + 8 + 8th Input Object to be mapped + UDINT + 32 + 240 + + ro + o + + + + 9 + 9th Input Object to be mapped + UDINT + 32 + 272 + + ro + o + + + + 10 + 10th Input Object to be mapped + UDINT + 32 + 304 + + ro + o + + + + + DT1C12ARR + UINT + 32 + + 1 + 2 + + + + DT1C12 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + o + + + + Elements + DT1C12ARR + 32 + 16 + + rw + o + + + + + DT1C13ARR + UINT + 32 + + 1 + 2 + + + + DT1C13 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + rw + o + + + + Elements + DT1C13ARR + 32 + 16 + + rw + o + + + + + DT2001 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Eu Node Id + USINT + 8 + 16 + + rw + o + + + + 2 + Eu Can BitRate + UINT + 16 + 32 + + rw + o + + + + + DT2010 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Current Loop Kp Default + UINT + 16 + 16 + + ro + o + + + + 2 + Current Loop Ki Default + UINT + 16 + 32 + + ro + o + + + + 3 + Current Loop Kp + UINT + 16 + 48 + + rw + o + + + + 4 + Current Loop Ki + UINT + 16 + 64 + + rw + o + + + + + DT2012 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Velocity Loop Kp Default + UINT + 16 + 16 + + ro + o + + + + 2 + Velocity Loop Ki Default + UINT + 16 + 32 + + ro + o + + + + 3 + Velocity Loop Kp + UINT + 16 + 48 + + rw + o + + + + 4 + Velocity Loop Ki + UINT + 16 + 64 + + rw + o + + + + + DT2013 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Position Loop Kp Default + UINT + 16 + 16 + + ro + o + + + + 2 + Position Loop Ki Default + UINT + 16 + 32 + + ro + o + + + + 3 + Position Loop Kp + UINT + 16 + 48 + + rw + o + + + + 4 + Position Loop Ki + UINT + 16 + 64 + + rw + o + + + + + DT2014 + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuBrakeControl + USINT + 8 + 16 + + rw + o + + + + 2 + EuBrakeState + USINT + 8 + 32 + + ro + o + + + + 3 + EuBrakeAutoState + USINT + 8 + 48 + + rw + o + + + + + DT2016 + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Servo Temperture + SINT + 8 + 16 + + ro + o + + + + 2 + EuHighTemperatureLimit + SINT + 8 + 32 + + rw + o + + + + 3 + EuHighTemperatureWindowsTime + UINT + 16 + 48 + + rw + o + + + + + DT2020 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuMotorBlockTorque + UINT + 16 + 16 + + ro + o + + + + 2 + EuMotorBlockTime + UINT + 16 + 32 + + rw + o + + + + 3 + EuMotorBlockVelocity + UDINT + 32 + 48 + + rw + o + + + + + DT2021 + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuVolecityFollowingErrorWindows + UDINT + 32 + 16 + + rw + o + + + + 2 + EuVolecityFollowingErrorTime + UINT + 16 + 48 + + rw + o + + + + + DT202D + 144 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuUnderVoltageThreshold + UDINT + 32 + 16 + + rw + o + + + + 2 + EuUnderVoltageTime + UDINT + 32 + 48 + + rw + o + + + + 3 + EuOverVoltageThreshold + UDINT + 32 + 80 + + rw + o + + + + 4 + EuOverVoltageTime + UDINT + 32 + 112 + + rw + o + + + + + DT202E + 128 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + EuVelocityFeedforwardControlSelect + INT + 16 + 16 + + rw + o + + + + 2 + EuVelocityFeedforwardFilterTimeConst + INT + 16 + 32 + + rw + o + + + + 3 + EuVelocityFeedforwardGain + INT + 16 + 48 + + rw + o + + + + 4 + EuVelocityFeedforwardOriginalValue + DINT + 32 + 64 + + ro + o + + + + 4 + EuVelocityFeedforwardFilteredValue + DINT + 32 + 96 + + ro + o + + + + + DT210F + 64 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + FocCurBiasA + UINT + 16 + 16 + + ro + o + + + + 2 + FocCurBiasB + UINT + 16 + 32 + + ro + o + + + + 3 + FocCurBiasC + UINT + 16 + 48 + + ro + o + + + + + DT607D + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Min position limit + DINT + 32 + 16 + + rw + o + + + + 2 + Max position limit + DINT + 32 + 48 + + rw + o + + + + + DT6091 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Motor revolutions + UDINT + 32 + 16 + + ro + o + + + + 2 + Shaft revolutions + UDINT + 32 + 48 + + rw + o + + + + + DT60C1 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Interpolation data record + DINT + 32 + 16 + + rw + o + + + + + DT60C2 + 32 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Interpolation period + USINT + 8 + 16 + + rw + o + + + + 2 + Interpolation Index + SINT + 8 + 24 + + rw + o + + + + + DT60FF + 32 + + 0 + Target Velocity + UDINT + 32 + 0 + + rw + o + R + + + + + DT6502 + 32 + + 0 + Supported Drive Modes + UDINT + 32 + 0 + + ro + o + + + + + DTF000 + 48 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + 1 + Module index distance + UINT + 16 + 16 + + ro + o + + + + 2 + Maximum number of modules + UINT + 16 + 32 + + ro + o + + + + + DTF010ARR + UDINT + 64 + + 1 + 2 + + + + DTF010 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DTF010ARR + 64 + 16 + + ro + o + + + + + + DTF030ARR + UDINT + 64 + + 1 + 2 + + + + DTF030 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DTF030ARR + 64 + 16 + + ro + o + + + + + + DTF050ARR + UDINT + 64 + + 1 + 2 + + + + DTF050 + 80 + + 0 + SubIndex 000 + USINT + 8 + 0 + + ro + o + + + + Elements + DTF050ARR + 64 + 16 + + ro + o + + + + + + + + #x1000 + Device type + UDINT + 32 + + 92010200 + + + ro + m + + + + #x1001 + Error register + USINT + 8 + + 00 + + + ro + o + + + + #x1008 + Device name + STRING(12) + 96 + + 4575504878782d787878 + + + ro + o + + + + #x1009 + Hardware version + STRING(10) + 80 + + 56332e3046343035 + + + ro + o + + + + #x100a + Software version + STRING(10) + 80 + + 56313433 + + + ro + o + + + + #x1c00 + Sync manager type + DT1C00 + 48 + + + SubIndex 000 + + 04 + + + + SubIndex 001 + + 01 + + + + SubIndex 002 + + 02 + + + + SubIndex 003 + + 03 + + + + SubIndex 004 + + 04 + + + + + ro + o + + + + #x1010 + Store Parameters + DT1010 + 112 + + + SubIndex 000 + + 03 + + + + Store All Parameters + + 00 + + + + Store Communication Parameters + + 00 + + + + Store Application Parameters + + 00 + + + + + rw + o + + + + #x1011 + Restore Default Parameters + DT1011 + 112 + + + SubIndex 000 + + 03 + + + + Store All Parameters + + 00 + + + + Store Communication Parameters + + 00 + + + + Store Application Parameters + + 00 + + + + + rw + o + + + + #x1018 + Identity + DT1018 + 144 + + + SubIndex 000 + + 04 + + + + Vendor ID + + 9710 + + + + Product code + + 0624 + + + + Revision + + 0002 + + + + Serial number + + 00000000 + + + + + ro + o + + + + #x10F1 + Error Settings + DT10F1 + 80 + + + SubIndex 000 + + 04 + + + + Local Error Reaction + + 01 + + + + Sync Error Counter Limit + + 04 + + + + + ro + o + + + + #x1c32 + SM output parameter + DT1C32 + 488 + + + SubIndex 000 + + 20 + + + + Synchronization Type + + 0100 + + + + Cycle Time + + 00000000 + + + + Synchronization Types supported + + 1E40 + + + + Minimum Cycle Time + + 50C30000 + + + + Calc and Copy Time + + 00000000 + + + + Get Cycle Time + + 0000 + + + + Delay Time + + 00000000 + + + + Sync0 Cycle Time + + 00000000 + + + + SM-Event Missed + + 0000 + + + + Cycle Time Too Small + + 0000 + + + + Sync Error + + 00 + + + + + ro + o + + + + #x1c33 + SM input parameter + DT1C33 + 488 + + + SubIndex 000 + + 20 + + + + Synchronization Type + + 2200 + + + + Cycle Time + + 00000000 + + + + Synchronization Types supported + + 1E40 + + + + Minimum Cycle Time + + 50C30000 + + + + Calc and Copy Time + + 00000000 + + + + Get Cycle Time + + 0000 + + + + Delay Time + + 00000000 + + + + Sync0 Cycle Time + + 00000000 + + + + SM-Event Missed + + 0000 + + + + Cycle Time Too Small + + 0000 + + + + Sync Error + + 00 + + + + + ro + o + + + + #x1c12 + RxPDO assign + DT1C12 + 48 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 0016 + + + + SubIndex 002 + + 0000 + + + + + ro + o + + + + #x1c13 + TxPDO assign + DT1C13 + 48 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 001a + + + + SubIndex 002 + + 0000 + + + + + ro + o + + + + #x1600 + csp/csv/cst RxPDO + DT1600 + 336 + + + SubIndex 000 + + 00 + 10 + #x06 + + + + 1st Output Object to be mapped + + #x60400010 + + + + 2st Output Object to be mapped + + #x607A0020 + + + + 3st Output Object to be mapped + + #x60FF0020 + + + + 4st Output Object to be mapped + + #x60710010 + + + + 5st Output Object to be mapped + + #x60600008 + + + + 6st Output Object to be mapped + + #x00000000 + + + + 7st Output Object to be mapped + + #x00000000 + + + + 8st Output Object to be mapped + + #x00000000 + + + + 9st Output Object to be mapped + + #x00000000 + + + + 10st Output Object to be mapped + + #x00000000 + + + + + rw + o + + + + #x1601 + pp/pv/pt RxPDO + DT1601 + 336 + + + SubIndex 000 + + #x0A + + + + 1st Output Object to be mapped + + #x60400010 + + + + 2st Output Object to be mapped + + #x607A0020 + + + + 3st Output Object to be mapped + + #x60FF0020 + + + + 4st Output Object to be mapped + + #x60710010 + + + + 5st Output Object to be mapped + + #x60830020 + + + + 6st Output Object to be mapped + + #x60840020 + + + + 7st Output Object to be mapped + + #x60810020 + + + + 8st Output Object to be mapped + + #x60870010 + + + + 9st Output Object to be mapped + + #x60600008 + + + + 10st Output Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x1602 + pv RxPDO + DT1602 + 336 + + + SubIndex 000 + + #x06 + + + + 1st Output Object to be mapped + + #x60400010 + + + + 2st Output Object to be mapped + + #x60FF0020 + + + + 3st Output Object to be mapped + + #x60830020 + + + + 4st Output Object to be mapped + + #x60840020 + + + + 5st Output Object to be mapped + + #x60600008 + + + + 6st Output Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x1a00 + csp/csv/cst TxPDO + DT1A00 + 336 + + + SubIndex 000 + + 00 + 10 + #x07 + + + + 1st Input Object to be mapped + + #x60410010 + + + + 2st Input Object to be mapped + + #x60640020 + + + + 3st Input Object to be mapped + + #x606C0020 + + + + 4st Input Object to be mapped + + #x60770010 + + + + 5st Input Object to be mapped + + #x60610008 + + + + 6st Input Object to be mapped + + #x603F0010 + + + + 7st Input Object to be mapped + + #x00000000 + + + + 8st Input Object to be mapped + + #x00000000 + + + + 9st Input Object to be mapped + + #x00000000 + + + + 10st Input Object to be mapped + + #x00000000 + + + + + rw + o + + + + #x1a01 + pp/pv/pt TxPDO + DT1A01 + 336 + + + SubIndex 000 + + #x07 + + + + 1st Input Object to be mapped + + #x60410010 + + + + 2st Input Object to be mapped + + #x60640020 + + + + 3st Input Object to be mapped + + #x606C0020 + + + + 4st Input Object to be mapped + + #x60770020 + + + + 5st Input Object to be mapped + + #x60610008 + + + + 6st Input Object to be mapped + + #x603F0010 + + + + 7st Input Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x1a02 + pv TxPDO + DT1A02 + 336 + + + SubIndex 000 + + #x05 + + + + 1st Input Object to be mapped + + #x60410010 + + + + 2st Input Object to be mapped + + #x606C0020 + + + + 3st Input Object to be mapped + + #x60610008 + + + + 4st Input Object to be mapped + + #x603F0010 + + + + 5st Input Object to be mapped + + #x00000000 + + + + + ro + o + + + + #x2000 + Eu Motor SN + UDINT + 32 + + + Eu Motor SN + + 00 + + + + + rw + o + + + + #x2001 + Servo Config + DT2001 + 48 + + + SubIndex 000 + + 02 + + + + Eu Node Id + + 01 + + + + Eu Can BitRate + + 1000 + + + + + ro + o + + + + #x2002 + Motor Para + UDINT + 32 + + + Motor Para + + 00 + + + + + rw + o + + + + #x2003 + Soft Limit State + UDINT + 32 + + + Soft Limit State + + 00 + + + + + rw + o + + + + #x2004 + EuCommMode + USINT + 8 + + + EuCommMode + + 00 + + + + + rw + o + + + + #x2010 + Current Loop Pi + DT2010 + 80 + + + SubIndex 000 + + 04 + + + + Current Loop Kp Default + + 0040 + + + + Current Loop Ki Default + + 0001 + + + + Current Loop Kp + + 0004 + + + + Current Loop Ki + + 0001 + + + + + ro + o + + + + #x2012 + Velocity Loop Pi + DT2012 + 80 + + + SubIndex 000 + + 04 + + + + Velocity Loop Kp Default + + D007 + + + + Velocity Loop Ki Default + + 64 + + + + Velocity Loop Kp + + D007 + + + + Velocity Loop Ki + + 64 + + + + + ro + o + + + + #x2013 + Position Loop Pi + DT2013 + 80 + + + SubIndex 000 + + 04 + + + + Position Loop Kp Default + + D007 + + + + Position Loop Ki Default + + 64 + + + + Position Loop Kp + + D007 + + + + Position Loop Ki + + 64 + + + + + ro + o + + + + #x2014 + brake control + DT2014 + 64 + + + SubIndex 000 + + 03 + + + + EuBrakeControl + + 00 + + + + EuBrakeState + + 00 + + + + EuBrakeAutoState + + 01 + + + + + ro + o + + + + #x2015 + Eu Pwm Inv + USINT + 8 + + + Eu Pwm Inv + + 00 + + + + + ro + o + + + + #x2016 + Servo Temperture + DT2016 + 64 + + + SubIndex 000 + + 03 + + + + EuTemperature + + 19 + + + + EuHighTemperatureLimit + + 55 + + + + EuHighTemperatureWindowsTime + + B80B + + + + + ro + o + + + + #x2017 + EuVelocityIntLimit + UINT + 16 + + + EuVelocityIntLimit + + D007 + + + + + rw + o + + + + #x2020 + Motor Block + DT2020 + 80 + + + SubIndex 000 + + 03 + + + + EuMotorBlockTorque + + E803 + + + + EuMotorBlockTime + + B80B + + + + EuMotorBlockVelocity + + 1027 + + + + + ro + o + + + + #x2021 + velocity floowing error + DT2021 + 64 + + + SubIndex 000 + + 02 + + + + EuVolecityFollowingErrorWindows + + A08601 + + + + EuVolecityFollowingErrorTime + + 0BB8 + + + + + ro + o + + + + #x2022 + torque window + UINT + 16 + + + torque window + + 0A + + + + + rw + o + + + + #x2023 + Torque window time + UINT + 16 + + + Torque window time + + 01 + + + + + rw + o + + + + #x2024 + EuOverSpeedThreshold + UDINT + 32 + + + EuOverSpeedThreshold + + AAAA42 + + + + + rw + o + + + + #x2025 + EuOverSpeedTime + UINT + 16 + + + EuOverSpeedTime + + 64 + + + + + rw + o + + + + #x2026 + EuBrakeDelayTime + USINT + 8 + + + EuBrakeDelayTime + + 64 + + + + + rw + o + + + + #x2027 + EuAutoMagnetAngleAlignmentFlag + UDINT + 32 + + + EuAutoMagnetAngleAlignmentFlag + + 00 + + + + + rw + o + + + + #x2028 + EuI2tLimit + UINT + 16 + + + EuI2tLimit + + 64 + + + + + rw + o + + + + #x2029 + EuI2tValue + UINT + 16 + + + EuI2tValue + + 00 + + + + + ro + o + + + + #x202A + EuFirstEncoderValue + DINT + 32 + + + EuFirstEncoderValue + + 00 + + + + + ro + o + + + + #x202B + EuSecondEncoderValue + DINT + 32 + + + EuSecondEncoderValue + + 00 + + + + + ro + o + + + + #x202C + EuThetaBias + DINT + 32 + + + EuThetaBias + + 00 + + + + + ro + o + + + + #x202D + Eu Voltage Threshold + DT202D + 144 + + + SubIndex 000 + + 04 + + + + EuUnderVoltageThreshold + + 283707 + + + + EuUnderVoltageTime + + 05 + + + + EuOverVoltageThreshold + + A00901 + + + + EuOverVoltageTime + + 05 + + + + + ro + o + + + + #x202E + Eu Velocity Feedforward + DT202E + 128 + + + SubIndex 000 + + 05 + + + + EuVelocityFeedforwardControlSelect + + 01 + + + + EuVelocityFeedforwardFilterTimeConst + + 00 + + + + EuVelocityFeedforwardGain + + 00 + + + + EuVelocityFeedforwardOriginalValue + + 00 + + + + EuVelocityFeedforwardFilteredValue + + 00 + + + + + ro + o + + + + #x2100 + EuDisableFpgaMu150Spi + DINT + 32 + + + EuDisableFpgaMu150Spi + + 00 + + + + + rw + o + + + + #x2104 + ServoMagnetAngleAlignment + UDINT + 32 + + + ServoMagnetAngleAlignment + + 00 + + + + + rw + o + + + + #x2109 + EuGoToBoot + DINT + 32 + + + EuGoToBoot + + 00 + + + + + rw + o + + + + #x210A + EuCompStart + DINT + 32 + + + EuCompStart + + 00 + + + + + rw + o + + + + #x210B + EuCompEn + DINT + 32 + + + EuCompEn + + 00 + + + + + rw + o + + + + #x210C + EuCompBias + DINT + 32 + + + EuCompBias + + 00 + + + + + rw + o + + + + #x210D + EuCompState + DINT + 32 + + + EuCompState + + 00 + + + + + rw + o + + + + #x210E + EuFocCurBiasSet + USINT + 8 + + + EuFocCurBiasSet + + 00 + + + + + rw + o + + + + #x210F + Foc Cur Bias + DT210F + 144 + + + SubIndex 000 + + 03 + + + + FocCurBiasA + + 00 + + + + FocCurBiasB + + 00 + + + + FocCurBiasC + + 00 + + + + + ro + o + + + + #x2110 + EuTorqueFactor + UINT + 16 + + + EuTorqueFactor + + E803 + + + + + rw + o + + + + #x2201 + EuSysOutPulseStep + UDINT + 32 + + + EuSysOutPulseStep + + 00 + + + + + rw + o + + + + #x2301 + Position Velocity Mix Position Kp + UINT + 16 + + + Position Velocity Mix Position Kp + + 00 + + + + + rw + o + + + + #x2302 + Position Velocity Mix Velocity Kd + UINT + 16 + + + Position Velocity Mix Velocity Kd + + 00 + + + + + rw + o + + + + #x603F + Error Code + UINT + 16 + + + Error Code + + 00 + + + + + ro + o + T + + + + #x6040 + Control Word + UINT + 16 + + + Control Word + + 00 + + + + + rw + o + R + + + + #x6041 + Status Word + UINT + 16 + + + Status Word + + 00 + + + + + ro + o + T + + + + #x605A + Quickstop Option Code + INT + 16 + + + Quickstop Option Code + + 02 + + + + + rw + o + + + + #x605B + Shutdown Option Code + INT + 16 + + + Shutdown Option Code + + 00 + + + + + rw + o + + + + #x605C + Disable Operation Option Code + INT + 16 + + + Disable Operation Option Code + + 01 + + + + + rw + o + + + + #x605D + Halt option code + INT + 16 + + + Halt option code + + 01 + + + + + rw + o + + + + #x605E + Fault Reaction Code + INT + 16 + + + Fault Reaction Code + + 02 + + + + + rw + o + + + + #x6060 + Modes of Operation + SINT + 8 + + + Modes of Operation + + 00 + + + + + rw + o + R + + + + #x6061 + Modes of Operation Display + SINT + 8 + + + Modes of Operation Display + + 00 + + + + + ro + o + T + + + + #x6062 + Position demannd value + DINT + 32 + + + Position demannd value + + 00 + + + + + ro + o + T + + + + #x6064 + Position Actual Value + DINT + 32 + + + Position Actual Value + + 00 + + + + + ro + o + T + + + + #x6065 + Maximal following error + UDINT + 32 + + + Maximal following error + + E09304 + + + + + rw + o + + + + #x6067 + Position window + UDINT + 32 + + + Position window + + 64 + + + + + rw + o + + + + #x6068 + Position window time + UINT + 16 + + + Position window time + + 01 + + + + + rw + o + + + + #x606B + Velocity demand value + DINT + 32 + + + Velocity demand value + + 00 + + + + + ro + o + T + + + + #x606C + Velocity Actual Value + DINT + 32 + + + Velocity Actual Value + + 00 + + + + + ro + o + T + + + + #x606D + Velocity window + UINT + 16 + + + Velocity window + + 1027 + + + + + ro + o + + + + #x606E + Velocity window time + UINT + 16 + + + Velocity window time + + 01 + + + + + ro + o + + + + #x606F + Velocity threshold + UINT + 16 + + + Velocity threshold + + E803 + + + + + ro + o + + + + #x6070 + Velocity threshold time + UINT + 16 + + + Velocity threshold time + + 01 + + + + + ro + o + + + + #x6071 + Target Torque + INT + 16 + + + Target Torque + + 00 + + + + + rw + o + + + + #x6072 + Max Torque + UINT + 16 + + + Max Torque + + D007 + + + + + rw + o + + + + #x6074 + Torque demand value + INT + 16 + + + Torque Actual Value + + 00 + + + + + ro + o + T + + + + #x6076 + Motor rated torque + UDINT + 32 + + + Motor rated torque + + 00 + + + + + rw + o + + + + #x6077 + Torque Actual Value + INT + 16 + + + Torque Actual Value + + 00 + + + + + ro + o + T + + + + #x6078 + Current actual value + INT + 16 + + + Current actual value + + 00 + + + + + ro + o + T + + + + #x6079 + DC link circuit voltage + UDINT + 32 + + + DC link circuit voltage + + 00 + + + + + rw + o + + + + #x607A + Target Position + DINT + 32 + + + Target Position + + 00 + + + + + rw + o + R + + + + #x607C + Home Offset + DINT + 32 + + + Home Offset + + 00 + + + + + rw + o + + + + #x607D + Software Position Limit + DT607D + 80 + + + SubIndex 000 + + 02 + + + + Min position limit + + 006CCA88 + + + + Max position limit + + 00943577 + + + + + ro + o + + + + #x607F + Max Profile Velocity + UDINT + 32 + + + Max Profile Velocity + + 00 + + + + + rw + o + + + + #x6081 + Profile Velocity + UDINT + 32 + + + Profile Velocity + + 00 + + + + + rw + o + R + + + + #x6083 + Profile Acceleration + UDINT + 32 + + + Profile Acceleration + + 00 + + + + + rw + o + R + + + + #x6084 + Profile Deceleration + UDINT + 32 + + + Profile Deceleration + + 00 + + + + + rw + o + R + + + + #x6085 + Quickstop Declaration + DINT + 32 + + + Quickstop Declaration + + 00 + + + + + rw + o + + + + #x6087 + Torque slope + UDINT + 32 + + + Torque slope + + E803 + + + + + rw + o + + + + #x6091 + Gear ratio + DT6091 + 80 + + + SubIndex 000 + + 02 + + + + Motor revolutions + + 000051 + + + + Shaft revolutions + + 000051 + + + + + ro + o + + + + #x60B1 + Velocity Offset + DINT + 32 + + + Velocity Offset + + 00 + + + + + rw + o + + + + #x60B2 + Torque Offset + INT + 16 + + + Torque Offset + + 00 + + + + + rw + o + + + + #x60C1 + Interpolation Data Record + DT60C1 + 48 + + + SubIndex 000 + + 01 + + + + Interpolation data record + + 00 + + + + + ro + o + + + + #x60C2 + Interpolation Time Period + DT60C2 + 32 + + + SubIndex 000 + + 02 + + + + Interpolation period + + 01 + + + + Interpolation Index + + -3 + + + + + ro + o + + + + #x60F4 + Following error actual value + DINT + 32 + + + Following error actual value + + 00 + + + + + ro + o + T + + + + #x60FF + Target Velocity + DINT + 32 + + + Target Velocity + + 00 + + + + + rw + o + R + + + + #x6502 + Supported Drive Modes + UDINT + 32 + + + Supported Drive Modes + + 0001 + + + + + ro + o + + + + #xf000 + Modular device profile + DTF000 + 48 + + + SubIndex 000 + + 02 + + + + Module index distance + + 2003 + + + + Maximum number of modules + + 02 + + + + + ro + o + + + + #xf010 + Module profile list + DTF010 + 80 + + + SubIndex 000 + + 02 + + + + SubIndex 001 + + 92010200 + + + + SubIndex 002 + + 00000000 + + + + + ro + o + + + + #xf030 + Configured module Ident list + DTF030 + 80 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 00983100 + + + + SubIndex 002 + + 00000000 + + + + + ro + o + + + + #xf050 + Module detected list + DTF050 + 80 + + + SubIndex 000 + + 01 + + + + SubIndex 001 + + 00983100 + + + + SubIndex 002 + + 00000000 + + + + + ro + o + + + + + + Outputs + Inputs + MBoxState + MBoxOut + MBoxIn + Outputs + Inputs + + #x1600 + Outputs + + #x6040 + 0 + 16 + Control Word + object 0x6040:0 + UINT + + + #x607A + 0 + 32 + Target Position + object 0x607A:0 + DINT + + + #x60FF + 0 + 32 + Target Velocity + object 0x60FF:0 + DINT + + + #x6071 + 0 + 16 + Target Torque + object 0x6071:0 + INT + + + #x6060 + 0 + 8 + Mode Of Operation + object 0x6060:0 + SINT + + + #x0 + 0 + 8 + + + + #x1601 + Outputs + + #x6040 + 0 + 16 + Control Word + object 0x6040:0 + UINT + + + #x607A + 0 + 32 + Target Position + object 0x607A:0 + DINT + + + #x60FF + 0 + 32 + Target Velocity + object 0x60FF:0 + DINT + + + #x6071 + 0 + 16 + Target Torque + object 0x6071:0 + INT + + + #x6083 + 0 + 32 + Profile Acceleration + object 0x6083:0 + UDINT + + + #x6084 + 0 + 32 + Profile Deceleration + object 0x6084:0 + UDINT + + + #x6081 + 0 + 32 + Profile Velocity + object 0x6081:0 + UDINT + + + #x6087 + 0 + 32 + Torque Slope + object 0x6087:0 + UDINT + + + #x6060 + 0 + 8 + Modes Of Operation + object 0x6060:0 + SINT + + + #x0 + 0 + 8 + + + + #x1602 + Outputs + + #x6040 + 0 + 16 + Control Word + object 0x6040:0 + UINT + + + #x60FF + 0 + 32 + Target Velocity + object 0x60FF:0 + DINT + + + #x6083 + 0 + 32 + Profile Acceleration + object 0x6083:0 + UDINT + + + #x6084 + 0 + 32 + Profile Deceleration + object 0x6084:0 + UDINT + + + #x6060 + 0 + 8 + Modes Of Operation + object 0x6060:0 + SINT + + + #x0 + 0 + 8 + + + + #x1A00 + Inputs + + #x6041 + 0 + 16 + Status Word + object 0x6041:0 + UINT + + + #x6064 + 0 + 32 + Actual Position + object 0x6064:0 + DINT + + + #x606C + 0 + 32 + Actual Velocity + object 0x606C:0 + DINT + + + #x6077 + 0 + 16 + Actual Torque + object 0x6077:0 + INT + + + #x6061 + 0 + 8 + Mode Of OperationDisplay + object 0x6061:0 + SINT + + + #x603F + 0 + 16 + Error Code + object 0x603F:0 + UINT + + + #x0 + 0 + 8 + + + + #x1A01 + Inputs + + #x6041 + 0 + 16 + Status Word + object 0x6041:0 + UINT + + + #x6064 + 0 + 32 + Actual Position + object 0x6064:0 + DINT + + + #x606C + 0 + 32 + Actual Velocity + object 0x606C:0 + DINT + + + #x6077 + 0 + 16 + Actual Torque + object 0x6077:0 + INT + + + #x6061 + 0 + 8 + Mode Of OperationDisplay + object 0x6061:0 + SINT + + + #x603F + 0 + 16 + Error Code + object 0x603F:0 + UINT + + + #x0 + 0 + 8 + + + + #x1A02 + Inputs + + #x6041 + 0 + 16 + Status Word + object 0x6041:0 + UINT + + + #x606C + 0 + 32 + Actual Velocity + object 0x606C:0 + DINT + + + #x6061 + 0 + 8 + Mode Of OperationDisplay + object 0x6061:0 + SINT + + + #x603F + 0 + 16 + Error Code + object 0x603F:0 + UINT + + + #x0 + 0 + 8 + + + + + + + + Synchron + SM-Synchron + #x0 + 0 + 0 + 0 + + + DC + DC-Synchron + #x300 + 0 + 0 + 0 + + + + 2048 + 800E00CC8813f000000000800000 + + 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 + + + + diff --git a/docs/ethercat_motor_tutorial.md b/docs/ethercat_motor_tutorial.md index 5720c0bc..2ba49119 100644 --- a/docs/ethercat_motor_tutorial.md +++ b/docs/ethercat_motor_tutorial.md @@ -1,25 +1,51 @@ # EtherCAT 电机接入教程 -这份文档只说明新增一种 EtherCAT 电机需要改哪里、怎么写。 +当前已接入意优 `EYOU_ServoModule_ECAT_V145`，协议为 EtherCAT CoE + CiA402，PDO 使用 `docs/EYOU_ServoModule_ECAT_V145_no_slot.xml` 中的 `0x1600/0x1A00` 映射。 -## 1. 增加 vendor +ESI XML 作为厂商通信说明和对照资料保存，运行时不直接解析 XML。实际 PDO 映射写在： -修改 `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; -} +```text +cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h ``` -`MOTOR_VENDOR_XXX` 改成真实厂商名，例如 `MOTOR_VENDOR_FOO`。不要复用 `TI5`。 +## 1. 系统依赖 -## 2. 写电机配置 +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 @@ -34,20 +60,28 @@ motor { motor_groups { id: "right_arm_ethercat" bus_type: MOTOR_BUS_ETHERCAT - vendor: MOTOR_VENDOR_XXX + vendor: MOTOR_VENDOR_EYOU protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402 - tool_frame: "R_FINGER_TIP" ethercat { - master_id: "eth0" + master_index: 0 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 } + + 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 + } + + 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 { @@ -63,21 +97,26 @@ motor { } 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 { 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 } } } } ``` -`motors.motors.id` 是系统内的电机逻辑 id。`ethercat.slaves.motor_id` 必须和它对应。 +字段说明： -`joint_limits` 按 `joint_name` 读取，和 CAN、MuJoCo 电机保持同一个风格。 +- `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. 注册设备 @@ -98,15 +137,9 @@ devices { } ``` -## 4. 机械臂使用 EtherCAT group +## 4. 机械臂使用 EtherCAT 电机 -修改机械臂配置，例如： - -```text -cmvr-es/config/devices/arm/arm.pb.txt -``` - -把 motor backend 改成： +修改机械臂配置中的 motor backend： ```proto motor { @@ -122,116 +155,66 @@ motor { } ``` -## 5. 实现 bus runtime +## 5. 代码结构 -EtherCAT 总线资源放在： +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` 负责： +它负责： + +- 打开 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 -读取 ethercat 配置 -初始化 EtherCAT master -扫描/校验 slave_index、vendor_id、product_code -启动 cyclic loop -保存 command/feedback buffer -停止 cyclic loop +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 ``` -bus runtime 不创建具体电机，也不关心厂商；它只保存总线连接、线程和数据缓存。 - -## 6. 增加具体电机 driver - -新增目录： +EYOU 私有适配： ```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 +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 ``` -`XxxEthercatMotor` 继承 `AbstractMotor`。 +它负责： -`XxxEthercatMotorProtocol` 继承 `MotorProtocolInterface`，把 `setTarget`、`setQd`、`getQ` 等接口转换成 EtherCAT command/feedback。 +- CiA402 `6040/6041` 状态机 +- 设置 `6060` 运行模式 +- 写 `607A/60FF/6071` +- 读 `6064/606C/6077/603F` +- `rad` 和 encoder count 的换算 -## 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(bus_runtime); - -if (group_cfg.vendor() == config::MOTOR_VENDOR_XXX && - group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) { - auto protocol = std::make_shared(ethercat_bus_runtime); - - std::vector> motors; - motors.reserve(motor_cfgs.size()); - for (const auto& cfg : motor_cfgs) { - auto motor = std::make_shared(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： +## 6. 编译验证 ```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 +cmake -S . -B cmake-build-debug +cmake --build cmake-build-debug --target motor_manager ``` -再编译： +真机验证顺序： ```bash -cmake --build cmake-build-debug --target mujoco_manual_ui_test +sudo script/ethercat/start_ethercat.sh eno1 +script/ethercat/status_ethercat.sh +cmake --build cmake-build-debug --target cmvr_es ``` -真机联调时先只验证初始化日志：master 打开、slave 数量、vendor/product 校验、cyclic loop 启动、每个 motor 注册成功。然后再下发运动命令。 +第一次联调先不要大幅度运动。先看 master 是否打开、slave 是否进入 OP、状态字是否更新，再给单个电机小角度目标。 diff --git a/docs/意优CANopen&EtherCAT应用手册V2.2.pdf b/docs/意优CANopen&EtherCAT应用手册V2.2.pdf new file mode 100644 index 00000000..e1c3df2a Binary files /dev/null and b/docs/意优CANopen&EtherCAT应用手册V2.2.pdf differ diff --git a/protos/cmvr/config/device_manager_config/device_manager_config.proto b/protos/cmvr/config/device_manager_config/device_manager_config.proto index c238d2bf..d36d328b 100644 --- a/protos/cmvr/config/device_manager_config/device_manager_config.proto +++ b/protos/cmvr/config/device_manager_config/device_manager_config.proto @@ -34,6 +34,7 @@ message DeviceManagerConfig { string version = 2; string description = 3; repeated DeviceConfigEntry devices = 4; + bool init_all_motors_when_no_active_joints = 20; } message DeviceManagerRootConfig { DeviceManagerConfig device_manager = 1; diff --git a/protos/cmvr/config/motor_config/motor_config.proto b/protos/cmvr/config/motor_config/motor_config.proto index e6d88215..c31a4d8a 100644 --- a/protos/cmvr/config/motor_config/motor_config.proto +++ b/protos/cmvr/config/motor_config/motor_config.proto @@ -10,6 +10,8 @@ message MotorConfigItem { double limit_q_ub = 4; double limit_qd = 5; double limit_qdd = 6; + double encoder_counts_per_rev = 7; + double gear_ratio = 8; } message MotorList { @@ -18,9 +20,26 @@ message MotorList { message EthercatSlaveConfig { int32 motor_id = 1; - int32 slave_index = 2; - uint32 vendor_id = 3; - uint32 product_code = 4; + uint32 alias = 2; + uint32 position = 3; +} + +message Cia402ProtocolConfig { + uint32 profile_position_trigger_delay_ms = 1; + uint32 state_transition_timeout_ms = 2; + uint32 velocity_stop_timeout_ms = 3; + uint32 status_poll_period_ms = 4; + double stopped_velocity_tolerance_rad_s = 5; +} + +message EtherCATDcConfig { + optional bool enable = 1; + optional int32 reference_motor_id = 2; + optional uint32 sync0_cycle_us = 3; + optional int32 sync0_shift_us = 4; + optional uint32 sync_reference_clock_period = 5; + optional uint32 assign_activate = 6; + optional uint32 sync_monitor_period_ms = 7; } message SocketCanConfig { @@ -29,8 +48,12 @@ message SocketCanConfig { } message EtherCATConfig { - string master_id = 1; + uint32 master_index = 1; int32 cycle_us = 2; + Cia402ProtocolConfig cia402 = 3; + EtherCATDcConfig dc = 4; + optional uint32 slave_op_timeout_ms = 5; + optional uint32 slave_state_poll_period_ms = 6; repeated EthercatSlaveConfig slaves = 10; } @@ -49,6 +72,7 @@ enum MotorVendor { MOTOR_VENDOR_UNKNOWN = 0; MOTOR_VENDOR_TI5 = 1; MOTOR_VENDOR_MUJOCO = 2; + MOTOR_VENDOR_EYOU = 3; } enum MotorProtocol { @@ -63,7 +87,6 @@ message MotorGroupConfig { MotorBusType bus_type = 2; MotorVendor vendor = 3; MotorProtocol protocol = 4; - string tool_frame = 5; oneof bus_config { SocketCanConfig can = 10; diff --git a/protos/cmvr/msgs/canopen.proto b/protos/cmvr/msgs/canopen.proto index 23914e79..14ab3279 100644 --- a/protos/cmvr/msgs/canopen.proto +++ b/protos/cmvr/msgs/canopen.proto @@ -5,8 +5,8 @@ package cmvr.msgs; message SdoFrame { uint32 node_id = 1; // 节点ID CommandSpecifier cs = 2; // SDO命令字 - ObIndex index = 3; // 对象字典索引 - ObSubIndex sub_index = 4; // 子索引 + uint32 index = 3; // 对象字典索引 + uint32 sub_index = 4; // 子索引 uint32 data = 5; // 数据区 } @@ -110,81 +110,38 @@ enum NmtCommand { } -// 索引 -enum ObIndex { - INDEX_ZERO = 0; - USER_SAVE_PARA_2000 = 0x2000; // 下发命令 1 保存参数 - POSITION_OFFSET_2008 = 0x2008; // 位置偏移，子索引 0x00，用于设置零点、起始位置 - // Error Codes - ERROR_CODE_6007 = 0x6007; - ERROR_CODE_603F = 0x603F; +// CANopen communication object dictionary indexes. +// CiA402 drive-profile objects are defined in cia402.proto. +enum CanopenObjectIndex { + CANOPEN_OBJECT_INDEX_ZERO = 0; - // Control and Status - CONTROL_WORD_6040 = 0x6040; - STATUS_WORD_6041 = 0x6041; + CANOPEN_PRODUCER_HEARTBEAT_TIME_1017 = 0x1017; - // Operation Modes - OPERATION_MODE_6060 = 0x6060; - MODE_DISPLAY_6061 = 0x6061; - - // Actual Values - ACTUAL_POSITION_6064 = 0x6064; - ACTUAL_SPEED_606C = 0x606C; - ACTUAL_CURRENT_6078 = 0x6078; - - // Torque-related - TARGET_TORQUE_6071 = 0x6071; - MAX_TORQUE_6072 = 0x6072; - DEMAND_TORQUE_6074 = 0x6074; - - // Position-related - TARGET_POSITION_607A = 0x607A; - SOFTWARE_POSITION_LIMIT_607D = 0x607D; // Sub-indexes: 1, 2 - - // Speed-related - MAX_SPEED_607F = 0x607F; - PROFILE_SPEED_6081 = 0x6081; - PROFILE_ACCELERATION_6083 = 0x6083; - PROFILE_DECELERATION_6084 = 0x6084; - - // Same as DEMAND_TORQUE? Verify correctness. - // TORQUE_SLOPE_6074 = 0x6074; - - // PID Control - CURRENT_LOOP_PID_60F6 = 0x60F6; // Sub-indexes: 1, 2 - SPEED_LOOP_PID_60F9 = 0x60F9; // Sub-indexes: 1, 2 - POSITION_LOOP_PID_60FB = 0x60FB; // Sub-indexes: 1, 2, 3 - - // Target Speed - TARGET_SPEED_60FF = 0x60FF; - - QUICK_STOP_OPTION_605A = 0x605A; - QUICK_STOP_DECEL_6085 = 0x6085; - - // ------------------------- // PDO 通信参数对象（Communication Object） - RPDO1_COMM_1400 = 0x1400; - RPDO2_COMM_1401 = 0x1401; - RPDO3_COMM_1402 = 0x1402; - RPDO4_COMM_1403 = 0x1403; + CANOPEN_RPDO1_COMM_1400 = 0x1400; + CANOPEN_RPDO2_COMM_1401 = 0x1401; + CANOPEN_RPDO3_COMM_1402 = 0x1402; + CANOPEN_RPDO4_COMM_1403 = 0x1403; - TPDO1_COMM_1800 = 0x1800; - TPDO2_COMM_1801 = 0x1801; - TPDO3_COMM_1802 = 0x1802; - TPDO4_COMM_1803 = 0x1803; + CANOPEN_TPDO1_COMM_1800 = 0x1800; + CANOPEN_TPDO2_COMM_1801 = 0x1801; + CANOPEN_TPDO3_COMM_1802 = 0x1802; + CANOPEN_TPDO4_COMM_1803 = 0x1803; // PDO 映射对象（Mapping Object） - RPDO1_MAP_1600 = 0x1600; - RPDO2_MAP_1601 = 0x1601; - RPDO3_MAP_1602 = 0x1602; - RPDO4_MAP_1603 = 0x1603; + CANOPEN_RPDO1_MAP_1600 = 0x1600; + CANOPEN_RPDO2_MAP_1601 = 0x1601; + CANOPEN_RPDO3_MAP_1602 = 0x1602; + CANOPEN_RPDO4_MAP_1603 = 0x1603; - TPDO1_MAP_1A00 = 0x1A00; - TPDO2_MAP_1A01 = 0x1A01; - TPDO3_MAP_1A02 = 0x1A02; - TPDO4_MAP_1A03 = 0x1A03; + CANOPEN_TPDO1_MAP_1A00 = 0x1A00; + CANOPEN_TPDO2_MAP_1A01 = 0x1A01; + CANOPEN_TPDO3_MAP_1A02 = 0x1A02; + CANOPEN_TPDO4_MAP_1A03 = 0x1A03; - PRODUCER_HEARTBEAT_TIME = 0x1017; + // Ti5 vendor-specific objects used through CANopen SDO. + CANOPEN_USER_SAVE_PARA_2000 = 0x2000; + CANOPEN_POSITION_OFFSET_2008 = 0x2008; } // 子索引 @@ -198,5 +155,3 @@ enum ObSubIndex { SUB_INDEX_6 = 6; SUB_INDEX_7 = 7; } - - diff --git a/protos/cmvr/msgs/cia402.proto b/protos/cmvr/msgs/cia402.proto new file mode 100644 index 00000000..96e46c74 --- /dev/null +++ b/protos/cmvr/msgs/cia402.proto @@ -0,0 +1,64 @@ +syntax = "proto3"; + +package cmvr.msgs; + +// CiA402 object dictionary indexes shared by CANopen and EtherCAT CoE drives. +enum Cia402ObjectIndex { + CIA402_OBJECT_INDEX_ZERO = 0; + + CIA402_ERROR_CODE_603F = 0x603F; + + CIA402_CONTROL_WORD_6040 = 0x6040; + CIA402_STATUS_WORD_6041 = 0x6041; + + CIA402_QUICK_STOP_OPTION_605A = 0x605A; + CIA402_SHUTDOWN_OPTION_605B = 0x605B; + CIA402_DISABLE_OPERATION_OPTION_605C = 0x605C; + CIA402_HALT_OPTION_605D = 0x605D; + CIA402_FAULT_REACTION_OPTION_605E = 0x605E; + + CIA402_OPERATION_MODE_6060 = 0x6060; + CIA402_MODE_DISPLAY_6061 = 0x6061; + + CIA402_POSITION_DEMAND_VALUE_6062 = 0x6062; + CIA402_ACTUAL_POSITION_6064 = 0x6064; + CIA402_MAX_FOLLOWING_ERROR_6065 = 0x6065; + CIA402_POSITION_WINDOW_6067 = 0x6067; + CIA402_POSITION_WINDOW_TIME_6068 = 0x6068; + + CIA402_VELOCITY_DEMAND_VALUE_606B = 0x606B; + CIA402_ACTUAL_VELOCITY_606C = 0x606C; + CIA402_VELOCITY_WINDOW_606D = 0x606D; + CIA402_VELOCITY_WINDOW_TIME_606E = 0x606E; + CIA402_VELOCITY_THRESHOLD_606F = 0x606F; + CIA402_VELOCITY_THRESHOLD_TIME_6070 = 0x6070; + + CIA402_TARGET_TORQUE_6071 = 0x6071; + CIA402_MAX_TORQUE_6072 = 0x6072; + CIA402_TORQUE_DEMAND_VALUE_6074 = 0x6074; + CIA402_MOTOR_RATED_TORQUE_6076 = 0x6076; + CIA402_ACTUAL_TORQUE_6077 = 0x6077; + CIA402_ACTUAL_CURRENT_6078 = 0x6078; + CIA402_DC_LINK_VOLTAGE_6079 = 0x6079; + + CIA402_TARGET_POSITION_607A = 0x607A; + CIA402_HOME_OFFSET_607C = 0x607C; + CIA402_SOFTWARE_POSITION_LIMIT_607D = 0x607D; + CIA402_MAX_PROFILE_VELOCITY_607F = 0x607F; + + CIA402_PROFILE_VELOCITY_6081 = 0x6081; + CIA402_PROFILE_ACCELERATION_6083 = 0x6083; + CIA402_PROFILE_DECELERATION_6084 = 0x6084; + CIA402_QUICK_STOP_DECELERATION_6085 = 0x6085; + CIA402_TORQUE_SLOPE_6087 = 0x6087; + + CIA402_GEAR_RATIO_6091 = 0x6091; + CIA402_VELOCITY_OFFSET_60B1 = 0x60B1; + CIA402_TORQUE_OFFSET_60B2 = 0x60B2; + CIA402_INTERPOLATION_DATA_RECORD_60C1 = 0x60C1; + CIA402_INTERPOLATION_TIME_PERIOD_60C2 = 0x60C2; + CIA402_FOLLOWING_ERROR_ACTUAL_VALUE_60F4 = 0x60F4; + CIA402_TARGET_VELOCITY_60FF = 0x60FF; + + CIA402_SUPPORTED_DRIVE_MODES_6502 = 0x6502; +} diff --git a/protos/cmvr/msgs/motor.proto b/protos/cmvr/msgs/motor.proto index a0336dea..55cda865 100644 --- a/protos/cmvr/msgs/motor.proto +++ b/protos/cmvr/msgs/motor.proto @@ -114,5 +114,3 @@ message MotorStatus { uint32 status_word = 44; } - - diff --git a/protos/cmvr/msgs/robot_detail.proto b/protos/cmvr/msgs/robot_detail.proto index 19a6595b..375fdfee 100644 --- a/protos/cmvr/msgs/robot_detail.proto +++ b/protos/cmvr/msgs/robot_detail.proto @@ -1,6 +1,5 @@ syntax = "proto3"; -import "cmvr/msgs/canopen.proto"; import "cmvr/msgs/motor.proto"; package cmvr.msgs; diff --git a/request.txt b/request.txt index 8642cc9a..f958bcfe 100644 --- a/request.txt +++ b/request.txt @@ -34,5 +34,6 @@ third_party/visp/3.7.0 third_party/mainif/0.0.5 third_party/matplotplusplus/1.2.0 third_party/huayan_robot/v1.0 +third_party/ethercat/v1.7.0 diff --git a/script/ethercat/start_ethercat.sh b/script/ethercat/start_ethercat.sh new file mode 100755 index 00000000..269aef67 --- /dev/null +++ b/script/ethercat/start_ethercat.sh @@ -0,0 +1,133 @@ +#!/usr/bin/env bash +set -euo pipefail + +usage() { + cat <<'USAGE' +Usage: + sudo script/ethercat/start_ethercat.sh [iface] [ethercat_dev] [start_wait_sec] + Any pure numeric argument is treated as start_wait_sec. + +Example: + sudo script/ethercat/start_ethercat.sh eno1 + sudo script/ethercat/start_ethercat.sh 10 + sudo script/ethercat/start_ethercat.sh eno1 10 + sudo script/ethercat/start_ethercat.sh eno1 /dev/EtherCAT1 + sudo script/ethercat/start_ethercat.sh eno1 /dev/EtherCAT0 10 + sudo script/ethercat/start_ethercat.sh eno1 10 /dev/EtherCAT0 + +Environment: + DEVICE_MODULES=generic IgH device module list. + IGH_ROOT=... Override bundled IgH install path. + ETHERCAT_CONF=... Override ethercatctl config path. + ETHERCAT_DEV=/dev/EtherCAT0 Override EtherCAT character device node. + START_WAIT_SEC=5 Seconds to wait for link/slave discovery. +USAGE +} + +if [[ "${1:-}" == "-h" || "${1:-}" == "--help" ]]; then + usage + exit 0 +fi + +if [[ "$(id -u)" -ne 0 ]]; then + echo "error: please run with sudo." >&2 + exit 1 +fi + +IFACE="${IFACE:-eno1}" +ETHERCAT_DEV="${ETHERCAT_DEV:-/dev/EtherCAT0}" +START_WAIT_SEC="${START_WAIT_SEC:-5}" + +NON_NUMERIC_ARG_COUNT=0 +for arg in "$@"; do + if [[ "${arg}" =~ ^[0-9]+$ ]]; then + START_WAIT_SEC="${arg}" + continue + fi + + case "${NON_NUMERIC_ARG_COUNT}" in + 0) + IFACE="${arg}" + ;; + 1) + ETHERCAT_DEV="${arg}" + ;; + *) + echo "error: unexpected argument: ${arg}" >&2 + usage >&2 + exit 1 + ;; + esac + NON_NUMERIC_ARG_COUNT=$((NON_NUMERIC_ARG_COUNT + 1)) +done +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)" +IGH_ROOT="${IGH_ROOT:-${REPO_ROOT}/dependency/x86/third_party/ethercat/v1.7.0}" +DEVICE_MODULES="${DEVICE_MODULES:-generic}" +ETHERCAT_CONF="${ETHERCAT_CONF:-${IGH_ROOT}/etc/ethercat.conf}" +ETHERCATCTL="${IGH_ROOT}/sbin/ethercatctl" +ETHERCAT="${IGH_ROOT}/bin/ethercat" + +if [[ ! -d "/sys/class/net/${IFACE}" ]]; then + echo "error: network interface '${IFACE}' does not exist." >&2 + exit 1 +fi + +if [[ ! -x "${ETHERCATCTL}" ]]; then + echo "error: ethercatctl not found: ${ETHERCATCTL}" >&2 + exit 1 +fi + +if [[ ! -x "${ETHERCAT}" ]]; then + echo "error: ethercat command not found: ${ETHERCAT}" >&2 + exit 1 +fi + +MAC="$(cat "/sys/class/net/${IFACE}/address")" + +echo "EtherCAT interface: ${IFACE}" +echo "EtherCAT MAC: ${MAC}" +echo "EtherCAT device: ${ETHERCAT_DEV}" +echo "IgH root: ${IGH_ROOT}" +echo "IgH config: ${ETHERCAT_CONF}" +echo "Device modules: ${DEVICE_MODULES}" +echo "Start wait: ${START_WAIT_SEC}s" + +mkdir -p "$(dirname "${ETHERCAT_CONF}")" + +if [[ -f "${ETHERCAT_CONF}" ]]; then + echo "Stopping existing EtherCAT master with current config..." + "${ETHERCATCTL}" -c "${ETHERCAT_CONF}" stop >/dev/null 2>&1 || true + sleep 1 +fi + +cat >"${ETHERCAT_CONF}" </dev/null 2>&1; then + nmcli device disconnect "${IFACE}" >/dev/null 2>&1 || true +fi + +ip addr flush dev "${IFACE}" +ip link set "${IFACE}" up + +"${ETHERCATCTL}" -c "${ETHERCAT_CONF}" start + +if [[ -e "${ETHERCAT_DEV}" ]]; then + chmod 666 "${ETHERCAT_DEV}" +else + echo "warning: ${ETHERCAT_DEV} not found; skip chmod. Check with: ls -l /dev/EtherCAT*" >&2 +fi + +for ((i = 0; i < START_WAIT_SEC; ++i)); do + if "${ETHERCAT}" slaves 2>/dev/null | grep -qE '^[0-9]+[[:space:]]'; then + break + fi + sleep 1 +done + +"${ETHERCAT}" master +"${ETHERCAT}" slaves || true diff --git a/script/ethercat/status_ethercat.sh b/script/ethercat/status_ethercat.sh new file mode 100755 index 00000000..0533b0fb --- /dev/null +++ b/script/ethercat/status_ethercat.sh @@ -0,0 +1,62 @@ +#!/usr/bin/env bash +set -euo pipefail + +usage() { + cat <<'USAGE' +Usage: + script/ethercat/status_ethercat.sh + +Environment: + IGH_ROOT=... Override bundled IgH install path. + ETHERCAT_CONF=... Override ethercatctl config path. +USAGE +} + +if [[ "${1:-}" == "-h" || "${1:-}" == "--help" ]]; then + usage + exit 0 +fi + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)" +IGH_ROOT="${IGH_ROOT:-${REPO_ROOT}/dependency/x86/third_party/ethercat/v1.7.0}" +ETHERCAT_CONF="${ETHERCAT_CONF:-${IGH_ROOT}/etc/ethercat.conf}" +ETHERCATCTL="${IGH_ROOT}/sbin/ethercatctl" +ETHERCAT="${IGH_ROOT}/bin/ethercat" + +if [[ ! -x "${ETHERCATCTL}" ]]; then + echo "error: ethercatctl not found: ${ETHERCATCTL}" >&2 + exit 1 +fi + +if [[ ! -x "${ETHERCAT}" ]]; then + echo "error: ethercat command not found: ${ETHERCAT}" >&2 + exit 1 +fi + +echo "== ${ETHERCAT_CONF} ==" +if [[ -f "${ETHERCAT_CONF}" ]]; then + sed -n '1,80p' "${ETHERCAT_CONF}" +else + echo "missing" +fi + +echo +echo "== kernel modules ==" +lsmod | grep -E '(^ec_master|^ec_generic|^ec_)' || true + +echo +echo "== ethercatctl ==" +"${ETHERCATCTL}" -c "${ETHERCAT_CONF}" status || true + +echo +echo "== master ==" +"${ETHERCAT}" master || true + +echo +echo "== slaves ==" +"${ETHERCAT}" slaves || true + +echo +echo "== pdos ==" +"${ETHERCAT}" pdos || true diff --git a/script/ethercat/stop_ethercat.sh b/script/ethercat/stop_ethercat.sh new file mode 100755 index 00000000..2e0d6f33 --- /dev/null +++ b/script/ethercat/stop_ethercat.sh @@ -0,0 +1,70 @@ +#!/usr/bin/env bash +set -euo pipefail + +usage() { + cat <<'USAGE' +Usage: + sudo script/ethercat/stop_ethercat.sh [iface] [--restore-network] + +Examples: + sudo script/ethercat/stop_ethercat.sh eno1 + sudo script/ethercat/stop_ethercat.sh eno1 --restore-network + +Environment: + IGH_ROOT=... Override bundled IgH install path. + ETHERCAT_CONF=... Override ethercatctl config path. +USAGE +} + +if [[ "${1:-}" == "-h" || "${1:-}" == "--help" ]]; then + usage + exit 0 +fi + +if [[ "$(id -u)" -ne 0 ]]; then + echo "error: please run with sudo." >&2 + exit 1 +fi + +IFACE="eno1" +RESTORE_NETWORK="false" + +for arg in "$@"; do + case "${arg}" in + --restore-network) + RESTORE_NETWORK="true" + ;; + -*) + echo "error: unknown option: ${arg}" >&2 + usage >&2 + exit 1 + ;; + *) + IFACE="${arg}" + ;; + esac +done + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REPO_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)" +IGH_ROOT="${IGH_ROOT:-${REPO_ROOT}/dependency/x86/third_party/ethercat/v1.7.0}" +ETHERCAT_CONF="${ETHERCAT_CONF:-${IGH_ROOT}/etc/ethercat.conf}" +ETHERCATCTL="${IGH_ROOT}/sbin/ethercatctl" + +if [[ ! -x "${ETHERCATCTL}" ]]; then + echo "error: ethercatctl not found: ${ETHERCATCTL}" >&2 + exit 1 +fi + +"${ETHERCATCTL}" -c "${ETHERCAT_CONF}" stop + +if [[ "${RESTORE_NETWORK}" == "true" ]]; then + ip link set "${IFACE}" up + if command -v nmcli >/dev/null 2>&1; then + nmcli device connect "${IFACE}" || true + fi + echo "Stopped EtherCAT and requested normal network restore on ${IFACE}." +else + echo "Stopped EtherCAT. Normal network restore skipped." + echo "Use '--restore-network' if this interface should return to NetworkManager." +fi