HiveCoreRD/hivecore_robot_drivers
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ethercat for R1 robot

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-23 15:18:11 +08:00
parent 199d10da6d
commit afa2b443f5
11 changed files with 631 additions and 520 deletions
Download Patch File Download Diff File Expand all files Collapse all files

106
ecrt_dev/config/r1controllers.yaml Normal file
View File

@@ -0,0 +1,106 @@
controller_manager:
ros__parameters:
update_rate: 1000 # Hz
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
gpio_command_controller:
type: gpio_controllers/GpioCommandController
trajectory_controller:
ros__parameters:
joints:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
- joint_7
- joint_8
command_interfaces:
- position
state_interfaces:
- position
- velocity
state_publish_rate: 200.0
action_monitor_rate: 20.0
allow_partial_joints_goal: false
open_loop_control: true
allow_integration_in_goal_trajectories: true
constraints:
goal_time: 0.5
stopped_velocity_tolerance: 0.02
gpio_command_controller:
ros__parameters:
gpios:
- joint_1
- joint_2
- joint_3
- joint_4
- joint_5
- joint_6
- joint_7
- joint_8
command_interfaces:
joint_1:
- interfaces:
- fault
- enable
joint_2:
- interfaces:
- fault
- enable
joint_3:
- interfaces:
- fault
- enable
joint_4:
- interfaces:
- fault
- enable
joint_5:
- interfaces:
- fault
- enable
joint_6:
- interfaces:
- fault
- enable
joint_7:
- interfaces:
- fault
- enable
joint_8:
- interfaces:
- fault
- enable
state_interfaces:
joint_1:
- interfaces:
- fault
joint_2:
- interfaces:
- fault
joint_3:
- interfaces:
- fault
joint_4:
- interfaces:
- fault
joint_5:
- interfaces:
- fault
joint_6:
- interfaces:
- fault
joint_7:
- interfaces:
- fault
joint_8:
- interfaces:
- fault

9
ecrt_dev/description/r1_motor_drive.config.xacro Normal file
View File

@@ -0,0 +1,9 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="motor_drive">
<link name="world"/>
<xacro:include filename="r1_motor_drive.ros2_control.xacro" />
<xacro:motor_drive/>
</robot>

58
ecrt_dev/description/r1_motor_drive.ros2_control.xacro Normal file
View File

@@ -0,0 +1,58 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- 公共参数定义（一次修改，全局生效） -->
<xacro:property name="master_id" value="0" />
<xacro:property name="control_frequency" value="10" />
<xacro:property name="ec_plugin" value="ethercat_generic_plugins/EcCiA402Drive" />
<xacro:property name="alias" value="0" />
<xacro:property name="mode_of_operation" value="8" />
<xacro:property name="slave_config_path" value="config/zeroErr_config.yaml" />
<!-- 子宏：封装单个关节的重复配置 -->
<!-- 参数：joint_name（关节名称）、ec_position（EtherCAT位置索引） -->
<xacro:macro name="single_joint_config" params="joint_name ec_position">
<joint name="${joint_name}">
<!-- 状态接口（位置/速度/力矩） -->
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="fault"/>
<!-- 命令接口（位置控制/故障重置/使能） -->
<command_interface name="fault"/>
<command_interface name="enable"/>
<command_interface name="position"/>
<!-- EtherCAT模块配置（复用公共参数） -->
<ec_module name="zeroErr">
<plugin>${ec_plugin}</plugin>
<param name="alias">${alias}</param>
<param name="position">${ec_position}</param>
<param name="mode_of_operation">${mode_of_operation}</param>
<param name="slave_config">${slave_config_path}</param>
</ec_module>
</joint>
</xacro:macro>
<!-- 主宏：电机驱动系统配置 -->
<xacro:macro name="motor_drive">
<ros2_control name="motor_drive" type="system">
<hardware>
<plugin>ethercat_driver/EthercatDriver</plugin>
<param name="master_id">${master_id}</param>
<param name="control_frequency">${control_frequency}</param>
</hardware>
<!-- 调用子宏生成多个关节（仅需指定名称和位置索引） -->
<xacro:single_joint_config joint_name="joint_1" ec_position="1" />
<xacro:single_joint_config joint_name="joint_2" ec_position="2" />
<xacro:single_joint_config joint_name="joint_3" ec_position="3" />
<xacro:single_joint_config joint_name="joint_4" ec_position="4" />
<xacro:single_joint_config joint_name="joint_5" ec_position="5" />
<xacro:single_joint_config joint_name="joint_6" ec_position="6" />
<xacro:single_joint_config joint_name="joint_7" ec_position="7" />
<xacro:single_joint_config joint_name="joint_8" ec_position="8" />
</ros2_control>
</xacro:macro>
</robot>

4
ecrt_dev/include/ethercat_driver/ethercat_driver.hpp
View File

@@ -75,12 +75,12 @@ public:
void check_domain1_state();
void check_slave_config_states();
void set_motor_enable(int id,bool enable){
if(id>0&&id<13){
if(id>0&&id<9){
motor_enable_arr[id-1].store(enable);
}
};
bool get_motor_enable(int id){
if(id>0&&id<13){
if(id>0&&id<9){
return motor_enable_arr[id-1].load();
}
return false;

2
ecrt_dev/include/ethercat_driver/zer_config.hpp
View File

@@ -1,6 +1,6 @@
#include "ecrt.h"
#include <math.h>
#define NUM_SLAVES 12
#define NUM_SLAVES 8
#define ZER_VID_PID 0x5a65726f,0x00029252
typedef struct {
unsigned int ctrl_word; // 0x6040:00

95
ecrt_dev/launch/motor_drive.launch copy.py Normal file
View File

@@ -0,0 +1,95 @@
from launch import LaunchDescription
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration, TextSubstitution
from launch.actions import DeclareLaunchArgument,TimerAction
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Declare arguments
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
'description_file',
default_value='motor_drive.config.xacro',
description='URDF/XACRO description file with the axis.',
)
)
description_file = LaunchConfiguration('description_file')
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[
FindPackageShare("ecrt_driver"),
"description",
description_file,
]
),
]
)
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution(
[
FindPackageShare("ecrt_driver"),
"config",
"controllers.yaml",
]
)
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers,
{
"lock_memory": True,
"thread_priority": 90
}
],
output="both",
)
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[robot_description],
output='screen'
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
)
gpio_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["gpio_command_controller", "-c", "/controller_manager"],
)
trajectory_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["trajectory_controller", "-c", "/controller_manager"],
)
delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner,trajectory_controller_spawner])
nodes = [
control_node,
robot_state_pub_node,
joint_state_broadcaster_spawner,
delay_node,
]#position_controller_spawner,
return LaunchDescription(
declared_arguments +
nodes)

