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

Merge branch 'main' of ssh://192.168.10.50:2222/HiveCoreRD/hivecore_robot_drivers

Browse Source
This commit is contained in:
NuoDaJia02
2025-10-20 20:09:48 +08:00
parent efd49b99cd afbc867b05
commit 2edd72e610
7 changed files with 399 additions and 274 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
.gitignore vendored
View File

@@ -1,5 +1,10 @@
ethercat_dev/build/**
ethercat_dev/install/**
ethercat_dev/log/**
ethercat_dev/csp_log.csv
# 忽略所有层级的 build 文件夹及其内容
**/build/
# 忽略所有层级的 install 文件夹及其内容
**/install/
# 忽略所有层级的 log 文件夹及其内容
**/log/
ethercat_dev/csp_log.csv

77
.vscode/settings.json vendored Normal file
View File

@@ -0,0 +1,77 @@
{
"files.associations": {
"cctype": "cpp",
"clocale": "cpp",
"cmath": "cpp",
"csignal": "cpp",
"cstdarg": "cpp",
"cstddef": "cpp",
"cstdio": "cpp",
"cstdlib": "cpp",
"cstring": "cpp",
"ctime": "cpp",
"cwchar": "cpp",
"cwctype": "cpp",
"any": "cpp",
"array": "cpp",
"atomic": "cpp",
"bit": "cpp",
"bitset": "cpp",
"chrono": "cpp",
"codecvt": "cpp",
"compare": "cpp",
"complex": "cpp",
"concepts": "cpp",
"condition_variable": "cpp",
"cstdint": "cpp",
"deque": "cpp",
"forward_list": "cpp",
"list": "cpp",
"map": "cpp",
"set": "cpp",
"string": "cpp",
"unordered_map": "cpp",
"unordered_set": "cpp",
"vector": "cpp",
"exception": "cpp",
"algorithm": "cpp",
"functional": "cpp",
"iterator": "cpp",
"memory": "cpp",
"memory_resource": "cpp",
"numeric": "cpp",
"optional": "cpp",
"random": "cpp",
"ratio": "cpp",
"regex": "cpp",
"string_view": "cpp",
"system_error": "cpp",
"tuple": "cpp",
"type_traits": "cpp",
"utility": "cpp",
"fstream": "cpp",
"future": "cpp",
"initializer_list": "cpp",
"iomanip": "cpp",
"iosfwd": "cpp",
"iostream": "cpp",
"istream": "cpp",
"limits": "cpp",
"mutex": "cpp",
"new": "cpp",
"numbers": "cpp",
"ostream": "cpp",
"semaphore": "cpp",
"sstream": "cpp",
"stdexcept": "cpp",
"stop_token": "cpp",
"streambuf": "cpp",
"thread": "cpp",
"cfenv": "cpp",
"cinttypes": "cpp",
"typeindex": "cpp",
"typeinfo": "cpp",
"valarray": "cpp",
"variant": "cpp"
}
}

80
ethercat_dev/src/ethercat_control/include/ethercat_control/et_config.hpp Normal file
View File

