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

modify axis number

Browse Source
This commit is contained in:
hivecore
2025-10-23 09:01:09 +08:00
parent dd522ad1e8
commit f55b8f6dfd
3 changed files with 70 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

@@ -72,8 +72,8 @@ public:
"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);
js_pub_ = create_publisher<sensor_msgs::msg::JointState>("ec_joint_states", 10);
js_cmd_pub_ = create_publisher<sensor_msgs::msg::JointState>("ec_joint_states_cmd", 10);
// 表格话题，把“轴i csp指令值/真实位置/误差”拼成一行
csp_table_pub_ = create_publisher<std_msgs::msg::String>("csp_table", 10);
@@ -195,6 +195,37 @@ private:
RCLCPP_WARN(get_logger(), "[S%d] pos needs a value; ignored", idx);
continue;
}
switch (idx)
{
case 0:
idx = 3;
break;
case 1:
idx = 4;
break;
case 2:
idx = 0;
break;
case 3:
idx = 2;
break;
case 4:
idx = 1;
break;
case 5:
idx = 7;
break;
case 6:
idx = 6;
break;
case 7:
idx = 5;
break;
default:
break;
}
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());

36
ethercat_dev/src/ethercat_control/src/motor_control.cpp
View File

@@ -107,9 +107,45 @@ 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);
// This should be done in HAL
// slave index transport into joint index.
switch (idx)
{
case 0:
idx = 3;
break;
case 1:
idx = 4;
break;
case 2:
idx = 0;
break;
case 3:
idx = 2;
break;
case 4:
idx = 1;
break;
case 5:
idx = 7;
break;
case 6:
idx = 6;
break;
case 7:
idx = 5;
break;
default:
break;
}
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);