119
ecrt_dev/launch/motor_drive.launch.py
View File

@@ -1,95 +1,144 @@
#!/usr/bin/env python3
from launch import LaunchDescription
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration, TextSubstitution
from launch.actions import DeclareLaunchArgument,TimerAction
# 补全必要导入，增强跨设备兼容性
from launch.substitutions import (
Command, FindExecutable, PathJoinSubstitution,
LaunchConfiguration, TextSubstitution, EnvironmentVariable
)
from launch.actions import (
DeclareLaunchArgument, TimerAction, OpaqueFunction
)
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
# 新增：导入系统路径模块，避免路径解析异常
import os
def generate_launch_description():
# Declare arguments
# -------------------------- 1. 声明参数（增强默认值和兼容性） --------------------------
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
'description_file',
default_value='motor_drive.config.xacro',
default_value=TextSubstitution(text='motor_drive.config.xacro'), # 显式用TextSubstitution，避免解析歧义
description='URDF/XACRO description file with the axis.',
)
)
# 新增：声明工作空间路径参数，适配不同设备的路径差异
declared_arguments.append(
DeclareLaunchArgument(
'pkg_share_path',
default_value=FindPackageShare("ecrt_driver"),
description='Path to ecrt_driver package share directory'
)
)
# 绑定参数，规范命名
description_file = LaunchConfiguration('description_file')
pkg_share_path = LaunchConfiguration('pkg_share_path')
# Get URDF via xacro
# -------------------------- 2. 重构URDF生成逻辑（解决拼接解析异常） --------------------------
# 优化：用TextSubstitution替换纯空格，避免跨设备解析时的空格截断问题
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
FindExecutable(name="xacro"),
TextSubstitution(text=" "), # 替换原纯空格，规范Substitution拼接
PathJoinSubstitution(
[
FindPackageShare("ecrt_driver"),
"description",
pkg_share_path, # 使用声明的参数，增强兼容性
TextSubstitution(text="description"),
description_file,
]
),
TextSubstitution(text=" "), # 确保xacro命令参数分隔清晰
]
)
robot_description = {"robot_description": robot_description_content}
# -------------------------- 3. 重构控制器配置路径（避免路径解析失败） --------------------------
robot_controllers = PathJoinSubstitution(
[
FindPackageShare("ecrt_driver"),
"config",
"controllers.yaml",
pkg_share_path,
TextSubstitution(text="config"),
TextSubstitution(text="controllers.yaml"),
]
)
# -------------------------- 4. 节点配置（增强跨设备兼容性） --------------------------
# 控制节点：添加emulate_tty=True，修复日志输出和权限问题；参数格式规范化
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers,
{
"lock_memory": True,
"thread_priority": 90
}
],
executable="ros2_control_node", # 确保executable为纯字符串，无替换对象
parameters=[
robot_description,
robot_controllers,
{
"lock_memory": True,
"thread_priority": 90,
"use_sim_time": False # 新增：显式关闭仿真时间，避免环境歧义
}
],
output="both",
emulate_tty=True, # 关键：修复跨设备终端输出解析问题
namespace="/ecrt_driver" # 新增：添加命名空间，避免节点冲突
)
# 机器人状态发布节点：优化输出和参数
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[robot_description],
output='screen'
executable="robot_state_publisher", # 纯字符串，无替换对象
parameters=[
robot_description,
{"use_sim_time": False} # 显式指定，增强兼容性
],
output='screen',
emulate_tty=True, # 修复TextSubstitution相关的输出解析问题
namespace="/ecrt_driver"
)
# 关节状态广播器：参数格式规范化
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
executable="spawner", # 纯字符串，无替换对象
arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
output='screen',
emulate_tty=True,
namespace="/ecrt_driver"
)
# GPIO控制器：参数格式规范化
gpio_controller_spawner = Node(
package="controller_manager",
executable="spawner",
executable="spawner", # 纯字符串，无替换对象
arguments=["gpio_command_controller", "-c", "/controller_manager"],
output='screen',
emulate_tty=True,
namespace="/ecrt_driver"
)
# 轨迹控制器：参数格式规范化
trajectory_controller_spawner = Node(
package="controller_manager",
executable="spawner",
executable="spawner", # 纯字符串，无替换对象
arguments=["trajectory_controller", "-c", "/controller_manager"],
output='screen',
emulate_tty=True,
namespace="/ecrt_driver"
)
delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner,trajectory_controller_spawner])
# -------------------------- 5. 延时节点（修复执行顺序解析问题） --------------------------
# 优化：TimerAction的actions用列表包裹，避免解析时的类型歧义
delay_node = TimerAction(
period=1.0,
actions=[gpio_controller_spawner, trajectory_controller_spawner],
)
# 组装所有节点
nodes = [
control_node,
robot_state_pub_node,
joint_state_broadcaster_spawner,
delay_node,
]#position_controller_spawner,
]
return LaunchDescription(
declared_arguments +
nodes)
# 返回LaunchDescription，确保参数声明和节点列表拼接规范
return LaunchDescription(declared_arguments + nodes)