@@ -0,0 +1,80 @@
#pragma once
#include "ecrt.h"
#include "ethercat_control/motor_control.hpp" // 电机控制库
#define ET_Device0 0,0 /*EtherCAT address on the bus 别名，位置*/
#define ET_VID_PID 0x00000009,0x26483062 /*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
} et_offset_t;
static et_offset_t et_offsets[NUM_SLAVES];
// ------------------- PDO 定义（CSV/CSP/CST），对应 0x1600/0x1A00 -------------------
// 下列结构由IGH主站 cstruct 自动生成
const static ec_pdo_entry_reg_t et_domain1_regs[] = {
{ET_Device0, ET_VID_PID, 0x6040, 0, &et_offsets[0].ctrl_word}, // 控制字
{ET_Device0, ET_VID_PID, 0x607A, 0, &et_offsets[0].target_position}, // 目标位置
{ET_Device0, ET_VID_PID, 0x60FF, 0, &et_offsets[0].target_velocity}, // 目标速度
{ET_Device0, ET_VID_PID, 0x6071, 0, &et_offsets[0].target_torque}, // 目标扭矩
{ET_Device0, ET_VID_PID, 0x6072, 0, &et_offsets[0].max_torque}, // 最大扭矩
{ET_Device0, ET_VID_PID, 0x6060, 0, &et_offsets[0].operation_mode}, // 运行模式
{ET_Device0, ET_VID_PID, 0x0000, 0, &et_offsets[0].reserved1}, // 保留字段1
{ET_Device0, ET_VID_PID, 0x6041, 0, &et_offsets[0].status_word}, // 状态字
{ET_Device0, ET_VID_PID, 0x6064, 0, &et_offsets[0].position_actual_value}, // 实际位置
{ET_Device0, ET_VID_PID, 0x606C, 0, &et_offsets[0].velocity_actual_value}, // 实际速度
{ET_Device0, ET_VID_PID, 0x6077, 0, &et_offsets[0].torque_actual_value}, // 实际扭矩）
{ET_Device0, ET_VID_PID, 0x603F, 0, &et_offsets[0].error_code}, // 错误代码
// {ET_Device0, ET_VID_PID, 0x6061, 0, &et_offsets[0].modes_of_operation_display}, // 模式显示
// {ET_Device0, ET_VID_PID, 0x5FF2, 0, &et_offsets[0].reserved2}, // 保留字段2
{} // 结束标记
};
static ec_pdo_entry_info_t et_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 */
{0x0000, 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 */
// {0x5ff2, 0x00, 8},
};
static ec_pdo_info_t et_device_pdos[] = {
//RxPdo
{0x1600, 7, et_device_pdo_entries + 0 },
//TxPdo
{0x1A00, 5, et_device_pdo_entries + 7 }
};
static ec_sync_info_t et_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, et_device_pdos + 0, EC_WD_ENABLE},
{3, EC_DIR_INPUT, 1, et_device_pdos + 1, EC_WD_DISABLE},
{0xff}
};

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 = 2; //定义电机数量
constexpr int NUM_SLAVES = 10; //定义电机数量
//运行模式
enum class OpMode : int8_t {

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

@@ -32,6 +32,7 @@
#include "ethercat_control/mt_config.hpp"
#include "ethercat_control/zer_config.hpp"
#include "ethercat_control/et_config.hpp"
// #include "ethercat_control/ds_config.hpp"
@@ -57,7 +58,7 @@
#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 65536 //65536 //CSP最大速度（计数/s）
//#define CSP_MAX_VEL_COUNTS_PER_S 1 //低速测试CSP最大速度（计数）
#define CSP_POS_DEADBAND 10 //CSP允许的误差(计数)
@@ -287,181 +288,6 @@ void cyclic_task()
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);
@@ -469,13 +295,6 @@ void cyclic_task()
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]);
@@ -504,27 +323,20 @@ void cyclic_task()
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) {
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);
// 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;
@@ -532,25 +344,6 @@ void cyclic_task()
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) {
@@ -559,10 +352,16 @@ void cyclic_task()
} else if ((status[i] & command[i]) == 0x0021) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007); // Switched on
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);
} 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) {
} else if ((status[i] & command[i]) == 0x0027) {
// ---- 4) 运行态写目标（按模式/运行意图） ----
if (run_enable) {
//EC_WRITE_U16(domain1_pd + offsets[i].ctrl_word, 0x001F); // 仅在PP模式下需要
@@ -634,6 +433,7 @@ void cyclic_task()
// 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);
// 可选：扭矩模式也清零（最好维持急停状态，待修改）
@@ -641,6 +441,160 @@ void cyclic_task()
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);
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 + 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;
} 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);
} 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) {
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;
}
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 状态
}
}
}
}
@@ -716,22 +670,6 @@ bool start()
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])
@@ -745,6 +683,27 @@ bool start()
std::cout << "Failed PDO config " << i << std::endl;
// fprintf(stderr,"[S%02d] Failed PDO config\n", i); g_started.store(false); return false;
}
}
else{
int position = i+1;
if (i > 5)
{
position = i+3;
}
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;
}
}
// DC配置
@@ -756,39 +715,6 @@ bool start()
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]);
@@ -815,12 +741,48 @@ bool start()
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)
{
int position = i+1;
if (i > 5)
{
position = i+3;
}
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;
}
}
}
// 5) 激活 & 取 process image 指针

4
ethercat_dev/src/ethercat_control/src/ethercat_node.cpp
View File

@@ -126,7 +126,7 @@ private:
si >> idx >> kind; //第一个token为轴号，第二个token为操作类型
if (!si || !mc_valid_index(idx)) {
RCLCPP_WARN(get_logger(), "[set] bad: '%s'", chunk.c_str());
// RCLCPP_WARN(get_logger(), "[set] bad: '%s'", chunk.c_str());
continue;
}
// 第三个Token为参数值
@@ -197,7 +197,7 @@ private:
}
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());
// 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);

3
motor_dev/CMakeLists.txt
View File

@@ -10,7 +10,8 @@ find_package(ament_cmake REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg" "msg/MotorPos.msg")
rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg")
# rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorCmd.msg" "msg/MotorPos.msg")
include_directories(include ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp)
add_executable(motor_dev_node src/main.cpp src/rs485_driver.cpp)
add_dependencies(motor_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp)