add real ethercat driver
This commit is contained in:
NuoDaJia
28
ethercat_dev/readme.txt
Normal file
28
ethercat_dev/readme.txt
Normal file
@@ -0,0 +1,28 @@
|
||||
#！！！程序需要开发板安装RT Linux系统！！！
|
||||
|
||||
#新建mt_ws文件夹，将src文件放在mt_ws下
|
||||
|
||||
#加载环境
|
||||
sudo ethercatctl start
|
||||
source /opt/ros/humble/setup.bash
|
||||
|
||||
#构建（在工作空间下）
|
||||
colcon build --symlink-install
|
||||
|
||||
给可执行文件setcap
|
||||
cmake --build build/ethercat_control --target setcap_ethercat_node
|
||||
|
||||
# 加载工作空间环境
|
||||
source ~/mt_ws/install/setup.bash
|
||||
#运行主程序
|
||||
ros2 run ethercat_control ethercat_node
|
||||
|
||||
#新开终端运行测试指令
|
||||
ros2 topic pub --rate 10 /ethercat/set std_msgs/msg/String "data: '0 pos 0; 1 pos 0; 2 pos 0;3 pos 0'"
|
||||
第一个参数为电机Ethercat上的id
|
||||
第二个参数为模式 分为pos vel tor，对应位置、速度、扭矩模式
|
||||
第三个参数为位置/速度/扭矩设定值
|
||||
|
||||
举例： 0 pos 0 为将0号电机运行至0位
|
||||
1 pos 65535 为将1号电机运行至180度位置
|
||||
|
||||
66
ethercat_dev/src/ethercat_control/CMakeLists.txt
Normal file
66
ethercat_dev/src/ethercat_control/CMakeLists.txt
Normal file
@@ -0,0 +1,66 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(ethercat_control)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
|
||||
# IGH ecrt（如需要，设置自定义路径）
|
||||
find_library(ECRT_LIB
|
||||
NAMES ethercat ecrt
|
||||
PATHS /opt/etherlab/lib /usr/local/lib /usr/lib
|
||||
)
|
||||
if(NOT ECRT_LIB)
|
||||
message(FATAL_ERROR "ecrt not found; set CMAKE_PREFIX_PATH or system ld path.")
|
||||
endif()
|
||||
|
||||
include_directories(include)
|
||||
|
||||
# 核心对象文件
|
||||
add_library(ethercat_core_obj OBJECT
|
||||
src/motor_control.cpp
|
||||
src/ethercat_core.cpp
|
||||
)
|
||||
|
||||
# 静态/共享库（任选其一）
|
||||
add_library(ethercat_core STATIC $<TARGET_OBJECTS:ethercat_core_obj>)
|
||||
target_link_libraries(ethercat_core ${ECRT_LIB} pthread)
|
||||
|
||||
# 节点
|
||||
add_executable(ethercat_node src/ethercat_node.cpp)
|
||||
target_link_libraries(ethercat_node ethercat_core)
|
||||
ament_target_dependencies(ethercat_node rclcpp std_msgs sensor_msgs rcl_interfaces)
|
||||
|
||||
add_executable(ethercat_csp_test src/ethercat_csp_test.cpp)
|
||||
ament_target_dependencies(ethercat_csp_test rclcpp std_msgs)
|
||||
install(TARGETS ethercat_csp_test DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
|
||||
install(TARGETS ethercat_node
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include)
|
||||
# 测试程序
|
||||
add_executable(ethercat_topic_test src/ethercat_topic_test.cpp)
|
||||
ament_target_dependencies(ethercat_topic_test rclcpp std_msgs)
|
||||
|
||||
install(TARGETS ethercat_topic_test
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
|
||||
|
||||
# 创建一个手动触发的目标：为真实二进制设置 cap_sys_nice
|
||||
add_custom_target(setcap_ethercat_node
|
||||
COMMAND ${CMAKE_COMMAND} -E echo "Setting cap_sys_nice on:"
|
||||
COMMAND ${CMAKE_COMMAND} -E echo "$<TARGET_FILE:ethercat_node>"
|
||||
COMMAND sudo setcap cap_sys_nice+ep $<TARGET_FILE:ethercat_node>
|
||||
COMMAND getcap $<TARGET_FILE:ethercat_node>
|
||||
DEPENDS ethercat_node
|
||||
VERBATIM
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,5 @@
|
||||
#pragma once
|
||||
#include <thread>
|
||||
|
||||
void start_rt_cyclic_thread(); // 启动实时线程跑 cyclic_task()
|
||||
void cli_command_loop(); // 在主线程调用：阻塞读命令
|
||||
@@ -0,0 +1,65 @@
|
||||
#pragma once
|
||||
#include "ecrt.h"
|
||||
#include "ethercat_control/motor_control.hpp" // 电机控制库
|
||||
|
||||
#define ZER_Device0 0,0 /*EtherCAT address on the bus 别名，位置*/
|
||||
|
||||
#define VID_PID 0x00202288,0x00000384 /*Vendor ID, product code 厂商ID和产品代码*/
|
||||
|
||||
typedef struct {
|
||||
unsigned int ctrl_word; // 0x6040:00 1
|
||||
unsigned int target_position; // 0x607A:00 1
|
||||
unsigned int target_velocity; // 0x60FF:00 1
|
||||
unsigned int operation_mode; // 0x6060:00 1
|
||||
unsigned int status_word; // 0x6041:00 1
|
||||
unsigned int position_actual_value; // 0x6064:00 1
|
||||
unsigned int velocity_actual_value; // 0x606C:00 1
|
||||
unsigned int error_code; // 0x603F:00 1
|
||||
unsigned int modes_of_operation_display; // 0x6061:00 1
|
||||
} zer_offset_t;
|
||||
|
||||
static zer_offset_t zer_offsets[NUM_SLAVES];
|
||||
|
||||
// ------------------- PDO 定义（CSV/CSP/CST），对应 0x1600/0x1A00 -------------------
|
||||
// 下列结构由IGH主站 cstruct 自动生成
|
||||
const static ec_pdo_entry_reg_t zer_domain1_regs[] = {
|
||||
{ZER_Device0, VID_PID, 0x6040, 0, &zer_offsets[0].ctrl_word}, // 控制字
|
||||
{ZER_Device0, VID_PID, 0x607A, 0, &zer_offsets[0].target_position}, // 目标位置
|
||||
{ZER_Device0, VID_PID, 0x60FF, 0, &zer_offsets[0].target_velocity}, // 目标速度
|
||||
{ZER_Device0, VID_PID, 0x6060, 0, &zer_offsets[0].operation_mode}, // 运行模式
|
||||
{ZER_Device0, VID_PID, 0x6041, 0, &zer_offsets[0].status_word}, // 状态字
|
||||
{ZER_Device0, VID_PID, 0x6064, 0, &zer_offsets[0].position_actual_value}, // 实际位置
|
||||
{ZER_Device0, VID_PID, 0x606C, 0, &zer_offsets[0].velocity_actual_value}, // 实际速度
|
||||
{ZER_Device0, VID_PID, 0x603F, 0, &zer_offsets[0].error_code}, // 错误代码
|
||||
{ZER_Device0, VID_PID, 0x6061, 0, &zer_offsets[0].modes_of_operation_display}, // 模式显示
|
||||
{} // 结束标记
|
||||
};
|
||||
|
||||
static ec_pdo_entry_info_t zer_device_pdo_entries[] = {
|
||||
/*RxPdo 0x1600*/
|
||||
{0x6040, 0x00, 16}, /* control word */
|
||||
{0x607a, 0x00, 32}, /* target position */
|
||||
{0x60ff, 0x00, 32}, /* target velocity */
|
||||
{0x6060, 0x00, 8}, /* modes of operation */
|
||||
/*TxPdo 0x1A00*/
|
||||
{0x6041, 0x00, 16}, /* status word */
|
||||
{0x6064, 0x00, 32}, /* position actual value */
|
||||
{0x606c, 0x00, 32}, /* velocity actual value */
|
||||
{0x603f, 0x00, 16}, /* error code */
|
||||
{0x6061, 0x00, 8}, /* modes of operation display */
|
||||
};
|
||||
|
||||
static ec_pdo_info_t zer_device_pdos[] = {
|
||||
//RxPdo
|
||||
{0x1600, 4, zer_device_pdo_entries + 0 },
|
||||
//TxPdo
|
||||
{0x1A00, 5, zer_device_pdo_entries + 4 }
|
||||
};
|
||||
|
||||
static ec_sync_info_t zer_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, zer_device_pdos + 0, EC_WD_ENABLE},
|
||||
{3, EC_DIR_INPUT, 1, zer_device_pdos + 1, EC_WD_DISABLE},
|
||||
{0xff}
|
||||
};
|
||||
@@ -0,0 +1,14 @@
|
||||
#pragma once
|
||||
#include <stdbool.h>
|
||||
|
||||
namespace ethercat_core {
|
||||
// 初始化 EtherCAT（请求 master、配置 PDO、激活、启动实时循环）。
|
||||
// 返回 true 表示已启动/已在运行；false 表示失败或重复启动。
|
||||
bool start();
|
||||
|
||||
// 请求实时循环退出（由 Ctrl+C 回调调用）
|
||||
void request_exit();
|
||||
|
||||
// （后续添加）停机：拉低 run_enable。实时线程不强退，保持可再次启用。
|
||||
void stop_soft();
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
#pragma once
|
||||
#include <array>
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
#include <algorithm>
|
||||
|
||||
constexpr int NUM_SLAVES = 2; //定义电机数量
|
||||
|
||||
//运行模式
|
||||
enum class OpMode : int8_t {
|
||||
CSP = 8, // Cyclic Synchronous Position
|
||||
CSV = 9, // Cyclic Synchronous Velocity
|
||||
CST = 10, // Cyclic Synchronous Torque
|
||||
PVT = 5 // PVT模式
|
||||
};
|
||||
|
||||
// 周期线程与上层通讯的共享结构
|
||||
// 电机命令
|
||||
struct MotorCommand {
|
||||
std::array<std::atomic<int8_t>, NUM_SLAVES> mode; // 运行模式
|
||||
std::array<std::atomic<int32_t>, NUM_SLAVES> target_vel; // 目标速度
|
||||
std::array<std::atomic<int32_t>, NUM_SLAVES> target_pos; // 目标位置
|
||||
std::array<std::atomic<int16_t>, NUM_SLAVES> target_torque; // 目标扭矩
|
||||
std::atomic<double> pvt_kp[NUM_SLAVES]; // PVT kp增益
|
||||
std::atomic<double> pvt_kd[NUM_SLAVES]; // PVT kd增益
|
||||
|
||||
// 运行控制意图（周期线程据此决定是否写 0x001F）
|
||||
std::array<std::atomic<bool>, NUM_SLAVES> run_enable;
|
||||
|
||||
// 故障复位/急停意图（待后续添加）
|
||||
std::array<std::atomic<bool>, NUM_SLAVES> fault_reset;
|
||||
std::array<std::atomic<bool>, NUM_SLAVES> quick_stop;
|
||||
};
|
||||
// 电机状态
|
||||
struct MotorState {
|
||||
std::array<std::atomic<uint16_t>,NUM_SLAVES> status_word; // 从站状态字
|
||||
std::array<std::atomic<int32_t>, NUM_SLAVES> pos_act; // 实际位置
|
||||
std::array<std::atomic<int32_t>, NUM_SLAVES> vel_act; // 实际速度
|
||||
std::array<std::atomic<int16_t>, NUM_SLAVES> torque_act; // 实际扭矩
|
||||
std::array<std::atomic<int32_t>, NUM_SLAVES> pos_cmd; // 每个周期CSP的目标位置（因速度限制，由target_pos拆分而来）
|
||||
};
|
||||
|
||||
// 全局共享实例（由 .cpp 定义）
|
||||
extern MotorCommand g_cmd;
|
||||
extern MotorState g_state;
|
||||
|
||||
// ---------- 控制接口（供上层/ROS2 调用） ----------
|
||||
|
||||
// 初始化缺省值（上电后调用一次）
|
||||
void mc_init_defaults(OpMode default_mode = OpMode::CSV);
|
||||
|
||||
// 设定单轴模式/目标值/运行意图
|
||||
void mc_set_mode(int idx, OpMode mode);
|
||||
void mc_set_target_velocity(int idx, int32_t vel);
|
||||
void mc_set_target_position(int idx, int32_t pos);
|
||||
void mc_set_target_torque(int idx, int16_t tq);
|
||||
void mc_enable_run(int idx, bool enable);
|
||||
|
||||
// 批量设置电机指令
|
||||
void mc_set_all_mode(OpMode mode);
|
||||
void mc_set_all_target_velocity(int32_t vel);
|
||||
void mc_enable_all(bool enable);
|
||||
|
||||
// 设置PVT参数
|
||||
void mc_set_all_pvt_gains(double kp, double kd);
|
||||
void mc_set_pvt_gains(int axis, double kp, double kd);
|
||||
|
||||
// 故障/急停意图（待后续添加）
|
||||
void mc_fault_reset(int idx);
|
||||
void mc_quick_stop(int idx);
|
||||
|
||||
// 状态读取（从共享状态快照中取，非直接读 PDO）
|
||||
void mc_get_state(int idx, uint16_t &status, int32_t &pos, int32_t &vel, int16_t &torque);
|
||||
|
||||
// 检查电机轴号是否合法
|
||||
inline bool mc_valid_index(int idx) { return idx >= 0 && idx < NUM_SLAVES; }
|
||||
|
||||
// void mc_get_pos_cmd_and_act(int idx, int32_t &pos_cmd, int32_t &pos_act);
|
||||
// void mc_print_pos_cmd_and_act(int idx); // 打印单轴
|
||||
// void mc_print_all_pos_cmd_and_act(); // 打印全部
|
||||
|
||||
@@ -0,0 +1,80 @@
|
||||
#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}
|
||||
};
|
||||
@@ -0,0 +1,80 @@
|
||||
#pragma once
|
||||
#include "ecrt.h"
|
||||
#include "ethercat_control/motor_control.hpp" // 电机控制库
|
||||
|
||||
#define ZER_Device0 0,0 /*EtherCAT address on the bus 别名，位置*/
|
||||
|
||||
#define ZER_VID_PID 0x5a65726f,0x00029252 /*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; // 0xf0ff: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; // 0xf0ff:00
|
||||
} zer_offset_t;
|
||||
|
||||
static zer_offset_t zer_offsets[NUM_SLAVES];
|
||||
|
||||
// ------------------- PDO 定义（CSV/CSP/CST），对应 0x1600/0x1A00 -------------------
|
||||
// 下列结构由IGH主站 cstruct 自动生成
|
||||
const static ec_pdo_entry_reg_t zer_domain1_regs[] = {
|
||||
{ZER_Device0, ZER_VID_PID, 0x6040, 0, &zer_offsets[0].ctrl_word}, // 控制字
|
||||
{ZER_Device0, ZER_VID_PID, 0x607A, 0, &zer_offsets[0].target_position}, // 目标位置
|
||||
{ZER_Device0, ZER_VID_PID, 0x60FF, 0, &zer_offsets[0].target_velocity}, // 目标速度
|
||||
{ZER_Device0, ZER_VID_PID, 0x6071, 0, &zer_offsets[0].target_torque}, // 目标扭矩
|
||||
{ZER_Device0, ZER_VID_PID, 0x6072, 0, &zer_offsets[0].max_torque}, // 最大扭矩
|
||||
{ZER_Device0, ZER_VID_PID, 0x6060, 0, &zer_offsets[0].operation_mode}, // 运行模式
|
||||
{ZER_Device0, ZER_VID_PID, 0xf0ff, 0, &zer_offsets[0].reserved1}, // 保留字段1
|
||||
{ZER_Device0, ZER_VID_PID, 0x6041, 0, &zer_offsets[0].status_word}, // 状态字
|
||||
{ZER_Device0, ZER_VID_PID, 0x6064, 0, &zer_offsets[0].position_actual_value}, // 实际位置
|
||||
{ZER_Device0, ZER_VID_PID, 0x606C, 0, &zer_offsets[0].velocity_actual_value}, // 实际速度
|
||||
{ZER_Device0, ZER_VID_PID, 0x6077, 0, &zer_offsets[0].torque_actual_value}, // 实际扭矩）
|
||||
{ZER_Device0, ZER_VID_PID, 0x603F, 0, &zer_offsets[0].error_code}, // 错误代码
|
||||
{ZER_Device0, ZER_VID_PID, 0x6061, 0, &zer_offsets[0].modes_of_operation_display}, // 模式显示
|
||||
{ZER_Device0, ZER_VID_PID, 0xf0ff, 0, &zer_offsets[0].reserved2}, // 保留字段2
|
||||
{} // 结束标记
|
||||
};
|
||||
|
||||
static ec_pdo_entry_info_t zer_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 */
|
||||
{0xf0ff, 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 */
|
||||
{0xf0ff, 0x00, 8},
|
||||
};
|
||||
|
||||
static ec_pdo_info_t zer_device_pdos[] = {
|
||||
//RxPdo
|
||||
{0x1600, 7, zer_device_pdo_entries + 0 },
|
||||
//TxPdo
|
||||
{0x1A00, 7, zer_device_pdo_entries + 7 }
|
||||
};
|
||||
|
||||
static ec_sync_info_t zer_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, zer_device_pdos + 0, EC_WD_ENABLE},
|
||||
{3, EC_DIR_INPUT, 1, zer_device_pdos + 1, EC_WD_DISABLE},
|
||||
{0xff}
|
||||
};
|
||||
23
ethercat_dev/src/ethercat_control/package.xml
Normal file
23
ethercat_dev/src/ethercat_control/package.xml
Normal file
@@ -0,0 +1,23 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>ethercat_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="you@example.com">orangepi</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>rcl_interfaces</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
265
ethercat_dev/src/ethercat_control/src/cmd_interface.cpp
Normal file
265
ethercat_dev/src/ethercat_control/src/cmd_interface.cpp
Normal file
@@ -0,0 +1,265 @@
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <thread>
|
||||
#include <pthread.h>
|
||||
#include <sched.h>
|
||||
#include <optional> // ✅ 新增
|
||||
#include "motor_control.hpp"
|
||||
|
||||
// 由你现有源码提供：
|
||||
void cyclic_task(); // 实时循环（已接入 g_cmd / g_state）
|
||||
void ethercat_after_init();// 可选：若你想在这里放 mc_init_defaults 等
|
||||
|
||||
// ---------- 模式解析（安全版）：失败返回 nullopt，不做任何电机操作 ----------
|
||||
static std::optional<OpMode> parse_mode(const std::string &s){
|
||||
if (s=="csv" || s=="CSV" || s=="vel" || s=="VEL" || s=="speed" || s=="SPEED")
|
||||
return OpMode::CSV; // 9
|
||||
if (s=="csp" || s=="CSP" || s=="pos" || s=="POS" || s=="position" || s=="POSITION")
|
||||
return OpMode::CSP; // 8
|
||||
if (s=="cst" || s=="CST" || s=="tor" || s=="TOR" || s=="torque" || s=="TORQUE")
|
||||
return OpMode::CST; // 10
|
||||
return std::nullopt; // ❗未知输入 -> 调用者处理，绝不默认 CSV
|
||||
}
|
||||
|
||||
void start_rt_cyclic_thread(){
|
||||
std::thread([]{
|
||||
// 把实时优先级设置到线程上（不占用主线程）
|
||||
sched_param param{};
|
||||
param.sched_priority = sched_get_priority_max(SCHED_FIFO);
|
||||
pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m);
|
||||
cyclic_task(); // 阻塞
|
||||
}).detach();
|
||||
}
|
||||
|
||||
static void print_help(){
|
||||
std::cout <<
|
||||
R"(命令格式：
|
||||
help 显示帮助
|
||||
quit / exit 退出程序
|
||||
enable on|off [i|-1] 使能/禁止运行；省略或 -1 表示所有电机
|
||||
state [i] 打印第 i 台状态（省略打印 0..19 摘要）
|
||||
set <i> csp pos <val>; <j> csv vel <val>; <k> cst tor <val> 同时设置多个轴
|
||||
（别名：csv|vel|speed，csp|pos|position，cst|tor|torque）
|
||||
)"
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
// static void print_help(){
|
||||
// std::cout <<
|
||||
// R"(命令格式：
|
||||
// help 显示帮助
|
||||
// quit / exit 退出程序
|
||||
// enable on|off [i|-1] 使能/禁止运行；省略或 -1 表示所有电机
|
||||
// mode i csv|csp|cst 设置第 i 台的模式（速度/位置/扭矩）
|
||||
// vel i value 设置第 i 台目标速度（int32）
|
||||
// pos i value 设置第 i 台目标位置（int32）
|
||||
// tor i value 设置第 i 台目标扭矩（int16）
|
||||
// all vel value 所有电机目标速度
|
||||
// state [i] 打印第 i 台状态（省略打印 0..19 摘要）
|
||||
// set <i> csp pos <val>; <j> csv vel <val>; <k> cst tor <val> 同时设置多个轴
|
||||
// （别名：csv|vel|speed，csp|pos|position，cst|tor|torque）
|
||||
// )"
|
||||
// << std::endl;
|
||||
// }
|
||||
void cli_command_loop(){
|
||||
print_help();
|
||||
std::string line;
|
||||
while (std::cout << "> " && std::getline(std::cin, line)){
|
||||
std::istringstream iss(line);
|
||||
std::string cmd; iss >> cmd;
|
||||
if (cmd.empty()) continue;
|
||||
|
||||
if (cmd=="help") { print_help(); continue; }
|
||||
if (cmd=="quit" || cmd=="exit") { break; }
|
||||
|
||||
if (cmd=="enable"){
|
||||
std::string onoff; int idx = -1;
|
||||
iss >> onoff >> idx;
|
||||
bool en = (onoff=="on" || onoff=="1" || onoff=="true");
|
||||
if (!iss || idx==-1){
|
||||
mc_enable_all(en);
|
||||
std::cout << "[ALL] enable=" << en << "\n";
|
||||
}else{
|
||||
if (mc_valid_index(idx)){ mc_enable_run(idx,en); std::cout << "[S"<<idx<<"] enable="<<en<<"\n";}
|
||||
else std::cout << "无效索引\n";
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
if (cmd=="mode"){
|
||||
int idx; std::string m; iss >> idx >> m;
|
||||
if (iss && mc_valid_index(idx)){
|
||||
auto pm = parse_mode(m);
|
||||
if (!pm){
|
||||
std::cout << "无效模式: " << m << "（允许: csv/csp/cst 或 vel/pos/tor）\n";
|
||||
continue; // ❗不做任何电机操作
|
||||
}
|
||||
mc_set_mode(idx, *pm);
|
||||
std::cout << "[S"<<idx<<"] mode="<<m<<"\n";
|
||||
}else {
|
||||
std::cout << "用法: mode i csv|csp|cst\n";
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
if (cmd=="vel"){
|
||||
int idx; long v; iss >> idx >> v;
|
||||
if (iss && mc_valid_index(idx)){
|
||||
mc_set_target_velocity(idx, (int32_t)v);
|
||||
std::cout << "[S"<<idx<<"] vel="<<v<<"\n";
|
||||
}else std::cout << "用法: vel i value\n";
|
||||
continue;
|
||||
}
|
||||
|
||||
if (cmd=="pos"){
|
||||
int idx; long p; iss >> idx >> p;
|
||||
if (iss && mc_valid_index(idx)){
|
||||
mc_set_target_position(idx, (int32_t)p);
|
||||
std::cout << "[S"<<idx<<"] pos="<<p<<"\n";
|
||||
}else std::cout << "用法: pos i value\n";
|
||||
continue;
|
||||
}
|
||||
|
||||
if (cmd=="tor"){
|
||||
int idx; long t; iss >> idx >> t;
|
||||
if (iss && mc_valid_index(idx)){
|
||||
mc_set_target_torque(idx, (int16_t)t);
|
||||
std::cout << "[S"<<idx<<"] tor="<<t<<"\n";
|
||||
}else std::cout << "用法: tor i value\n";
|
||||
continue;
|
||||
}
|
||||
|
||||
if (cmd=="all"){
|
||||
std::string what; long v;
|
||||
iss >> what >> v;
|
||||
if (what=="vel" && iss){
|
||||
mc_set_all_target_velocity((int32_t)v);
|
||||
std::cout << "[ALL] vel="<<v<<"\n";
|
||||
}else{
|
||||
std::cout << "用法: all vel value\n";
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
if (cmd == "set") { // 批量控制函数（先校验，后执行）
|
||||
// 把这一行剩余内容整体取出来，然后按 ';' 切成多段
|
||||
std::string rest;
|
||||
std::getline(iss, rest);
|
||||
if (rest.empty()) {
|
||||
std::cout << "用法: set <i> csp pos <val>; <j> csv vel <val>; <k> cst tor <val>\n";
|
||||
continue;
|
||||
}
|
||||
|
||||
// 去掉可能的开头空格/结尾空格
|
||||
auto ltrim = [](std::string& s){
|
||||
size_t p = s.find_first_not_of(" \t");
|
||||
if (p != std::string::npos) s.erase(0, p);
|
||||
};
|
||||
auto rtrim = [](std::string& s){
|
||||
size_t p = s.find_last_not_of(" \t");
|
||||
if (p != std::string::npos) s.erase(p+1);
|
||||
};
|
||||
|
||||
// 按 ';' 分割子命令
|
||||
std::vector<std::string> chunks;
|
||||
{
|
||||
std::stringstream ss(rest);
|
||||
std::string piece;
|
||||
while (std::getline(ss, piece, ';')) {
|
||||
ltrim(piece); rtrim(piece);
|
||||
if (!piece.empty()) chunks.push_back(piece);
|
||||
}
|
||||
}
|
||||
if (chunks.empty()) {
|
||||
std::cout << "用法: set <i> csp pos <val>; <j> csv vel <val>; <k> cst tor <val>\n";
|
||||
continue;
|
||||
}
|
||||
|
||||
// 逐个子命令解析与执行：⚠️ 先校验，再动作；出错则跳过该子命令
|
||||
for (auto& one : chunks) {
|
||||
std::istringstream si(one);
|
||||
int idx = -1;
|
||||
std::string mode_s, field;
|
||||
long long val = 0;
|
||||
|
||||
si >> idx >> mode_s >> field >> val;
|
||||
if (!si || !mc_valid_index(idx)) {
|
||||
std::cout << "[set] 子命令格式错误或轴号越界: \"" << one << "\"\n";
|
||||
continue; // ❗不做任何电机操作
|
||||
}
|
||||
|
||||
// 解析模式
|
||||
auto pm = parse_mode(mode_s);
|
||||
if (!pm) {
|
||||
std::cout << "[set] 未知模式: " << mode_s << " in \"" << one << "\"\n";
|
||||
continue; // ❗不做任何电机操作
|
||||
}
|
||||
OpMode mode = *pm;
|
||||
|
||||
// 校验字段是否与模式匹配
|
||||
bool ok_field = false;
|
||||
if (mode == OpMode::CSP) {
|
||||
ok_field = (field=="pos" || field=="position" || field=="POS" || field=="POSITION");
|
||||
if (!ok_field) {
|
||||
std::cout << "[set] CSP 需要字段: pos <val> ；Got \"" << field << "\"\n";
|
||||
continue; // ❗不做任何电机操作
|
||||
}
|
||||
} else if (mode == OpMode::CSV) {
|
||||
ok_field = (field=="vel" || field=="velocity" || field=="speed" ||
|
||||
field=="VEL" || field=="VELOCITY" || field=="SPEED");
|
||||
if (!ok_field) {
|
||||
std::cout << "[set] CSV 需要字段: vel <val> ；Got \"" << field << "\"\n";
|
||||
continue; // ❗不做任何电机操作
|
||||
}
|
||||
} else if (mode == OpMode::CST) {
|
||||
ok_field = (field=="tor" || field=="torque" || field=="TOR" || field=="TORQUE");
|
||||
if (!ok_field) {
|
||||
std::cout << "[set] CST 需要字段: tor <val> ；Got \"" << field << "\"\n";
|
||||
continue; // ❗不做任何电机操作
|
||||
}
|
||||
}
|
||||
|
||||
// —— 所有校验通过，才真正执行 —— //
|
||||
mc_set_mode(idx, mode);
|
||||
if (mode == OpMode::CSP) {
|
||||
mc_set_target_position(idx, static_cast<int32_t>(val));
|
||||
std::cout << "[S" << idx << "] mode=CSP pos=" << val << "\n";
|
||||
} else if (mode == OpMode::CSV) {
|
||||
mc_set_target_velocity(idx, static_cast<int32_t>(val));
|
||||
std::cout << "[S" << idx << "] mode=CSV vel=" << val << "\n";
|
||||
} else if (mode == OpMode::CST) {
|
||||
mc_set_target_torque(idx, static_cast<int16_t>(val));
|
||||
std::cout << "[S" << idx << "] mode=CST tor=" << val << "\n";
|
||||
}
|
||||
// 是否自动 enable 不在此处改变，保持你当前逻辑；如需可追加：
|
||||
// mc_enable_run(idx, true);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
if (cmd=="state"){
|
||||
int idx=-1; iss >> idx;
|
||||
if (!iss){ // 打印摘要
|
||||
for (int i=0;i<NUM_SLAVES;++i){
|
||||
uint16_t sw; int32_t pos,vel; int16_t tq;
|
||||
mc_get_state(i, sw, pos, vel, tq);
|
||||
std::cout << "[S"<<i<<"] SW=0x"<<std::hex<<sw<<std::dec
|
||||
<<" pos="<<pos<<" vel="<<vel<<" tq="<<tq<<"\n";
|
||||
}
|
||||
}else if (mc_valid_index(idx)){
|
||||
uint16_t sw; int32_t pos,vel; int16_t tq;
|
||||
mc_get_state(idx, sw, pos, vel, tq);
|
||||
std::cout << "[S"<<idx<<"] SW=0x"<<std::hex<<sw<<std::dec
|
||||
<<" pos="<<pos<<" vel="<<vel<<" tq="<<tq<<"\n";
|
||||
}else{
|
||||
std::cout << "索引无效\n";
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
std::cout << "未知命令，输入 help 查看帮助\n";
|
||||
}
|
||||
std::cout << "退出命令行控制。\n";
|
||||
}
|
||||
855
ethercat_dev/src/ethercat_control/src/ethercat_core.cpp
Normal file
855
ethercat_dev/src/ethercat_control/src/ethercat_core.cpp
Normal file
@@ -0,0 +1,855 @@
|
||||
/*
|
||||
* 程序功能为通过EtherCAT周期控制伺服电机
|
||||
*
|
||||
* 功能总览：
|
||||
* 1) 初始化 EtherCAT Master / Domain / 从站配置（PDO/同步管理/DC 时钟）；
|
||||
* 2) 注册 PDO 映射，建立 process image 的偏移（offsets[]）；
|
||||
* 3) 启动 1kHz 实时循环（cyclic_task）：
|
||||
* - 同步 DC 时钟，收发域数据；
|
||||
* - 跟踪各轴 DS402 状态机（Ready->Switched on->Operation enabled）；
|
||||
* - 根据上层 g_cmd（CSV/CSP/CST/PVT）写入目标值到 PDO；
|
||||
|
||||
*/
|
||||
#include <errno.h>
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <sys/resource.h>
|
||||
#include <sys/time.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
#include <time.h>
|
||||
#include <sys/mman.h>
|
||||
#include <malloc.h>
|
||||
#include <sched.h> /* sched_setscheduler() */
|
||||
#include <thread>
|
||||
#include <atomic>
|
||||
#include <pthread.h>
|
||||
#include <cmath> // for std::llround
|
||||
#include <iostream>
|
||||
|
||||
|
||||
|
||||
#include "ethercat_control/mt_config.hpp"
|
||||
#include "ethercat_control/zer_config.hpp"
|
||||
// #include "ethercat_control/ds_config.hpp"
|
||||
|
||||
|
||||
//#include "cmd_interface.hpp" //命令行控制库
|
||||
|
||||
|
||||
// sudo /opt/etherlab/bin/ethercat
|
||||
// sudo /etc/init.d/ethercat start
|
||||
// sudo dmesg | grep EtherCAT
|
||||
// sudo /opt/etherlab/bin/ethercat cstruct -a 0
|
||||
|
||||
// ------------------- 应用参数 -------------------
|
||||
#define FREQUENCY 1000 //控制周期频率1000Hz
|
||||
#define CLOCK_TO_USE CLOCK_MONOTONIC
|
||||
|
||||
//#define MEASURE_TIMING
|
||||
|
||||
//#define NUM_SLAVES 3 //从端数量在motor_control.hpp中定义
|
||||
//#define TARGET_VALUE 10000 //目标位置、速度或者扭矩
|
||||
|
||||
#define CYCLIC_POSITION 8 //CSP 周期同步位置模式
|
||||
#define CYCLIC_VELOCITY 9 //CSP 周期同步速度模式
|
||||
#define CYCLIC_TORQUE 10 //CSP 周期同步扭矩模式
|
||||
#define PVT_MODE 5 //PVT模式
|
||||
|
||||
#define CSP_MAX_VEL_COUNTS_PER_S 65536 //CSP最大速度（计数/s）
|
||||
//#define CSP_MAX_VEL_COUNTS_PER_S 1 //低速测试CSP最大速度（计数）
|
||||
|
||||
#define CSP_POS_DEADBAND 10 //CSP允许的误差(计数)
|
||||
|
||||
#define NSEC_PER_SEC (1000000000L)
|
||||
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) //本次设置周期PERIOD_NS为1ms
|
||||
|
||||
#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \
|
||||
(B).tv_nsec - (A).tv_nsec)
|
||||
|
||||
#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)
|
||||
|
||||
static std::atomic<bool> g_started{false}; //g_started 是一个进程内的“已启动”标志
|
||||
static std::atomic<bool> g_run{true}; //g_run是运行标志，为1时电机可运行
|
||||
|
||||
|
||||
// ------------------- EtherCAT 句柄 -------------------
|
||||
static ec_master_t *master = NULL;
|
||||
static ec_master_state_t master_state = {};
|
||||
|
||||
static ec_domain_t *domain1 = NULL;
|
||||
static ec_domain_state_t domain1_state = {};
|
||||
|
||||
static ec_slave_config_t *sc[NUM_SLAVES] = {0};
|
||||
static ec_slave_config_state_t sc_state[NUM_SLAVES] = {};
|
||||
|
||||
static uint8_t *domain1_pd = NULL; // process data
|
||||
|
||||
static unsigned int counter = 0;
|
||||
static unsigned int sync_ref_counter = 0;
|
||||
const struct timespec cycletime = {0, PERIOD_NS};
|
||||
|
||||
// ------------------- 工具函数 -------------------
|
||||
// 相加时间并处理纳秒进位
|
||||
struct timespec timespec_add(struct timespec time1, struct timespec time2)
|
||||
{
|
||||
struct timespec result;
|
||||
|
||||
if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
|
||||
result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
|
||||
result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
|
||||
} else {
|
||||
result.tv_sec = time1.tv_sec + time2.tv_sec;
|
||||
result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
//查看domain1工作状态
|
||||
void check_domain1_state(void)
|
||||
{
|
||||
ec_domain_state_t ds;
|
||||
|
||||
ecrt_domain_state(domain1, &ds);
|
||||
|
||||
if (ds.working_counter != domain1_state.working_counter)
|
||||
//printf("Domain1: WC %u.\n", ds.working_counter); //当数字变化，表示丢包
|
||||
if (ds.wc_state != domain1_state.wc_state)
|
||||
//printf("Domain1: State %u.\n", ds.wc_state);
|
||||
|
||||
domain1_state = ds;
|
||||
}
|
||||
|
||||
//查看master工作状态
|
||||
void check_master_state(void)
|
||||
{
|
||||
ec_master_state_t ms;
|
||||
|
||||
ecrt_master_state(master, &ms);
|
||||
|
||||
if (ms.slaves_responding != master_state.slaves_responding)
|
||||
//printf("%u slave(s).\n", ms.slaves_responding);
|
||||
if (ms.al_states != master_state.al_states)
|
||||
//printf("AL states: 0x%02X.\n", ms.al_states);
|
||||
if (ms.link_up != master_state.link_up)
|
||||
//printf("Link is %s.\n", ms.link_up ? "up" : "down");
|
||||
|
||||
master_state = ms;
|
||||
}
|
||||
|
||||
//查询并更新每个从站的在线/AL/operational 状态
|
||||
void check_slave_config_states(void)
|
||||
{
|
||||
ec_slave_config_state_t s;
|
||||
for (int i = 0; i < NUM_SLAVES; ++i) {
|
||||
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);
|
||||
if (s.online != sc_state[i].online)
|
||||
//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 ");
|
||||
sc_state[i] = s;
|
||||
}
|
||||
}
|
||||
|
||||
// ------------------- 实时循环 -------------------
|
||||
/**
|
||||
* 1kHz 实时控制主循环
|
||||
*
|
||||
* 核心流程（每周期）：
|
||||
* 1) 绝对时钟休眠到下个周期；写入应用时间到主站；
|
||||
* 2) 接收帧 / 处理域数据；
|
||||
* 3) 遍历每个从站：
|
||||
* - 读取 TxPDO：状态字/实际位置/速度/扭矩/模式显示；
|
||||
* - 把读到的实际值写回共享状态 g_state（给上位机/ROS2）；
|
||||
* - 读取上层命令 g_cmd：期望模式、run_enable、pos/vel/tor、PVT kp/kd；
|
||||
* - 模式切换：降级到perop→写 0x6060→初始化必要变量→进入下周期再跑；
|
||||
* - 跟踪 DS402 状态机，达到 Operation enabled 后根据模式写入：
|
||||
* CSP：写 0x607A；
|
||||
* CSV：写 0x60FF；
|
||||
* CST：写 0x6071；
|
||||
* PVT：写 0x607A/0x60FF/0x6071 及 KP/KD(0x2000/0x2001)；
|
||||
* - run_enable=false：安全处理（速度清零、CSP 对齐当前位置、扭矩清零）。
|
||||
* 4) 同步 DC 参考时钟与从站时钟；队列域数据并发送帧。
|
||||
*
|
||||
*/
|
||||
|
||||
int lastStatus = 0;
|
||||
|
||||
void cyclic_task()
|
||||
{
|
||||
struct timespec wakeupTime, time;
|
||||
#ifdef MEASURE_TIMING
|
||||
struct timespec startTime, endTime, lastStartTime = {};
|
||||
uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0,
|
||||
latency_min_ns = 0, latency_max_ns = 0,
|
||||
period_min_ns = 0, period_max_ns = 0,
|
||||
exec_min_ns = 0, exec_max_ns = 0;
|
||||
#endif
|
||||
|
||||
clock_gettime(CLOCK_TO_USE, &wakeupTime); //当前时间
|
||||
static uint16_t command[NUM_SLAVES]; //状态字掩码
|
||||
static uint16_t status[NUM_SLAVES]; //状态字
|
||||
static uint16_t last_status[NUM_SLAVES]; //上个循环的状态字
|
||||
static int inited = 0; //初始化
|
||||
|
||||
while(g_run.load(std::memory_order_relaxed)) { //主循环
|
||||
wakeupTime = timespec_add(wakeupTime, cycletime);
|
||||
clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); //使用绝对时间，精确等待下一个周期，避免循环执行的时间造成周期漂移
|
||||
|
||||
// Write application time to master
|
||||
//
|
||||
// It is a good idea to use the target time (not the measured time) as
|
||||
// application time, because it is more stable.
|
||||
//
|
||||
ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime)); // 更新主站应用时间
|
||||
|
||||
#ifdef MEASURE_TIMING
|
||||
clock_gettime(CLOCK_TO_USE, &startTime);
|
||||
latency_ns = DIFF_NS(wakeupTime, startTime);
|
||||
period_ns = DIFF_NS(lastStartTime, startTime);
|
||||
exec_ns = DIFF_NS(lastStartTime, endTime);
|
||||
lastStartTime = startTime;
|
||||
|
||||
if (latency_ns > latency_max_ns) {
|
||||
latency_max_ns = latency_ns;
|
||||
}
|
||||
if (latency_ns < latency_min_ns) {
|
||||
latency_min_ns = latency_ns;
|
||||
}
|
||||
if (period_ns > period_max_ns) {
|
||||
period_max_ns = period_ns;
|
||||
}
|
||||
if (period_ns < period_min_ns) {
|
||||
period_min_ns = period_ns;
|
||||
}
|
||||
if (exec_ns > exec_max_ns) {
|
||||
exec_max_ns = exec_ns;
|
||||
}
|
||||
if (exec_ns < exec_min_ns) {
|
||||
exec_min_ns = exec_ns;
|
||||
}
|
||||
#endif
|
||||
// receive process data
|
||||
ecrt_master_receive(master); //接收 EtherCAT 帧
|
||||
ecrt_domain_process(domain1); //处理域数据
|
||||
|
||||
// check process data state (optional)
|
||||
check_domain1_state(); //检查域状态
|
||||
|
||||
if (counter) {
|
||||
counter--;
|
||||
} else { // do this at 1 Hz
|
||||
counter = FREQUENCY;
|
||||
|
||||
// check for master state (optional)
|
||||
check_master_state(); //检查主站状态
|
||||
// check for slave configuration state(s)
|
||||
check_slave_config_states(); //检查从站状态
|
||||
|
||||
#ifdef MEASURE_TIMING
|
||||
// output timing stats
|
||||
printf("period %10u ... %10u\n",
|
||||
period_min_ns, period_max_ns);
|
||||
printf("exec %10u ... %10u\n",
|
||||
exec_min_ns, exec_max_ns);
|
||||
printf("latency %10u ... %10u\n",
|
||||
latency_min_ns, latency_max_ns);
|
||||
printf("status word: 0x%4x\n",status);
|
||||
period_max_ns = 0;
|
||||
period_min_ns = 0xffffffff;
|
||||
exec_max_ns = 0;
|
||||
exec_min_ns = 0xffffffff;
|
||||
latency_max_ns = 0;
|
||||
latency_min_ns = 0xffffffff;
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
if (!inited) {
|
||||
for (int i = 0; i < NUM_SLAVES; ++i) {
|
||||
command[i] = 0x004F;
|
||||
status[i] = 0x000F;
|
||||
last_status[i] = status[i];
|
||||
}
|
||||
inited = 1;
|
||||
}
|
||||
|
||||
static uint8_t csp_initialized[NUM_SLAVES] = {0}; // 进入CSP后是否已对齐过目标（防止冲击）
|
||||
static uint8_t last_mode_cmd_inited = 0;
|
||||
static int8_t last_mode_cmd[NUM_SLAVES] = {CYCLIC_VELOCITY}; // 记录上周期的期望模式（默认为CSV）
|
||||
|
||||
// 读取从站状态字、当前位置、速度、扭矩、运行模式
|
||||
for (int i = 0; i < NUM_SLAVES; ++i)
|
||||
{
|
||||
if (i == 0)
|
||||
{
|
||||
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);
|
||||
|
||||
// std::cout << "sw: " << sw << std::endl;
|
||||
// std::cout << "md: " << md << std::endl;
|
||||
// std::cout << "pv: " << pv << std::endl;
|
||||
// std::cout << "vv: " << vv << std::endl;
|
||||
// std::cout << "tv: " << tv << std::endl;
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// ---- 模式切换处理：先降级到Ready to switch on再设模式 ----
|
||||
|
||||
|
||||
|
||||
if (md != mode_cmd) {
|
||||
|
||||
//检查模式是否改变
|
||||
// std::cout << "md: " << md << std::endl;
|
||||
// std::cout << "mode_cmd: " << mode_cmd << std::endl;
|
||||
// std::cout << "last_mode_cmd: " << last_mode_cmd[i] << std::endl;
|
||||
|
||||
// 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;
|
||||
} else {
|
||||
csp_initialized[i] = 0;
|
||||
}
|
||||
last_mode_cmd[i] = mode_cmd;
|
||||
continue; // 本周期先不再下其他目标，等模式生效
|
||||
}
|
||||
|
||||
// 捕捉从非CSP→CSP 的沿：再次保证目标位置对齐当前实际位置，避免冲击
|
||||
// if (mode_cmd == CYCLIC_POSITION && last_mode_cmd[i] != CYCLIC_POSITION) {
|
||||
// g_cmd.target_pos[i].store(pv, std::memory_order_relaxed);
|
||||
// EC_WRITE_S32(domain1_pd + offsets[i].target_position, pv);
|
||||
// csp_initialized[i] = 1;
|
||||
// last_mode_cmd[i] = mode_cmd;
|
||||
// } else if (last_mode_cmd[i] != mode_cmd) {
|
||||
// // 其它模式切换沿
|
||||
// csp_initialized[i] = 0;
|
||||
// last_mode_cmd[i] = mode_cmd;
|
||||
// }
|
||||
|
||||
// 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;
|
||||
} else if ((status[i] & command[i]) == 0x0021) {
|
||||
EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0007); // Switched on
|
||||
command[i] = 0x006F;
|
||||
} else if ((status[i] & command[i]) == 0x0023) {
|
||||
EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x000f); // Operation enabled
|
||||
command[i] = 0x006F;
|
||||
} 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 + 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);
|
||||
|
||||
// 本周期允许的最大步长（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;
|
||||
}
|
||||
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;
|
||||
|
||||
}
|
||||
} 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 状态
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (i == 1)
|
||||
{
|
||||
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);
|
||||
|
||||
std::cout << "sw: " << sw << std::endl;
|
||||
std::cout << "md: " << md << std::endl;
|
||||
std::cout << "pv: " << pv << std::endl;
|
||||
std::cout << "vv: " << vv << std::endl;
|
||||
std::cout << "tv: " << tv << std::endl;
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// ---- 模式切换处理：先降级到Ready to switch on再设模式 ----
|
||||
|
||||
|
||||
|
||||
if (md != mode_cmd) {
|
||||
|
||||
//检查模式是否改变
|
||||
// std::cout << "md: " << md << std::endl;
|
||||
// std::cout << "mode_cmd: " << mode_cmd << std::endl;
|
||||
// std::cout << "last_mode_cmd: " << last_mode_cmd[i] << std::endl;
|
||||
|
||||
// 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
|
||||
// 进入新模式时做必要初始化
|
||||
if (mode_cmd == CYCLIC_POSITION) {
|
||||
// 初次进入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; // 本周期先不再下其他目标，等模式生效
|
||||
}
|
||||
|
||||
// 捕捉从非CSP→CSP 的沿：再次保证目标位置对齐当前实际位置，避免冲击
|
||||
// if (mode_cmd == CYCLIC_POSITION && last_mode_cmd[i] != CYCLIC_POSITION) {
|
||||
// g_cmd.target_pos[i].store(pv, std::memory_order_relaxed);
|
||||
// EC_WRITE_S32(domain1_pd + offsets[i].target_position, pv);
|
||||
// csp_initialized[i] = 1;
|
||||
// last_mode_cmd[i] = mode_cmd;
|
||||
// } else if (last_mode_cmd[i] != mode_cmd) {
|
||||
// // 其它模式切换沿
|
||||
// csp_initialized[i] = 0;
|
||||
// last_mode_cmd[i] = mode_cmd;
|
||||
// }
|
||||
|
||||
|
||||
if(lastStatus !=(status[i] & command[i]))
|
||||
{
|
||||
printf("[S%02d] 状态: 0x%04X\n", i, (status[i] & command[i]));
|
||||
lastStatus = (status[i] & command[i]);
|
||||
}
|
||||
|
||||
// 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;
|
||||
} else if ((status[i] & command[i]) == 0x0021) {
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007); // Switched on
|
||||
command[i] = 0x006F;
|
||||
} else if ((status[i] & command[i]) == 0x0023) {
|
||||
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f); // Operation enabled
|
||||
command[i] = 0x006F;
|
||||
} 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;
|
||||
}
|
||||
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) {
|
||||
sync_ref_counter--;
|
||||
} else {
|
||||
sync_ref_counter = 1; // sync every 2 cycle
|
||||
|
||||
clock_gettime(CLOCK_TO_USE, &time);
|
||||
ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); //每隔一个周期同步时钟
|
||||
}
|
||||
ecrt_master_sync_slave_clocks(master);
|
||||
|
||||
// send process data
|
||||
ecrt_domain_queue(domain1); //排队域数据
|
||||
ecrt_master_send(master); //发送ETHERCAT帧
|
||||
|
||||
#ifdef MEASURE_TIMING
|
||||
clock_gettime(CLOCK_TO_USE, &endTime);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
// ------------------- 启动/停机接口 -------------------
|
||||
namespace ethercat_core {
|
||||
|
||||
//启动实时线程
|
||||
static void start_rt_cyclic_thread(){
|
||||
std::thread([]{
|
||||
// 锁内存 & 实时优先级
|
||||
mlockall(MCL_CURRENT | MCL_FUTURE); //将当前进程使用的所有内存（MCL_CURRENT）以及将来可能分配的所有内存（MCL_FUTURE）都锁定在物理 RAM 中，禁止将其交换到磁盘（Swap）
|
||||
sched_param param{}; param.sched_priority = sched_get_priority_max(SCHED_FIFO); //将当前线程的调度策略设置为 SCHED_FIFO (First-In, First-Out)，并赋予最高优先级
|
||||
pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m);
|
||||
cyclic_task(); // 进入控制循环（阻塞）
|
||||
}).detach();
|
||||
}
|
||||
|
||||
/**
|
||||
* 初始化 EtherCAT master / domain / PDO / DC 并启动实时循环
|
||||
* return true 成功；false 失败（会复位 g_started 标志）
|
||||
*
|
||||
* 步骤：
|
||||
* 1) ecrt_request_master / ecrt_master_create_domain；
|
||||
* 2) 根据 use_pvt 选择配置 device_syncs 或 device_syncs_pvt；
|
||||
* 3) ecrt_slave_config_dc 启用 DC 同步；
|
||||
* 4) ecrt_domain_reg_pdo_entry_list 注册 PDO entry，保存 offsets[];
|
||||
* 5) ecrt_master_activate + ecrt_domain_data 获取 process image 指针；
|
||||
* 6) 设定上层默认模式（PVT 或 CSV），拉起 run_enable；
|
||||
* 7) 启动实时线程。
|
||||
*/
|
||||
bool start()
|
||||
{
|
||||
printf("Starting EtherCAT master...\n");
|
||||
if (g_started.exchange(true)) {
|
||||
// 已经启动
|
||||
return true;
|
||||
}
|
||||
// 申请 master / 域
|
||||
master = ecrt_request_master(0);
|
||||
if (!master) {
|
||||
perror("ecrt_request_master");
|
||||
g_started.store(false); return false;
|
||||
}
|
||||
domain1 = ecrt_master_create_domain(master);
|
||||
if (!domain1){
|
||||
perror("ecrt_master_create_domain");
|
||||
g_started.store(false); return false;
|
||||
}
|
||||
|
||||
// 从站配置
|
||||
for (int i = 0; i < NUM_SLAVES; ++i) {
|
||||
|
||||
std::cout << "Configuring slave " << i << "..." << std::endl;
|
||||
|
||||
|
||||
if (i == 0)
|
||||
{
|
||||
sc[i] = ecrt_master_slave_config(master, 0, i+1, MT_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, mt_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;
|
||||
}
|
||||
}
|
||||
|
||||
if (i == 1)
|
||||
{
|
||||
sc[i] = ecrt_master_slave_config(master, 0, i+1, 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;
|
||||
}
|
||||
}
|
||||
|
||||
// DC配置
|
||||
ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 200, 0, 0);
|
||||
}
|
||||
|
||||
|
||||
// 注册 PDO entry 偏移
|
||||
for (int i = 0; i < NUM_SLAVES; ++i) {
|
||||
|
||||
if (i == 0)
|
||||
{
|
||||
// ---- 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 = i+1;
|
||||
}
|
||||
|
||||
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 (i == 1)
|
||||
{
|
||||
// ---- 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 = i+1;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// 5) 激活 & 取 process image 指针
|
||||
if (ecrt_master_activate(master)) { perror("ecrt_master_activate"); g_started.store(false); return false; }
|
||||
domain1_pd = ecrt_domain_data(domain1);
|
||||
if (!domain1_pd)
|
||||
{
|
||||
fprintf(stderr,"domain1_pd null\n");
|
||||
g_started.store(false);
|
||||
return false;
|
||||
}
|
||||
|
||||
std::cout << "Starting EtherCAT master done." << std::endl;
|
||||
|
||||
mc_init_defaults(OpMode::CSP);
|
||||
|
||||
mc_enable_all(true);
|
||||
// mc_set_all_target_velocity(0);
|
||||
|
||||
// 启动实时循环线程
|
||||
start_rt_cyclic_thread();
|
||||
return true;
|
||||
}
|
||||
|
||||
void stop_soft(){
|
||||
// 软停：拉低 run_enable；保持 S7 switched on，速度清零
|
||||
mc_enable_all(false);
|
||||
}
|
||||
void request_exit(){
|
||||
g_run.store(false); // 让循环跳出
|
||||
}
|
||||
} // namespace ethercat_core
|
||||
166
ethercat_dev/src/ethercat_control/src/ethercat_csp_test.cpp
Normal file
166
ethercat_dev/src/ethercat_control/src/ethercat_csp_test.cpp
Normal file
@@ -0,0 +1,166 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <std_msgs/msg/string.hpp>
|
||||
#include <sstream>
|
||||
#include <iomanip>
|
||||
#include <cmath>
|
||||
#include <chrono>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
class PosRampNode : public rclcpp::Node {
|
||||
public:
|
||||
PosRampNode() : Node("pos_ramp_node"),
|
||||
steady_clock_(RCL_STEADY_TIME)
|
||||
{
|
||||
// 参数
|
||||
declare_parameter<int>("num_joints", 6); // 轴数
|
||||
declare_parameter<int>("start_pos", 0); // 起始位置（计数）
|
||||
declare_parameter<int>("end_pos", 2000); // 终点位置（计数）
|
||||
declare_parameter<double>("v_max", 8000.0); // 最大速度（counts/s）
|
||||
declare_parameter<double>("a_max", 20000.0); // 最大加速度（counts/s^2）
|
||||
declare_parameter<double>("pause_sec", 5.0); // 暂停时长（s）
|
||||
declare_parameter<int>("rate_hz", 100); // 发布频率（Hz）
|
||||
|
||||
get_parameter("num_joints", num_joints_);
|
||||
get_parameter("start_pos", start_pos_);
|
||||
get_parameter("end_pos", end_pos_);
|
||||
get_parameter("v_max", v_max_);
|
||||
get_parameter("a_max", a_max_);
|
||||
get_parameter("pause_sec", pause_sec_);
|
||||
get_parameter("rate_hz", rate_hz_);
|
||||
|
||||
if (rate_hz_ <= 0) rate_hz_ = 100;
|
||||
dt_ = 1.0 / static_cast<double>(rate_hz_);
|
||||
|
||||
pub_ = create_publisher<std_msgs::msg::String>("ethercat/set", 10);
|
||||
|
||||
// 轨迹状态初始化
|
||||
phase_ = Phase::MoveOut;
|
||||
pos_ = static_cast<double>(start_pos_);
|
||||
vel_ = 0.0;
|
||||
goal_ = static_cast<double>(end_pos_);
|
||||
last_ = steady_clock_.now();
|
||||
|
||||
timer_ = create_wall_timer(
|
||||
std::chrono::milliseconds(static_cast<int>(1000.0 / rate_hz_)),
|
||||
std::bind(&PosRampNode::onTimer, this));
|
||||
|
||||
RCLCPP_INFO(get_logger(),
|
||||
"pos_ramp_node started: num_joints=%d, start=%d, end=%d, v_max=%.1f, a_max=%.1f, pause=%.1fs, rate=%d Hz",
|
||||
num_joints_, start_pos_, end_pos_, v_max_, a_max_, pause_sec_, rate_hz_);
|
||||
}
|
||||
|
||||
private:
|
||||
enum class Phase { MoveOut, HoldOut, MoveBack, HoldStart };
|
||||
|
||||
void onTimer() {
|
||||
// 用稳态时钟计算 dt，尽量跟墙钟定时器保持一致
|
||||
const rclcpp::Time now = steady_clock_.now();
|
||||
double dt = (now - last_).seconds();
|
||||
last_ = now;
|
||||
if (dt <= 0.0) dt = dt_; // 回退到标称步长
|
||||
// 为了轨迹稳定，限制 dt 的异常尖峰
|
||||
if (dt > 3 * dt_) dt = dt_;
|
||||
|
||||
switch (phase_) {
|
||||
case Phase::MoveOut: stepMotion(+1, dt, static_cast<double>(end_pos_), Phase::HoldOut); break;
|
||||
case Phase::HoldOut: stepHold(dt, Phase::MoveBack, static_cast<double>(start_pos_)); break;
|
||||
case Phase::MoveBack: stepMotion(-1, dt, static_cast<double>(start_pos_), Phase::HoldStart);break;
|
||||
case Phase::HoldStart: stepHold(dt, Phase::MoveOut, static_cast<double>(end_pos_)); break;
|
||||
}
|
||||
|
||||
// 发布到 ethercat/set：对所有轴发相同位置
|
||||
const int pos_i = static_cast<int>(std::llround(pos_));
|
||||
std::ostringstream ss;
|
||||
for (int i = 0; i < num_joints_; ++i) {
|
||||
if (i) ss << "; ";
|
||||
ss << i << " pos " << pos_i;
|
||||
}
|
||||
std_msgs::msg::String msg;
|
||||
msg.data = ss.str();
|
||||
pub_->publish(msg);
|
||||
}
|
||||
|
||||
// 运动步进：梯形速度（加速/匀速/减速），避免超调，平滑到达 goal
|
||||
void stepMotion(int dir, double dt, double goal_target, Phase next_hold_phase) {
|
||||
const double s_rem = (goal_target - pos_); // 剩余位移
|
||||
const double sign = (dir >= 0) ? 1.0 : -1.0;
|
||||
const double a = a_max_ * sign;
|
||||
|
||||
// 需要的减速距离 d = v^2 / (2a_max)
|
||||
const double d_decel = (vel_*vel_) / (2.0 * a_max_ + 1e-9);
|
||||
|
||||
// 决策：加速还是减速（或保持）
|
||||
if (sign * s_rem <= d_decel) {
|
||||
// 该减速了
|
||||
vel_ -= a_max_ * sign * dt;
|
||||
// 过零保护
|
||||
if (sign * vel_ < 0.0) vel_ = 0.0;
|
||||
} else {
|
||||
// 加速直到 v_max
|
||||
vel_ += a * dt;
|
||||
const double v_lim = v_max_;
|
||||
if (std::fabs(vel_) > v_lim) vel_ = v_lim * sign;
|
||||
}
|
||||
|
||||
// 积分位置
|
||||
pos_ += vel_ * dt;
|
||||
|
||||
// 过冲保护：若越过目标，截断到目标并进入下一相位
|
||||
if ((sign > 0.0 && pos_ >= goal_target) ||
|
||||
(sign < 0.0 && pos_ <= goal_target)) {
|
||||
pos_ = goal_target;
|
||||
vel_ = 0.0;
|
||||
phase_ = next_hold_phase;
|
||||
hold_t_ = 0.0;
|
||||
goal_ = (next_hold_phase == Phase::HoldOut)
|
||||
? static_cast<double>(end_pos_)
|
||||
: static_cast<double>(start_pos_);
|
||||
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000,
|
||||
"Reached %.3f, entering hold.", goal_target);
|
||||
}
|
||||
}
|
||||
|
||||
// 暂停相位：计时到 pause_sec_ 后切换到下一运动相位，并设置新目标
|
||||
void stepHold(double dt, Phase next_move_phase, double next_goal) {
|
||||
hold_t_ += dt;
|
||||
pos_ = goal_; // 保持
|
||||
vel_ = 0.0;
|
||||
if (hold_t_ >= pause_sec_) {
|
||||
phase_ = next_move_phase;
|
||||
goal_ = next_goal;
|
||||
RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000,
|
||||
"Leaving hold, next goal=%.3f", next_goal);
|
||||
}
|
||||
}
|
||||
|
||||
// ---- members ----
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
rclcpp::Clock steady_clock_;
|
||||
rclcpp::Time last_;
|
||||
|
||||
int num_joints_{6};
|
||||
int start_pos_{0};
|
||||
int end_pos_{2000};
|
||||
int rate_hz_{100};
|
||||
double v_max_{8000.0};
|
||||
double a_max_{20000.0};
|
||||
double pause_sec_{5.0};
|
||||
double dt_{0.01};
|
||||
|
||||
Phase phase_{Phase::MoveOut};
|
||||
double pos_{0.0};
|
||||
double vel_{0.0};
|
||||
double goal_{0.0};
|
||||
double hold_t_{0.0};
|
||||
};
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<PosRampNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
376
ethercat_dev/src/ethercat_control/src/ethercat_node.cpp
Normal file
376
ethercat_dev/src/ethercat_control/src/ethercat_node.cpp
Normal file
@@ -0,0 +1,376 @@
|
||||
/**
|
||||
* ROS2 节点：与 EtherCAT 周期线程共享指令/状态，提供文本话题指令解析、
|
||||
* JointState 发布、CSP 指令回显与 CSV 记录、超时看门狗等。
|
||||
*
|
||||
* 主要功能：
|
||||
* - 启动底层 EtherCAT 周期线程（见 ethercat_core::start）
|
||||
* - 订阅字符串命令话题，解析并写入 MotorCommand（g_cmd）
|
||||
* - 周期发布：
|
||||
* * joint_states（实际位置/速度/扭矩）
|
||||
* * joint_states_cmd（CSP 目标位置的回显）
|
||||
* * csp_table（时间戳+每轴的 csp_cmd/real/err）
|
||||
* - 将表格同时写入 CSV 文件
|
||||
* - 看门狗：一段时间未收到上层命令则将速度/扭矩清零
|
||||
*
|
||||
* 使用示例：
|
||||
* - 切到 CSP 并给 0 号轴位置：
|
||||
* ros2 topic pub /ethercat/set std_msgs/msg/String "data: '0 pos 10000'"
|
||||
* - 切到 CSV 并给 0 号轴速度：
|
||||
* ros2 topic pub /ethercat/set std_msgs/msg/String "data: '0 vel 2000'"
|
||||
* - PVT 模式设置与给值（多条用分号隔开）：
|
||||
* ros2 topic pub --rate 10 /ethercat/set std_msgs/msg/String \
|
||||
* "data: '0 pvt; 0 kp 5; 0 kd 5; 0 pvtpos 10000; 0 pvtvel 2000; 0 pvttor 0'"
|
||||
*/
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
#include "ethercat_control/ethercat_core.hpp"
|
||||
#include "ethercat_control/motor_control.hpp"
|
||||
#include <sstream>
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <filesystem>
|
||||
|
||||
using std::placeholders::_1;
|
||||
|
||||
class EthercatNode : public rclcpp::Node {
|
||||
public:
|
||||
/**
|
||||
* 构造函数：初始化 EtherCAT、参数、话题与定时器
|
||||
*
|
||||
* 过程：
|
||||
* 1) 调用 ethercat_core::start() 启动底层主站与 1kHz 周期线程
|
||||
* 2) 读取参数 num_joints/csv_path
|
||||
* 3) 创建订阅（/ethercat/set）与发布者（joint_states / joint_states_cmd / csp_table）
|
||||
* 4) 创建 100Hz 的状态发布定时器
|
||||
* 5) 打开 CSV 并写表头
|
||||
* 6) 启动看门狗定时器（100ms 检查一次）
|
||||
*
|
||||
*/
|
||||
EthercatNode() : Node("ethercat_node") {
|
||||
// 启动 EtherCAT 周期线程
|
||||
if (!ethercat_core::start()) {
|
||||
RCLCPP_FATAL(this->get_logger(), "EtherCAT core start failed.");
|
||||
throw std::runtime_error("ethercat start failed");
|
||||
}
|
||||
|
||||
// 关节名（给 JointState 用）
|
||||
declare_parameter<int>("num_joints", NUM_SLAVES);
|
||||
get_parameter("num_joints", num_joints_);
|
||||
joint_names_.resize(num_joints_);
|
||||
for (int i = 0; i < num_joints_; ++i) {
|
||||
joint_names_[i] = "joint_" + std::to_string(i);
|
||||
}
|
||||
|
||||
// CSV 文件路径参数，默认存到工作目录下
|
||||
declare_parameter<std::string>("csv_path", "csp_log.csv");
|
||||
get_parameter("csv_path", csv_path_);
|
||||
|
||||
// 订阅 set 文本命令：例如 "set 0 vel 1000; 1 pos 3000"
|
||||
set_sub_ = create_subscription<std_msgs::msg::String>(
|
||||
"ethercat/set", 10, std::bind(&EthercatNode::onSet, this, _1));
|
||||
|
||||
// 发布 JointState（100Hz）
|
||||
js_pub_ = create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
|
||||
js_cmd_pub_ = create_publisher<sensor_msgs::msg::JointState>("joint_states_cmd", 10);
|
||||
|
||||
// 表格话题，把“轴i csp指令值/真实位置/误差”拼成一行
|
||||
csp_table_pub_ = create_publisher<std_msgs::msg::String>("csp_table", 10);
|
||||
|
||||
// 定时器:每 100ms 发布一次状态
|
||||
js_timer_ = create_wall_timer(
|
||||
std::chrono::milliseconds(100),
|
||||
std::bind(&EthercatNode::publishJointState, this));
|
||||
|
||||
// 打开 CSV 并写表头
|
||||
open_csv_and_write_header();
|
||||
start_time_steady_ = steady_clock_.now();
|
||||
|
||||
RCLCPP_INFO(get_logger(), "ethercat_node ready.");
|
||||
|
||||
// 看门狗，每100ms检测指令是否收到
|
||||
last_command_time_ = this->get_clock()->now();
|
||||
watchdog_timer_ = this->create_wall_timer(
|
||||
std::chrono::milliseconds(100),
|
||||
std::bind(&EthercatNode::watchdogCallback, this));
|
||||
}
|
||||
|
||||
// 析构时收尾
|
||||
~EthercatNode() override {
|
||||
if (csv_.is_open()) {
|
||||
csv_.flush();
|
||||
csv_.close();
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
void onSet(const std_msgs::msg::String::SharedPtr msg) {
|
||||
last_command_time_ = this->get_clock()->now(); //刷新看门狗时间
|
||||
// 对读到的控制指令进行处理
|
||||
// 简易去空白工具
|
||||
auto ltrim=[](std::string& s){ auto p=s.find_first_not_of(" \t"); if(p!=std::string::npos) s.erase(0,p); };
|
||||
auto rtrim=[](std::string& s){ auto p=s.find_last_not_of(" \t"); if(p!=std::string::npos) s.erase(p+1); };
|
||||
|
||||
std::stringstream ss(msg->data); //整条命令
|
||||
std::string chunk; //单条命令（整条命令切割而来）
|
||||
|
||||
while (std::getline(ss, chunk, ';')) { //将指令用；分割
|
||||
ltrim(chunk); rtrim(chunk);
|
||||
if (chunk.empty()) continue; //空段跳过
|
||||
|
||||
std::istringstream si(chunk);
|
||||
int idx = -1; // 轴号
|
||||
std::string kind; // 操作类型
|
||||
std::string vstr; // 操作值
|
||||
|
||||
si >> idx >> kind; //第一个token为轴号，第二个token为操作类型
|
||||
if (!si || !mc_valid_index(idx)) {
|
||||
RCLCPP_WARN(get_logger(), "[set] bad: '%s'", chunk.c_str());
|
||||
continue;
|
||||
}
|
||||
// 第三个Token为参数值
|
||||
if (!(si >> vstr)) vstr.clear();
|
||||
|
||||
// 转换函数 字符串转换为long long和double形式
|
||||
auto to_ll = [](const std::string& s)->long long {
|
||||
try { size_t pos=0; long long v = std::stoll(s, &pos, 0); (void)pos; return v; }
|
||||
catch (...) { return 0LL; }
|
||||
};
|
||||
auto to_d = [](const std::string& s)->double {
|
||||
try { size_t pos=0; double v = std::stod(s, &pos); (void)pos; return v; }
|
||||
catch (...) { return 0.0; }
|
||||
};
|
||||
|
||||
// ======= PVT 指令解析处理 =======
|
||||
if (kind == "pvt") {
|
||||
mc_set_mode(idx, OpMode::PVT);
|
||||
RCLCPP_INFO(get_logger(), "[S%d] switch to PVT mode", idx);
|
||||
continue;
|
||||
}
|
||||
else if (kind == "kp" || kind == "pvt_kp") {
|
||||
if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] kp needs a value", idx); continue; }
|
||||
const double kp = to_d(vstr);
|
||||
// 取当前 kd，避免把 kd 覆盖没了
|
||||
const double kd_cur = g_cmd.pvt_kd[idx].load(std::memory_order_relaxed);
|
||||
mc_set_pvt_gains(idx, kp, kd_cur);
|
||||
RCLCPP_INFO(get_logger(), "[S%d] PVT_KP=%.6f (KD=%.6f keep)", idx, kp, kd_cur);
|
||||
continue;
|
||||
}
|
||||
else if (kind == "kd" || kind == "pvt_kd") {
|
||||
if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] kd needs a value", idx); continue; }
|
||||
const double kd = to_d(vstr);
|
||||
// 取当前 kp，避免把 kp 覆盖没了
|
||||
const double kp_cur = g_cmd.pvt_kp[idx].load(std::memory_order_relaxed);
|
||||
mc_set_pvt_gains(idx, kp_cur, kd);
|
||||
RCLCPP_INFO(get_logger(), "[S%d] PVT_KD=%.6f (KP=%.6f keep)", idx, kd, kp_cur);
|
||||
continue;
|
||||
}
|
||||
// 在 PVT 模式下下发目标（为了简单，提供带前缀的别名）
|
||||
else if (kind == "pvtpos" || kind == "pvt_pos") {
|
||||
if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] pvtpos needs a value", idx); continue; }
|
||||
mc_set_mode(idx, OpMode::PVT);
|
||||
mc_set_target_position(idx, static_cast<int32_t>(to_ll(vstr)));
|
||||
RCLCPP_INFO(get_logger(), "[S%d] PVT pos=%s", idx, vstr.c_str());
|
||||
continue;
|
||||
}
|
||||
else if (kind == "pvtvel" || kind == "pvt_vel") {
|
||||
if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] pvtvel needs a value", idx); continue; }
|
||||
mc_set_mode(idx, OpMode::PVT);
|
||||
mc_set_target_velocity(idx, static_cast<int32_t>(to_ll(vstr)));
|
||||
RCLCPP_INFO(get_logger(), "[S%d] PVT vel=%s", idx, vstr.c_str());
|
||||
continue;
|
||||
}
|
||||
else if (kind == "pvttor" || kind == "pvt_tor") {
|
||||
if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] pvttor needs a value", idx); continue; }
|
||||
mc_set_mode(idx, OpMode::PVT);
|
||||
mc_set_target_torque(idx, static_cast<int16_t>(to_ll(vstr)));
|
||||
RCLCPP_INFO(get_logger(), "[S%d] PVT tor=%s", idx, vstr.c_str());
|
||||
continue;
|
||||
}
|
||||
|
||||
// ======= CSP / CSV / CST 指令解析处理 =======
|
||||
if (kind=="pos" || kind=="position") {
|
||||
if (vstr.empty()) {
|
||||
RCLCPP_WARN(get_logger(), "[S%d] pos needs a value; ignored", idx);
|
||||
continue;
|
||||
}
|
||||
mc_set_mode(idx, OpMode::CSP);
|
||||
mc_set_target_position(idx, static_cast<int32_t>(to_ll(vstr)));
|
||||
RCLCPP_INFO(get_logger(), "[S%d] CSP pos=%s", idx, vstr.c_str());
|
||||
} else if (kind=="vel" || kind=="velocity" || kind=="speed") {
|
||||
if (vstr.empty()) {
|
||||
RCLCPP_WARN(get_logger(), "[S%d] vel needs a value; ignored", idx);
|
||||
continue;
|
||||
}
|
||||
mc_set_mode(idx, OpMode::CSV);
|
||||
mc_set_target_velocity(idx, static_cast<int32_t>(to_ll(vstr)));
|
||||
RCLCPP_INFO(get_logger(), "[S%d] CSV vel=%s", idx, vstr.c_str());
|
||||
} else if (kind=="tor" || kind=="torque") {
|
||||
if (vstr.empty()) {
|
||||
RCLCPP_WARN(get_logger(), "[S%d] tor needs a value; ignored", idx);
|
||||
continue;
|
||||
}
|
||||
mc_set_mode(idx, OpMode::CST);
|
||||
mc_set_target_torque(idx, static_cast<int16_t>(to_ll(vstr)));
|
||||
RCLCPP_INFO(get_logger(), "[S%d] CST tor=%s", idx, vstr.c_str());
|
||||
} else if (kind=="ena" || kind=="enable") {
|
||||
if (vstr.empty()) {
|
||||
RCLCPP_WARN(get_logger(), "[S%d] tor needs a value; ignored", idx);
|
||||
continue;
|
||||
}
|
||||
mc_enable_run(idx, to_ll(vstr));
|
||||
RCLCPP_INFO(get_logger(), "[S%d] enable=%s", idx, vstr.c_str());
|
||||
}else {
|
||||
RCLCPP_WARN(get_logger(), "[set] unknown field: '%s'", kind.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 计时：以节点启动时为零，使用稳态时钟
|
||||
rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
|
||||
rclcpp::Time start_time_steady_{0, 0, RCL_STEADY_TIME};
|
||||
|
||||
// 关节状态发布（实际值和命令值）
|
||||
void publishJointState() {
|
||||
// 实际值
|
||||
sensor_msgs::msg::JointState js;
|
||||
js.header.stamp = now();
|
||||
js.name = joint_names_;
|
||||
js.position.resize(num_joints_);
|
||||
js.velocity.resize(num_joints_);
|
||||
js.effort.resize(num_joints_);
|
||||
for (int i = 0; i < num_joints_; ++i) {
|
||||
uint16_t sw; int32_t pos, vel; int16_t tq;
|
||||
mc_get_state(i, sw, pos, vel, tq);
|
||||
js.position[i] = static_cast<double>(pos);
|
||||
js.velocity[i] = static_cast<double>(vel);
|
||||
js.effort[i] = static_cast<double>(tq);
|
||||
}
|
||||
js_pub_->publish(js);
|
||||
|
||||
// 命令位置（CSP 回显），与实际值用同一时间戳
|
||||
sensor_msgs::msg::JointState js_cmd;
|
||||
js_cmd.header.stamp = js.header.stamp;
|
||||
js_cmd.name = joint_names_;
|
||||
js_cmd.position.resize(num_joints_);
|
||||
for (int i = 0; i < num_joints_; ++i) {
|
||||
const int32_t pc = g_state.pos_cmd[i].load(std::memory_order_relaxed); // 读取csp值
|
||||
js_cmd.position[i] = static_cast<double>(pc);
|
||||
}
|
||||
js_cmd_pub_->publish(js_cmd);
|
||||
|
||||
// 计算相对启动的时间（ms），并量化到 0.1 ms
|
||||
const rclcpp::Duration dt = steady_clock_.now() - start_time_steady_;
|
||||
double t_ms = dt.seconds() * 1000.0;
|
||||
t_ms = std::round(t_ms * 10.0) / 10.0; // 四舍五入到 0.1ms
|
||||
|
||||
// 生成“表格行”：timestamp,轴1csp指令值,轴1真实位置,误差,轴2csp指令值,轴2真实位置,误差,...
|
||||
std::ostringstream row;
|
||||
row.setf(std::ios::fixed);
|
||||
// 先写时间戳
|
||||
row << std::setprecision(1) << t_ms;
|
||||
|
||||
if (num_joints_ > 0) row << ",";
|
||||
for (int i = 0; i < num_joints_; ++i) {
|
||||
const long long csp = static_cast<long long>(js_cmd.position[i]);
|
||||
const long long act = static_cast<long long>(js.position[i]);
|
||||
const long long err = csp - act;
|
||||
row << csp << ',' << act << ',' << err;
|
||||
if (i != num_joints_ - 1) row << ',';
|
||||
}
|
||||
const std::string line = row.str();
|
||||
|
||||
// 发布到 csp_table 话题
|
||||
std_msgs::msg::String msg;
|
||||
msg.data = line;
|
||||
csp_table_pub_->publish(msg);
|
||||
|
||||
// 写入 CSV（首行已写表头）
|
||||
if (csv_.is_open()) {
|
||||
csv_ << line << '\n';
|
||||
if ((++csv_lines_ % 100) == 0) csv_.flush(); // 每100行刷一次盘，减轻开销
|
||||
}
|
||||
}
|
||||
|
||||
// 打开 CSV 并写表头
|
||||
void open_csv_and_write_header() {
|
||||
try {
|
||||
std::filesystem::path p(csv_path_);
|
||||
if (p.has_parent_path()) {
|
||||
std::error_code ec;
|
||||
std::filesystem::create_directories(p.parent_path(), ec);
|
||||
if (ec) {
|
||||
RCLCPP_WARN(get_logger(), "Failed to create dir for CSV: %s", ec.message().c_str());
|
||||
}
|
||||
}
|
||||
csv_.open(csv_path_, std::ios::out | std::ios::trunc);
|
||||
if (!csv_) {
|
||||
RCLCPP_ERROR(get_logger(), "Cannot open CSV file: %s", csv_path_.c_str());
|
||||
return;
|
||||
}
|
||||
// 表头：轴1csp指令值,轴1真实位置,误差,轴2csp指令值,...
|
||||
std::ostringstream header;
|
||||
header << "time(ms)";
|
||||
if (num_joints_ > 0) header << ","; // 后面还有列再加逗号
|
||||
for (int i = 0; i < num_joints_; ++i) {
|
||||
header << "axis" << (i+1) << "csp_command"
|
||||
<< "," << "axis" << (i+1) << "real_position"
|
||||
<< "," << "error";
|
||||
if (i != num_joints_ - 1) header << ",";
|
||||
}
|
||||
csv_ << header.str() << '\n';
|
||||
csv_.flush();
|
||||
|
||||
// 同步把表头也发到 topic
|
||||
std_msgs::msg::String head_msg;
|
||||
head_msg.data = header.str();
|
||||
csp_table_pub_->publish(head_msg);
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Logging CSP table to: %s", csv_path_.c_str());
|
||||
} catch (const std::exception& e) {
|
||||
RCLCPP_ERROR(get_logger(), "open_csv_and_write_header exception: %s", e.what());
|
||||
}
|
||||
}
|
||||
|
||||
int num_joints_{0};
|
||||
std::vector<std::string> joint_names_;
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr set_sub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr js_pub_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr js_cmd_pub_;
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr csp_table_pub_; // NEW
|
||||
rclcpp::TimerBase::SharedPtr js_timer_;
|
||||
rclcpp::TimerBase::SharedPtr watchdog_timer_;
|
||||
rclcpp::Time last_command_time_;
|
||||
|
||||
// CSV
|
||||
std::string csv_path_;
|
||||
std::ofstream csv_;
|
||||
size_t csv_lines_{0};
|
||||
|
||||
//看门狗
|
||||
void watchdogCallback() {
|
||||
const rclcpp::Time now = this->get_clock()->now();
|
||||
const rclcpp::Duration dt = now - last_command_time_;
|
||||
if (dt.seconds() > 0.2) {
|
||||
//RCLCPP_WARN(get_logger(), "Command timeout! Stopping all motors."); //看门狗超时信息
|
||||
for (int i = 0; i < num_joints_; ++i) {
|
||||
mc_set_target_velocity(i, 0);
|
||||
mc_set_target_torque(i, 0);
|
||||
}
|
||||
last_command_time_ = now;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char** argv){
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::on_shutdown([]{
|
||||
ethercat_core::stop_soft();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
ethercat_core::request_exit();
|
||||
});
|
||||
rclcpp::spin(std::make_shared<EthercatNode>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
157
ethercat_dev/src/ethercat_control/src/ethercat_topic_test.cpp
Normal file
157
ethercat_dev/src/ethercat_control/src/ethercat_topic_test.cpp
Normal file
@@ -0,0 +1,157 @@
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "std_msgs/msg/string.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
class EthercatVelLoop : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
EthercatVelLoop() : Node("ethercat_vel_loop")
|
||||
{
|
||||
// 参数
|
||||
topic_ = declare_parameter<std::string>("topic", "/ethercat/set");
|
||||
max_vel_ = declare_parameter<int>("max_vel", 30000);
|
||||
|
||||
// 兼容旧参数名 forward_duration_sec（如果外部还在用）
|
||||
double forward_duration = declare_parameter<double>("forward_duration_sec", 3.0);
|
||||
ramp_duration_ = declare_parameter<double>("ramp_duration_sec", forward_duration);
|
||||
|
||||
decel_duration_ = declare_parameter<double>("decel_duration_sec", 1.0);
|
||||
stop_duration_ = declare_parameter<double>("stop_duration_sec", 1.0);
|
||||
rate_hz_ = declare_parameter<double>("rate_hz", 10.0);
|
||||
motor_ids_ = declare_parameter<std::vector<int64_t>>("motor_ids", {0, 1, 2});
|
||||
|
||||
if (rate_hz_ <= 0.0) {
|
||||
RCLCPP_WARN(get_logger(), "rate_hz <= 0, force to 10 Hz");
|
||||
rate_hz_ = 10.0;
|
||||
}
|
||||
|
||||
pub_ = create_publisher<std_msgs::msg::String>(topic_, 10);
|
||||
|
||||
timer_period_ = std::chrono::duration<double>(1.0 / rate_hz_);
|
||||
timer_ = create_wall_timer(
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(timer_period_),
|
||||
std::bind(&EthercatVelLoop::onTimer, this));
|
||||
|
||||
RCLCPP_INFO(get_logger(),
|
||||
"Started ethercat_vel_loop: topic=%s, max_vel=%d, ramp=%.3fs, decel=%.3fs, stop=%.3fs, rate=%.1fHz",
|
||||
topic_.c_str(), max_vel_, ramp_duration_, decel_duration_, stop_duration_, rate_hz_);
|
||||
}
|
||||
|
||||
private:
|
||||
enum class Phase { FWD_ACCEL, FWD_DECEL, STOP1, REV_ACCEL, REV_DECEL, STOP2 };
|
||||
|
||||
void onTimer()
|
||||
{
|
||||
time_in_phase_ += std::chrono::duration<double>(timer_period_).count();
|
||||
|
||||
int target_vel = 0;
|
||||
|
||||
switch (phase_) {
|
||||
case Phase::FWD_ACCEL: {
|
||||
// 0 -> +max_vel_，线性加速，持续 ramp_duration_
|
||||
double frac = ramp_duration_ > 0.0 ? std::min(1.0, time_in_phase_ / ramp_duration_) : 1.0;
|
||||
target_vel = static_cast<int>(std::round(max_vel_ * frac));
|
||||
if (time_in_phase_ >= ramp_duration_) {
|
||||
// 加速完成后：如果需要减速缓冲，进入减速段，否则直接停
|
||||
if (decel_duration_ > 0.0) switchPhase(Phase::FWD_DECEL);
|
||||
else switchPhase(Phase::STOP1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case Phase::FWD_DECEL: {
|
||||
// +max_vel_ -> 0，线性减速，持续 decel_duration_
|
||||
double frac = decel_duration_ > 0.0 ? std::min(1.0, time_in_phase_ / decel_duration_) : 1.0;
|
||||
target_vel = static_cast<int>(std::round(max_vel_ * (1.0 - frac)));
|
||||
if (time_in_phase_ >= decel_duration_) {
|
||||
switchPhase(Phase::STOP1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case Phase::STOP1: {
|
||||
target_vel = 0;
|
||||
if (time_in_phase_ >= stop_duration_) {
|
||||
switchPhase(Phase::REV_ACCEL);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case Phase::REV_ACCEL: {
|
||||
// 0 -> -max_vel_，线性加速，持续 ramp_duration_
|
||||
double frac = ramp_duration_ > 0.0 ? std::min(1.0, time_in_phase_ / ramp_duration_) : 1.0;
|
||||
target_vel = -static_cast<int>(std::round(max_vel_ * frac));
|
||||
if (time_in_phase_ >= ramp_duration_) {
|
||||
if (decel_duration_ > 0.0) switchPhase(Phase::REV_DECEL);
|
||||
else switchPhase(Phase::STOP2);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case Phase::REV_DECEL: {
|
||||
// -max_vel_ -> 0，线性减速，持续 decel_duration_
|
||||
double frac = decel_duration_ > 0.0 ? std::min(1.0, time_in_phase_ / decel_duration_) : 1.0;
|
||||
target_vel = -static_cast<int>(std::round(max_vel_ * (1.0 - frac)));
|
||||
if (time_in_phase_ >= decel_duration_) {
|
||||
switchPhase(Phase::STOP2);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case Phase::STOP2: {
|
||||
target_vel = 0;
|
||||
if (time_in_phase_ >= stop_duration_) {
|
||||
switchPhase(Phase::FWD_ACCEL);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 拼装字符串："0 vel XXX; 1 vel XXX; 2 vel XXX"
|
||||
std::ostringstream oss;
|
||||
for (size_t i = 0; i < motor_ids_.size(); ++i) {
|
||||
if (i > 0) oss << "; ";
|
||||
oss << motor_ids_[i] << " vel " << target_vel;
|
||||
}
|
||||
|
||||
std_msgs::msg::String msg;
|
||||
msg.data = oss.str();
|
||||
pub_->publish(msg);
|
||||
}
|
||||
|
||||
void switchPhase(Phase next)
|
||||
{
|
||||
phase_ = next;
|
||||
time_in_phase_ = 0.0;
|
||||
}
|
||||
|
||||
// 参数
|
||||
std::string topic_;
|
||||
int max_vel_;
|
||||
double ramp_duration_;
|
||||
double decel_duration_;
|
||||
double stop_duration_;
|
||||
double rate_hz_;
|
||||
std::vector<int64_t> motor_ids_;
|
||||
|
||||
// 发布与定时
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
std::chrono::duration<double> timer_period_;
|
||||
|
||||
// 状态机
|
||||
Phase phase_{Phase::FWD_ACCEL};
|
||||
double time_in_phase_{0.0};
|
||||
};
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<EthercatVelLoop>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
117
ethercat_dev/src/ethercat_control/src/motor_control.cpp
Normal file
117
ethercat_dev/src/ethercat_control/src/motor_control.cpp
Normal file
@@ -0,0 +1,117 @@
|
||||
/**
|
||||
* ROS2上位机和实时循环的通信桥梁
|
||||
*
|
||||
* 模块功能：
|
||||
* - 读取实时循环状态 g_state
|
||||
* - 给实时循环发送指令 g_cmd
|
||||
*
|
||||
* 线程模型与并发：
|
||||
* - 多生产者（ROS2 回调/控制线程） --> 写 g_cmd / 读 g_state；
|
||||
* - 单消费者（1kHz 实时循环） --> 读 g_cmd / 写 g_state。
|
||||
* - 全部原子变量使用 memory_order_relaxed，低延迟、低开销；
|
||||
*
|
||||
* 数据与单位约定（与从站字典保持一致）：
|
||||
* - 位置：编码器计数（counts）
|
||||
* - 速度：counts/s
|
||||
* - 扭矩：
|
||||
* - 模式：DS402 0x6060（CSP=8, CSV=9, CST=10, PVT=5）
|
||||
* - PVT 增益：这里以 double 表示的无量纲系数
|
||||
*
|
||||
*/
|
||||
#include "ethercat_control/motor_control.hpp"
|
||||
|
||||
MotorCommand g_cmd; //全局命令缓冲
|
||||
MotorState g_state; //全局状态缓冲
|
||||
|
||||
static inline int clamp_index(int idx) { return std::clamp(idx, 0, NUM_SLAVES - 1); }
|
||||
|
||||
// 初始化所有轴的缺省命令与状态
|
||||
void mc_init_defaults(OpMode default_mode) {
|
||||
for (int i = 0; i < NUM_SLAVES; ++i) {
|
||||
g_cmd.target_vel[i].store(0, std::memory_order_relaxed);
|
||||
g_cmd.target_pos[i].store(0, std::memory_order_relaxed);
|
||||
g_cmd.target_torque[i].store(0, std::memory_order_relaxed);
|
||||
g_cmd.mode[i].store(static_cast<int8_t>(default_mode), std::memory_order_relaxed);
|
||||
g_cmd.run_enable[i].store(false, std::memory_order_relaxed);
|
||||
g_cmd.fault_reset[i].store(false, std::memory_order_relaxed);
|
||||
g_cmd.quick_stop[i].store(false, std::memory_order_relaxed);
|
||||
|
||||
g_state.status_word[i].store(0, std::memory_order_relaxed);
|
||||
g_state.pos_act[i].store(0, std::memory_order_relaxed);
|
||||
g_state.vel_act[i].store(0, std::memory_order_relaxed);
|
||||
g_state.torque_act[i].store(0, std::memory_order_relaxed);
|
||||
g_state.pos_cmd[i].store(0, std::memory_order_relaxed);
|
||||
// 新增：PVT 缺省增益（可按需改）
|
||||
g_cmd.pvt_kp[i].store(0.0, std::memory_order_relaxed);
|
||||
g_cmd.pvt_kd[i].store(0.0, std::memory_order_relaxed);
|
||||
|
||||
}
|
||||
}
|
||||
//设置工作模式（轴号，操作模式）
|
||||
void mc_set_mode(int idx, OpMode mode) {
|
||||
if (!mc_valid_index(idx)) return;
|
||||
g_cmd.mode[idx].store(static_cast<int8_t>(mode), std::memory_order_relaxed);
|
||||
}
|
||||
//设置速度（轴号，速度）
|
||||
void mc_set_target_velocity(int idx, int32_t vel) {
|
||||
if (!mc_valid_index(idx)) return;
|
||||
g_cmd.target_vel[idx].store(vel, std::memory_order_relaxed);
|
||||
}
|
||||
//设置位置（轴号，位置）
|
||||
void mc_set_target_position(int idx, int32_t pos) {
|
||||
if (!mc_valid_index(idx)) return;
|
||||
g_cmd.target_pos[idx].store(pos, std::memory_order_relaxed);
|
||||
}
|
||||
//设置扭矩（轴号，扭矩）
|
||||
void mc_set_target_torque(int idx, int16_t tq) {
|
||||
if (!mc_valid_index(idx)) return;
|
||||
g_cmd.target_torque[idx].store(tq, std::memory_order_relaxed);
|
||||
}
|
||||
// 设置所有电机PVT kp kd
|
||||
void mc_set_all_pvt_gains(double kp, double kd) {
|
||||
for (int i = 0; i < NUM_SLAVES; ++i) {
|
||||
g_cmd.pvt_kp[i].store(kp, std::memory_order_relaxed);
|
||||
g_cmd.pvt_kd[i].store(kd, std::memory_order_relaxed);
|
||||
}
|
||||
}
|
||||
// 设置PVT kp kd （轴号，kp，kd）
|
||||
void mc_set_pvt_gains(int axis, double kp, double kd) {
|
||||
if (axis < 0 || axis >= NUM_SLAVES) return;
|
||||
g_cmd.pvt_kp[axis].store(kp, std::memory_order_relaxed);
|
||||
g_cmd.pvt_kd[axis].store(kd, std::memory_order_relaxed);
|
||||
}
|
||||
//使能/禁止运行（轴号）
|
||||
void mc_enable_run(int idx, bool enable) {
|
||||
if (!mc_valid_index(idx)) return;
|
||||
g_cmd.run_enable[idx].store(enable, std::memory_order_relaxed);
|
||||
}
|
||||
//设置所有电机运行模式（模式）
|
||||
void mc_set_all_mode(OpMode mode) {
|
||||
for (int i = 0; i < NUM_SLAVES; ++i) mc_set_mode(i, mode);
|
||||
}
|
||||
//设置所有电机速度（速度）
|
||||
void mc_set_all_target_velocity(int32_t vel) {
|
||||
for (int i = 0; i < NUM_SLAVES; ++i) mc_set_target_velocity(i, vel);
|
||||
}
|
||||
//批量使能/禁止运行
|
||||
void mc_enable_all(bool enable) {
|
||||
for (int i = 0; i < NUM_SLAVES; ++i) mc_enable_run(i, enable);
|
||||
}
|
||||
//故障复位，待开发
|
||||
void mc_fault_reset(int idx) {
|
||||
if (!mc_valid_index(idx)) return;
|
||||
g_cmd.fault_reset[idx].store(true, std::memory_order_relaxed);
|
||||
}
|
||||
//急停（轴号）
|
||||
void mc_quick_stop(int idx) {
|
||||
if (!mc_valid_index(idx)) return;
|
||||
g_cmd.quick_stop[idx].store(true, std::memory_order_relaxed);
|
||||
}
|
||||
//读取轴状态（轴号，状态字，位置，速度，扭矩）
|
||||
void mc_get_state(int idx, uint16_t &status, int32_t &pos, int32_t &vel, int16_t &torque) {
|
||||
idx = clamp_index(idx);
|
||||
status = g_state.status_word[idx].load(std::memory_order_relaxed);
|
||||
pos = g_state.pos_act[idx].load(std::memory_order_relaxed);
|
||||
vel = g_state.vel_act[idx].load(std::memory_order_relaxed);
|
||||
torque = g_state.torque_act[idx].load(std::memory_order_relaxed);
|
||||
}
|
||||
Reference in New Issue
Block a user