88
ecrt_dev/launch/r1_motor_drive.launch.py Normal file
View File

@@ -0,0 +1,88 @@
from launch import LaunchDescription
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
from launch.actions import DeclareLaunchArgument,TimerAction
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Declare arguments
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
'description_file',
default_value='r1_motor_drive.config.xacro',
description='URDF/XACRO description file with the axis.',
)
)
description_file = LaunchConfiguration('description_file')
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[
FindPackageShare("ecrt_driver"),
"description",
description_file,
]
),
]
)
robot_description = {"robot_description": robot_description_content}
robot_controllers = PathJoinSubstitution(
[
FindPackageShare("ecrt_driver"),
"config",
"r1controllers.yaml",
]
)
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_controllers,
{
"lock_memory": True,
"thread_priority": 90
}
],
output="both",
)
print('444444')
robot_state_pub_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[robot_description],
output='screen'
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
)
gpio_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["gpio_command_controller", "-c", "/controller_manager"],
)
trajectory_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["trajectory_controller", "-c", "/controller_manager"],
)
delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner,trajectory_controller_spawner])
nodes = [
control_node,
robot_state_pub_node,
joint_state_broadcaster_spawner,
delay_node,
]#position_controller_spawner,
return LaunchDescription(
declared_arguments +
nodes)

2
ethercat_dev/src/ethercat_control/include/ethercat_control/motor_control.hpp
View File

@@ -4,7 +4,7 @@
#include <cstdint>
#include <algorithm>
constexpr int NUM_SLAVES = 1; //定义电机数量
constexpr int NUM_SLAVES = 8; //定义电机数量
//运行模式
enum class OpMode : int8_t {

80
ethercat_dev/src/ethercat_control/include/ethercat_control/mt_config.hpp
View File

@@ -1,80 +0,0 @@
#pragma once
#include "ecrt.h"
#include "ethercat_control/motor_control.hpp" // 电机控制库
#define MT_Device0 0,0 /*EtherCAT address on the bus 别名，位置*/
#define MT_VID_PID 0x00202008,0x00000000 /*Vendor ID, product code 厂商ID和产品代码*/
typedef struct {
unsigned int ctrl_word; // 0x6040:00
unsigned int target_position; // 0x607A:00
unsigned int target_velocity; // 0x60FF:00
unsigned int target_torque; // 0x6071:00
unsigned int max_torque; // 0x6072:00
unsigned int operation_mode; // 0x6060:00
unsigned int reserved1; // 0x5FFE:00
unsigned int status_word; // 0x6041:00
unsigned int position_actual_value; // 0x6064:00
unsigned int velocity_actual_value; // 0x606C:00
unsigned int torque_actual_value; // 0x6077:00
unsigned int error_code; // 0x603F:00
unsigned int modes_of_operation_display; // 0x6061:00
unsigned int reserved2; // 0x5FFE:00
} mt_offset_t;
static mt_offset_t mt_offsets[NUM_SLAVES];
// ------------------- PDO 定义（CSV/CSP/CST），对应 0x1600/0x1A00 -------------------
// 下列结构由IGH主站 cstruct 自动生成
const static ec_pdo_entry_reg_t mt_domain1_regs[] = {
{MT_Device0, MT_VID_PID, 0x6040, 0, &mt_offsets[0].ctrl_word}, // 控制字
{MT_Device0, MT_VID_PID, 0x607A, 0, &mt_offsets[0].target_position}, // 目标位置
{MT_Device0, MT_VID_PID, 0x60FF, 0, &mt_offsets[0].target_velocity}, // 目标速度
{MT_Device0, MT_VID_PID, 0x6071, 0, &mt_offsets[0].target_torque}, // 目标扭矩
{MT_Device0, MT_VID_PID, 0x6072, 0, &mt_offsets[0].max_torque}, // 最大扭矩
{MT_Device0, MT_VID_PID, 0x6060, 0, &mt_offsets[0].operation_mode}, // 运行模式
{MT_Device0, MT_VID_PID, 0x5FFE, 0, &mt_offsets[0].reserved1}, // 保留字段1
{MT_Device0, MT_VID_PID, 0x6041, 0, &mt_offsets[0].status_word}, // 状态字
{MT_Device0, MT_VID_PID, 0x6064, 0, &mt_offsets[0].position_actual_value}, // 实际位置
{MT_Device0, MT_VID_PID, 0x606C, 0, &mt_offsets[0].velocity_actual_value}, // 实际速度
{MT_Device0, MT_VID_PID, 0x6077, 0, &mt_offsets[0].torque_actual_value}, // 实际扭矩）
{MT_Device0, MT_VID_PID, 0x603F, 0, &mt_offsets[0].error_code}, // 错误代码
{MT_Device0, MT_VID_PID, 0x6061, 0, &mt_offsets[0].modes_of_operation_display}, // 模式显示
{MT_Device0, MT_VID_PID, 0x5FFE, 0, &mt_offsets[0].reserved2}, // 保留字段2
{} // 结束标记
};
static ec_pdo_entry_info_t mt_device_pdo_entries[] = {
/*RxPdo 0x1600*/
{0x6040, 0x00, 16}, /* control word */
{0x607a, 0x00, 32}, /* target position */
{0x60ff, 0x00, 32}, /* target velocity */
{0x6071, 0x00, 16}, /* Target Torque */
{0x6072, 0x00, 16}, /* Max Torque */
{0x6060, 0x00, 8}, /* modes of operation */
{0x5ffe, 0x00, 8},
/*TxPdo 0x1A00*/
{0x6041, 0x00, 16}, /* status word */
{0x6064, 0x00, 32}, /* position actual value */
{0x606c, 0x00, 32}, /* velocity actual value */
{0x6077, 0x00, 16}, /* torque actual value */
{0x603f, 0x00, 16}, /* error code */
{0x6061, 0x00, 8}, /* modes of operation display */
{0x5ffe, 0x00, 8},
};
static ec_pdo_info_t mt_device_pdos[] = {
//RxPdo
{0x1600, 7, mt_device_pdo_entries + 0 },
//TxPdo
{0x1A00, 7, mt_device_pdo_entries + 7 }
};
static ec_sync_info_t mt_device_syncs[] = {
{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
{1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
{2, EC_DIR_OUTPUT, 1, mt_device_pdos + 0, EC_WD_ENABLE},
{3, EC_DIR_INPUT, 1, mt_device_pdos + 1, EC_WD_DISABLE},
{0xff}
};

588
ethercat_dev/src/ethercat_control/src/ethercat_core.cpp
View File

@@ -28,9 +28,6 @@
#include <cmath> // for std::llround
#include <iostream>
#include "ethercat_control/mt_config.hpp"
#include "ethercat_control/zer_config.hpp"
#include "ethercat_control/et_config.hpp"
// #include "ethercat_control/ds_config.hpp"
@@ -148,11 +145,11 @@ void check_slave_config_states(void)
if (!sc[i]) continue;
ecrt_slave_config_state(sc[i], &s);
if (s.al_state != sc_state[i].al_state)
//printf("[S%02d] State 0x%02X.\n", i, s.al_state);
printf("[S%02d] State 0x%02X.\n", i, s.al_state);
if (s.online != sc_state[i].online)
//printf("[S%02d] %s.\n", i, s.online ? "online" : "offline");
printf("[S%02d] %s.\n", i, s.online ? "online" : "offline");
if (s.operational != sc_state[i].operational)
//printf("[S%02d] %soperational.\n", i, s.operational ? "" : "Not ");
printf("[S%02d] %soperational.\n", i, s.operational ? "" : "Not ");
sc_state[i] = s;
}
}
@@ -289,326 +286,173 @@ void cyclic_task()
// 读取从站状态字、当前位置、速度、扭矩、运行模式
for (int i = 0; i < NUM_SLAVES; ++i)
{
if (i == 0)
// if (false)
{
uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word);
int32_t pv = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
int32_t vv = EC_READ_S32(domain1_pd + zer_offsets[i].velocity_actual_value);
int16_t tv = EC_READ_S16(domain1_pd + zer_offsets[i].torque_actual_value);
int8_t md = EC_READ_S8 (domain1_pd + zer_offsets[i].modes_of_operation_display);
uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word);
int32_t pv = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
int32_t vv = EC_READ_S32(domain1_pd + zer_offsets[i].velocity_actual_value);
int16_t tv = EC_READ_S16(domain1_pd + zer_offsets[i].torque_actual_value);
int8_t md = EC_READ_S8 (domain1_pd + zer_offsets[i].modes_of_operation_display);
status[i] = sw; //侦测子站状态字变化
if (status[i] != last_status[i]) {
// printf("[S%02d] 状态改变为: 0x%04X\n", i, status[i]);
last_status[i] = status[i];
status[i] = sw; //侦测子站状态字变化
if (status[i] != last_status[i]) {
// printf("[S%02d] 状态改变为: 0x%04X\n", i, status[i]);
last_status[i] = status[i];
}
// ----- 1) 写共享状态（供ROS2 读取）-----
g_state.status_word[i].store(sw, std::memory_order_relaxed);
g_state.pos_act[i].store(pv, std::memory_order_relaxed);
g_state.vel_act[i].store(vv, std::memory_order_relaxed);
g_state.torque_act[i].store(tv, std::memory_order_relaxed);
// ----- 2) 读取本周期命令 -----
// 读CSP/CSV/CST/PVT 命令
const int8_t mode_cmd = g_cmd.mode[i].load(std::memory_order_relaxed);
const bool run_enable = g_cmd.run_enable[i].load(std::memory_order_relaxed);
const int32_t vel_cmd = g_cmd.target_vel[i].load(std::memory_order_relaxed);
const int32_t pos_cmd = g_cmd.target_pos[i].load(std::memory_order_relaxed);
const int16_t tor_cmd = g_cmd.target_torque[i].load(std::memory_order_relaxed);
//EC_WRITE_S8 (domain1_pd + offsets[i].operation_mode, mode_cmd); //模式设定
// 第一次运行时把 last_mode_cmd 初始化为实际显示模式，避免“假沿”
if (!last_mode_cmd_inited) {
last_mode_cmd[i] = md;
if (i == NUM_SLAVES - 1) last_mode_cmd_inited = 1;
}
if (md != mode_cmd) {
// 1) 拉到 Ready to switch on（允许改模式）
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);
// 2) 写目标模式
EC_WRITE_S8 (domain1_pd + zer_offsets[i].operation_mode, mode_cmd);
// 3) 下一周期状态机会自动从 0x0021→0x0023→0x0027
// 进入新模式时做必要初始化
bool csp_mode = (mode_cmd == CYCLIC_POSITION);
if (csp_mode) {
// 初次进入CSP：把目标位置对齐当前实际位置，避免冲击
// g_cmd.target_pos[i].store(pv, std::memory_order_relaxed); //把电机当前位置更新进上层
// EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv);
csp_initialized[i] = 1;
} else {
csp_initialized[i] = 0;
}
// ----- 1) 写共享状态（供ROS2 读取）-----
g_state.status_word[i].store(sw, std::memory_order_relaxed);
g_state.pos_act[i].store(pv, std::memory_order_relaxed);
g_state.vel_act[i].store(vv, std::memory_order_relaxed);
g_state.torque_act[i].store(tv, std::memory_order_relaxed);
last_mode_cmd[i] = mode_cmd;
continue; // 本周期先不再下其他目标，等模式生效
}
// ----- 2) 读取本周期命令 -----
// 读CSP/CSV/CST/PVT 命令
const int8_t mode_cmd = g_cmd.mode[i].load(std::memory_order_relaxed);
const bool run_enable = g_cmd.run_enable[i].load(std::memory_order_relaxed);
const int32_t vel_cmd = g_cmd.target_vel[i].load(std::memory_order_relaxed);
const int32_t pos_cmd = g_cmd.target_pos[i].load(std::memory_order_relaxed);
const int16_t tor_cmd = g_cmd.target_torque[i].load(std::memory_order_relaxed);
if ((status[i]&0x006f)==0x0008){
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
//EC_WRITE_S8 (domain1_pd + offsets[i].operation_mode, mode_cmd); //模式设定
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0080);
std::cout << "clear : " << i << " " << err << std::endl;
}
// 第一次运行时把 last_mode_cmd 初始化为实际显示模式，避免“假沿”
if (!last_mode_cmd_inited) {
last_mode_cmd[i] = md;
if (i == NUM_SLAVES - 1) last_mode_cmd_inited = 1;
}
// continue;
// DS402 状态机 & 写控制/目标
if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006); // Ready to switch on
command[i] = 0x006F;
//std::cout << " Motor : " << i << " Ready to switch on" << std::endl;
} else if ((status[i] & command[i]) == 0x0021) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007); // Switched on
command[i] = 0x006F;
if (md != mode_cmd) {
// 1) 拉到 Ready to switch on（允许改模式）
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);
// 2) 写目标模式
EC_WRITE_S8 (domain1_pd + zer_offsets[i].operation_mode, mode_cmd);
// 3) 下一周期状态机会自动从 0x0021→0x0023→0x0027
// 进入新模式时做必要初始化
// 同步当前位置为目标位置
int32_t pv2 = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv2);
g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);
bool csp_mode = (mode_cmd == CYCLIC_POSITION);
if (csp_mode) {
// 初次进入CSP：把目标位置对齐当前实际位置，避免冲击
// g_cmd.target_pos[i].store(pv, std::memory_order_relaxed); //把电机当前位置更新进上层
// EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv);
csp_initialized[i] = 1;
} else {
csp_initialized[i] = 0;
}
last_mode_cmd[i] = mode_cmd;
continue; // 本周期先不再下其他目标，等模式生效
}
// DS402 状态机 & 写控制/目标
if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006); // Ready to switch on
command[i] = 0x006F;
std::cout << " Motor : " << i << " Ready to switch on" << std::endl;
} else if ((status[i] & command[i]) == 0x0021) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007); // Switched on
command[i] = 0x006F;
motorCount1 ++;
//std::cout << " Motor : " << i << " times : " << motorCount1 << " Switched on" << std::endl;
} else if ((status[i] & command[i]) == 0x0023) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f); // Operation enabled
command[i] = 0x006F;
// 同步当前位置为目标位置
int32_t pv2 = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv2);
g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);
//std::cout << " Motor : " << i << " Operation enabled" << std::endl;
} else if ((status[i] & command[i]) == 0x0027) {
// ---- 4) 运行态写目标（按模式/运行意图） ----
if (run_enable) {
//EC_WRITE_U16(domain1_pd + offsets[i].ctrl_word, 0x001F); // 仅在PP模式下需要
switch (mode_cmd) {
case CYCLIC_VELOCITY: // 9
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, vel_cmd);
break;
case CYCLIC_POSITION: { // 8
// ---- 绝对式目标生成，不再用 pv 做增量基准 ----
// 每轴记住“上次下发的命令位置”
static int32_t csp_cmd_pos[NUM_SLAVES]; // 上次下发到 0x607A 的值
static uint8_t csp_inited[NUM_SLAVES] = {0}; // 是否已初始化
// 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题
static int64_t vmax_acc[NUM_SLAVES] = {0}; // 单位：counts/s 累加器
motorCount1 ++;
std::cout << " Motor : " << i << " times : " << motorCount1 << " Switched on" << std::endl;
} else if ((status[i] & command[i]) == 0x0023) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f); // Operation enabled
command[i] = 0x006F;
std::cout << " Motor : " << i << " Operation enabled" << std::endl;
} else if ((status[i] & command[i]) == 0x0027) {
// ---- 4) 运行态写目标（按模式/运行意图） ----
if (run_enable) {
//EC_WRITE_U16(domain1_pd + offsets[i].ctrl_word, 0x001F); // 仅在PP模式下需要
switch (mode_cmd) {
case CYCLIC_VELOCITY: // 9
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, vel_cmd);
break;
case CYCLIC_POSITION: { // 8
// ---- 绝对式目标生成，不再用 pv 做增量基准 ----
// 每轴记住“上次下发的命令位置”
static int32_t csp_cmd_pos[NUM_SLAVES]; // 上次下发到 0x607A 的值
static uint8_t csp_inited[NUM_SLAVES] = {0}; // 是否已初始化
// 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题
static int64_t vmax_acc[NUM_SLAVES] = {0}; // 单位：counts/s 累加器
// 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
csp_cmd_pos[i] = pv; // 先对齐
vmax_acc[i] = 0;
csp_inited[i] = 1;
}
// 期望的“最终绝对目标”
const int32_t goal = g_cmd.target_pos[i].load(std::memory_order_relaxed);
// 本周期允许的最大步长（counts/周期），用定点累加保证平均速度上限严格等于 CSP_MAX_VEL_COUNTS_PER_S
vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S; // +v [counts/s]
int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长
vmax_acc[i] -= (int64_t)max_step * FREQUENCY; // 保留余数到下周期
// 相对“命令轨迹”的误差（不是相对 pv）
const int32_t err_cmd = goal - csp_cmd_pos[i];
// 死区：足够近就贴合到 goal
if (err_cmd > -CSP_POS_DEADBAND && err_cmd < CSP_POS_DEADBAND) {
csp_cmd_pos[i] = goal;
} else {
// 限速迈步：命令轨迹只按时间往 goal 逼近
if (err_cmd > 0) csp_cmd_pos[i] += (err_cmd > max_step) ? max_step : err_cmd;
else csp_cmd_pos[i] += (err_cmd < -max_step) ? -max_step : err_cmd;
}
// 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
// const int32_t follow_err = csp_cmd_pos[i] - pv;
// const int32_t FE_LIMIT = (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
// if (std::abs(follow_err) > FE_LIMIT) {
// // 例如：冻结或减半 max_step，给驱动时间追上
// }
// 下发“绝对目标”（不是增量）
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]);
// 回显给上层（/joint_states_cmd 用）
g_state.pos_cmd[i].store(csp_cmd_pos[i], std::memory_order_relaxed);
break;
// 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
csp_cmd_pos[i] = pv; // 先对齐
vmax_acc[i] = 0;
csp_inited[i] = 1;
}
case CYCLIC_TORQUE: // 10
EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, tor_cmd);
break;
default:
// 未知模式：默认速度模式安全置 0
//EC_WRITE_S32(domain1_pd + offsets[i].target_velocity, 0);
break;
}
} else {
// run_enable=false：清零目标
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
// CSV 安全：速度清零
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
// CSP 安全：目标位置=当前位置 -> 立即“冻结”
int32_t pv_hold = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv_hold);
// 可选：扭矩模式也清零（最好维持急停状态，待修改）
EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, 0);
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
}
}
}else{
uint16_t sw = EC_READ_U16(domain1_pd + mt_offsets[i].status_word);
int32_t pv = EC_READ_S32(domain1_pd + mt_offsets[i].position_actual_value);
int32_t vv = EC_READ_S32(domain1_pd + mt_offsets[i].velocity_actual_value);
int16_t tv = EC_READ_S16(domain1_pd + mt_offsets[i].torque_actual_value);
int8_t md = EC_READ_S8 (domain1_pd + mt_offsets[i].modes_of_operation_display);
// printf("[S%02d] 状态字: 0x%04X, 位置: %d, 速度: %d, 力矩: %d, 模式: %d\n", i, sw, pv, vv, tv, md);
// 期望的“最终绝对目标”
const int32_t goal = g_cmd.target_pos[i].load(std::memory_order_relaxed);
status[i] = sw; //侦测子站状态字变化
if (status[i] != last_status[i]) {
// printf("[S%02d] 状态改变为: 0x%04X\n", i, status[i]);
last_status[i] = status[i];
}
// ----- 1) 写共享状态（供ROS2 读取）-----
g_state.status_word[i].store(sw, std::memory_order_relaxed);
g_state.pos_act[i].store(pv, std::memory_order_relaxed);
g_state.vel_act[i].store(vv, std::memory_order_relaxed);
g_state.torque_act[i].store(tv, std::memory_order_relaxed);
// 本周期允许的最大步长（counts/周期），用定点累加保证平均速度上限严格等于 CSP_MAX_VEL_COUNTS_PER_S
vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S; // +v [counts/s]
int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长
vmax_acc[i] -= (int64_t)max_step * FREQUENCY; // 保留余数到下周期
// ----- 2) 读取本周期命令 -----
// 读CSP/CSV/CST/PVT 命令
const int8_t mode_cmd = g_cmd.mode[i].load(std::memory_order_relaxed);
const bool run_enable = g_cmd.run_enable[i].load(std::memory_order_relaxed);
const int32_t vel_cmd = g_cmd.target_vel[i].load(std::memory_order_relaxed);
const int32_t pos_cmd = g_cmd.target_pos[i].load(std::memory_order_relaxed);
const int16_t tor_cmd = g_cmd.target_torque[i].load(std::memory_order_relaxed);
// 相对“命令轨迹”的误差（不是相对 pv）
const int32_t err_cmd = goal - csp_cmd_pos[i];
//EC_WRITE_S8 (domain1_pd + offsets[i].operation_mode, mode_cmd); //模式设定
// 第一次运行时把 last_mode_cmd 初始化为实际显示模式，避免“假沿”
if (!last_mode_cmd_inited) {
last_mode_cmd[i] = md;
if (i == NUM_SLAVES - 1) last_mode_cmd_inited = 1;
}
if (md != mode_cmd) {
// 1) 拉到 Ready to switch on（允许改模式）
EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0006);
// 2) 写目标模式
EC_WRITE_S8 (domain1_pd + mt_offsets[i].operation_mode, mode_cmd);
// 3) 下一周期状态机会自动从 0x0021→0x0023→0x0027
// 进入新模式时做必要初始化
if (mode_cmd == CYCLIC_POSITION) {
// 初次进入CSP：把目标位置对齐当前实际位置，避免冲击
// g_cmd.target_pos[i].store(pv, std::memory_order_relaxed); //把电机当前位置更新进上层
// EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, pv);
csp_initialized[i] = 1;
// std::cout << "new Goal : " << pv << std::endl;
} else {
csp_initialized[i] = 0;
}
last_mode_cmd[i] = mode_cmd;
continue; // 本周期先不再下其他目标，等模式生效
}
// DS402 状态机 & 写控制/目标
if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0006); // Ready to switch on
command[i] = 0x006F;
std::cout << " Motor : " << i << " Ready to switch on" << std::endl;
} else if ((status[i] & command[i]) == 0x0021) {
EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0007); // Switched on
command[i] = 0x006F;
// 同步当前位置为目标位置
int32_t pv2 = EC_READ_S32(domain1_pd + mt_offsets[i].position_actual_value);
EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, pv2);
g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);
motorCount2 ++;
std::cout << " Motor : " << i << " times : " << motorCount2 << " Switched on" << std::endl;
} else if ((status[i] & command[i]) == 0x0023) {
EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x000f); // Operation enabled
command[i] = 0x006F;
std::cout << " Motor : " << i << " Operation enabled" << std::endl;
} else if ((status[i] & command[i]) == 0x0027) {
// ---- 4) 运行态写目标（按模式/运行意图） ----
if (run_enable) {
switch (mode_cmd) {
case CYCLIC_VELOCITY: // 9
EC_WRITE_S32(domain1_pd + mt_offsets[i].target_velocity, vel_cmd);
break;
case CYCLIC_POSITION: { // 8
// ---- 绝对式目标生成，不再用 pv 做增量基准 ----
// 每轴记住“上次下发的命令位置”
static int32_t csp_cmd_pos[NUM_SLAVES]; // 上次下发到 0x607A 的值
static uint8_t csp_inited[NUM_SLAVES] = {0}; // 是否已初始化
// 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题
static int64_t vmax_acc[NUM_SLAVES] = {0}; // 单位：counts/s 累加器
// 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
csp_cmd_pos[i] = pv; // 先对齐
vmax_acc[i] = 0;
csp_inited[i] = 1;
}
// 期望的“最终绝对目标”
const int32_t goal = g_cmd.target_pos[i].load(std::memory_order_relaxed);
// std::cout << "goal: " << goal << std::endl;
// 本周期允许的最大步长（counts/周期），用定点累加保证平均速度上限严格等于 CSP_MAX_VEL_COUNTS_PER_S
vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S; // +v [counts/s]
int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长
vmax_acc[i] -= (int64_t)max_step * FREQUENCY; // 保留余数到下周期
// 相对“命令轨迹”的误差（不是相对 pv）
const int32_t err_cmd = goal - csp_cmd_pos[i];
// 死区：足够近就贴合到 goal
if (err_cmd > -CSP_POS_DEADBAND && err_cmd < CSP_POS_DEADBAND) {
csp_cmd_pos[i] = goal;
} else {
// 限速迈步：命令轨迹只按时间往 goal 逼近
if (err_cmd > 0) csp_cmd_pos[i] += (err_cmd > max_step) ? max_step : err_cmd;
else csp_cmd_pos[i] += (err_cmd < -max_step) ? -max_step : err_cmd;
}
// 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
// const int32_t follow_err = csp_cmd_pos[i] - pv;
// const int32_t FE_LIMIT = (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
// if (std::abs(follow_err) > FE_LIMIT) {
// // 例如：冻结或减半 max_step，给驱动时间追上
// }
// 下发“绝对目标”（不是增量）
EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, csp_cmd_pos[i]);
// 回显给上层（/joint_states_cmd 用）
g_state.pos_cmd[i].store(csp_cmd_pos[i], std::memory_order_relaxed);
break;
// 死区：足够近就贴合到 goal
if (err_cmd > -CSP_POS_DEADBAND && err_cmd < CSP_POS_DEADBAND) {
csp_cmd_pos[i] = goal;
} else {
// 限速迈步：命令轨迹只按时间往 goal 逼近
if (err_cmd > 0) csp_cmd_pos[i] += (err_cmd > max_step) ? max_step : err_cmd;
else csp_cmd_pos[i] += (err_cmd < -max_step) ? -max_step : err_cmd;
}
case CYCLIC_TORQUE: // 10
EC_WRITE_S16(domain1_pd + mt_offsets[i].target_torque, tor_cmd);
break;
default:
// 未知模式：默认速度模式安全置 0
//EC_WRITE_S32(domain1_pd + offsets[i].target_velocity, 0);
break;
// 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
// const int32_t follow_err = csp_cmd_pos[i] - pv;
// const int32_t FE_LIMIT = (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
// if (std::abs(follow_err) > FE_LIMIT) {
// // 例如：冻结或减半 max_step，给驱动时间追上
// }
// 下发“绝对目标”（不是增量）
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]);
// 回显给上层（/joint_states_cmd 用）
g_state.pos_cmd[i].store(csp_cmd_pos[i], std::memory_order_relaxed);
break;
}
} else {
// run_enable=false：清零目标
EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0007);
// CSV 安全：速度清零
EC_WRITE_S32(domain1_pd + mt_offsets[i].target_velocity, 0);
// CSP 安全：目标位置=当前位置 -> 立即“冻结”
int32_t pv_hold = EC_READ_S32(domain1_pd + mt_offsets[i].position_actual_value);
EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, pv_hold);
// 可选：扭矩模式也清零（最好维持急停状态，待修改）
EC_WRITE_S16(domain1_pd + mt_offsets[i].target_torque, 0);
EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
case CYCLIC_TORQUE: // 10
EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, tor_cmd);
break;
default:
// 未知模式：默认速度模式安全置 0
//EC_WRITE_S32(domain1_pd + offsets[i].target_velocity, 0);
break;
}
} else {
// run_enable=false：清零目标
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
// CSV 安全：速度清零
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
// CSP 安全：目标位置=当前位置 -> 立即“冻结”
int32_t pv_hold = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv_hold);
// 可选：扭矩模式也清零（最好维持急停状态，待修改）
EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, 0);
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
}
}
}
if (sync_ref_counter) {
@@ -683,39 +527,19 @@ bool start()
int position = i+1;
// if (i > 0)
// {
// position = i+2;
// }
sc[i] = ecrt_master_slave_config(master, 0, position, ZER_VID_PID);
if (!sc[i])
{
std::cout << "Failed slave cfg at pos " << i << std::endl;
// fprintf(stderr,"Failed slave cfg at pos %d\n", i); g_started.store(false); return false;
}
if (i == 0)
if (ecrt_slave_config_pdos(sc[i], EC_END, zer_device_syncs))
{
sc[i] = ecrt_master_slave_config(master, 0, position, ZER_VID_PID);
if (!sc[i])
{
std::cout << "Failed slave cfg at pos " << i << std::endl;
// fprintf(stderr,"Failed slave cfg at pos %d\n", i); g_started.store(false); return false;
}
if (ecrt_slave_config_pdos(sc[i], EC_END, zer_device_syncs))
{
std::cout << "Failed PDO config " << i << std::endl;
// fprintf(stderr,"[S%02d] Failed PDO config\n", i); g_started.store(false); return false;
}
}
else{
sc[i] = ecrt_master_slave_config(master, 0, position, MT_VID_PID);
if (!sc[i])
{
std::cout << "Failed slave cfg at pos " << i << std::endl;
}
if (ecrt_slave_config_pdos(sc[i], EC_END, mt_device_syncs))
{
std::cout << "Failed PDO config " << i << std::endl;
}
std::cout << "Failed PDO config " << i << std::endl;
// fprintf(stderr,"[S%02d] Failed PDO config\n", i); g_started.store(false); return false;
}
// DC配置
ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 200, 0, 0);
@@ -727,72 +551,34 @@ bool start()
int position = i+1;
// if (i > 1)
// {
// position = i+2;
// }
// ---- CSP/CSV/CST PDO ----
const size_t N = sizeof(zer_domain1_regs)/sizeof(zer_domain1_regs[0]);
ec_pdo_entry_reg_t regs[N];
memcpy(regs, zer_domain1_regs, sizeof(zer_domain1_regs));
for (size_t j = 0; j + 1 < N; ++j)
{
regs[j].alias = 0;
regs[j].position = position;
}
if (i == 0)
{
// ---- CSP/CSV/CST PDO ----
const size_t N = sizeof(zer_domain1_regs)/sizeof(zer_domain1_regs[0]);
ec_pdo_entry_reg_t regs[N];
memcpy(regs, zer_domain1_regs, sizeof(zer_domain1_regs));
for (size_t j = 0; j + 1 < N; ++j)
{
regs[j].alias = 0;
regs[j].position = position;
}
regs[0].offset = &zer_offsets[i].ctrl_word;
regs[1].offset = &zer_offsets[i].target_position;
regs[2].offset = &zer_offsets[i].target_velocity;
regs[3].offset = &zer_offsets[i].target_torque;
regs[4].offset = &zer_offsets[i].max_torque;
regs[5].offset = &zer_offsets[i].operation_mode;
regs[6].offset = &zer_offsets[i].reserved1;
regs[7].offset = &zer_offsets[i].status_word;
regs[8].offset = &zer_offsets[i].position_actual_value;
regs[9].offset = &zer_offsets[i].velocity_actual_value;
regs[10].offset = &zer_offsets[i].torque_actual_value;
regs[11].offset = &zer_offsets[i].error_code;
regs[12].offset = &zer_offsets[i].modes_of_operation_display;
regs[13].offset = &zer_offsets[i].reserved2;
regs[0].offset = &zer_offsets[i].ctrl_word;
regs[1].offset = &zer_offsets[i].target_position;
regs[2].offset = &zer_offsets[i].target_velocity;
regs[3].offset = &zer_offsets[i].target_torque;
regs[4].offset = &zer_offsets[i].max_torque;
regs[5].offset = &zer_offsets[i].operation_mode;
regs[6].offset = &zer_offsets[i].reserved1;
regs[7].offset = &zer_offsets[i].status_word;
regs[8].offset = &zer_offsets[i].position_actual_value;
regs[9].offset = &zer_offsets[i].velocity_actual_value;
regs[10].offset = &zer_offsets[i].torque_actual_value;
regs[11].offset = &zer_offsets[i].error_code;
regs[12].offset = &zer_offsets[i].modes_of_operation_display;
regs[13].offset = &zer_offsets[i].reserved2;
if (ecrt_domain_reg_pdo_entry_list(domain1, regs)) {
fprintf(stderr, "[S%02d] PDO entry reg failed\n", i);
g_started.store(false); return false;
}
}else{
// ---- CSP/CSV/CST PDO ----
const size_t N = sizeof(mt_domain1_regs)/sizeof(mt_domain1_regs[0]);
ec_pdo_entry_reg_t regs[N];
memcpy(regs, mt_domain1_regs, sizeof(mt_domain1_regs));
for (size_t j = 0; j + 1 < N; ++j)
{
regs[j].alias = 0;
regs[j].position = position;
}
regs[0].offset = &mt_offsets[i].ctrl_word;
regs[1].offset = &mt_offsets[i].target_position;
regs[2].offset = &mt_offsets[i].target_velocity;
regs[3].offset = &mt_offsets[i].target_torque;
regs[4].offset = &mt_offsets[i].max_torque;
regs[5].offset = &mt_offsets[i].operation_mode;
regs[6].offset = &mt_offsets[i].reserved1;
regs[7].offset = &mt_offsets[i].status_word;
regs[8].offset = &mt_offsets[i].position_actual_value;
regs[9].offset = &mt_offsets[i].velocity_actual_value;
regs[10].offset = &mt_offsets[i].torque_actual_value;
regs[11].offset = &mt_offsets[i].error_code;
regs[12].offset = &mt_offsets[i].modes_of_operation_display;
regs[13].offset = &mt_offsets[i].reserved2;
if (ecrt_domain_reg_pdo_entry_list(domain1, regs)) {
fprintf(stderr, "[S%02d] PDO entry reg failed\n", i);
g_started.store(false); return false;
}
if (ecrt_domain_reg_pdo_entry_list(domain1, regs)) {
fprintf(stderr, "[S%02d] PDO entry reg failed\n", i);
g_started.store(false); return false;
}
}