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

new ecrt_dev

Browse Source
This commit is contained in:
Your Name
2026-01-12 14:55:45 +08:00
parent 93f31592d8
commit c80ca011dd
4 changed files with 91 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -128,8 +128,8 @@ private:
#define CSP_POS_DEADBAND 10
#define CSP_DEADBAND1 100 //CSP允许的误差(计数),原来是10
#define CSP_DEADBAND2 500
#define CSP_SPEED1 20
#define CSP_SPEED2 50
#define CSP_SPEED1 30
#define CSP_SPEED2 200
#define CLOCK_TO_USE CLOCK_MONOTONIC
#define NSEC_PER_SEC (1000000000L)
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) //本次设置周期PERIOD_NS为1ms

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

@@ -2,6 +2,7 @@
#include <math.h>
#define NUM_SLAVES 12
#define ZER_VID_PID 0x5a65726f,0x00029252
#define YY_VID_PID 0x00001097,0x00002406
typedef struct {
unsigned int ctrl_word; // 0x6040:00
unsigned int target_position; // 0x607A:00
@@ -26,7 +27,21 @@ constexpr double rad_to_count= 524288.0/(2*M_PI);
constexpr double count_to_rad=2*M_PI/524288.0;
constexpr double speed_to_count=524288.0/(2*M_PI);//262144.0/(2*M_PI);
constexpr double count_to_speed=2*M_PI/524288.0;//2*M_PI/262144.0;
#define YY_PDO_ENTRY(alias,position,index) \
{alias,position, YY_VID_PID, 0x6040, 0, &zer_offsets[index].ctrl_word,NULL}, \
{alias,position, YY_VID_PID, 0x607A, 0, &zer_offsets[index].target_position,NULL},\
{alias,position, YY_VID_PID, 0x60FF, 0, &zer_offsets[index].target_velocity,NULL},\
{alias,position, YY_VID_PID, 0x6071, 0, &zer_offsets[index].target_torque,NULL},\
{alias,position, YY_VID_PID, 0x6072, 0, &zer_offsets[index].max_torque,NULL},\
{alias,position, YY_VID_PID, 0x6060, 0, &zer_offsets[index].operation_mode,NULL},\
{alias,position, YY_VID_PID, 0xf0ff, 0, &zer_offsets[index].reserved1,NULL},\
{alias,position, YY_VID_PID, 0x6041, 0, &zer_offsets[index].status_word,NULL},\
{alias,position, YY_VID_PID, 0x6064, 0, &zer_offsets[index].position_actual_value,NULL},\
{alias,position, YY_VID_PID, 0x606C, 0, &zer_offsets[index].velocity_actual_value,NULL},\
{alias,position, YY_VID_PID, 0x6077, 0, &zer_offsets[index].torque_actual_value,NULL},\
{alias,position, YY_VID_PID, 0x603F, 0, &zer_offsets[index].error_code,NULL},\
{alias,position, YY_VID_PID, 0x6061, 0, &zer_offsets[index].modes_of_operation_display,NULL},\
{alias,position, YY_VID_PID, 0xf0ff, 0, &zer_offsets[index].reserved2,NULL},
#define PDO_ENTRY(alias,position,index) \
{alias,position, ZER_VID_PID, 0x6040, 0, &zer_offsets[index].ctrl_word,NULL}, \
{alias,position, ZER_VID_PID, 0x607A, 0, &zer_offsets[index].target_position,NULL},\
@@ -43,7 +58,7 @@ constexpr double count_to_speed=2*M_PI/524288.0;//2*M_PI/262144.0;
{alias,position, ZER_VID_PID, 0x6061, 0, &zer_offsets[index].modes_of_operation_display,NULL},\
{alias,position, ZER_VID_PID, 0xf0ff, 0, &zer_offsets[index].reserved2,NULL},
// ------------------- PDO 定义（CSV/CSP/CST），对应 0x1600/0x1A00 -------------------
// 下列结构由IGH主站 cstruct 自动生成
#if 1
const static ec_pdo_entry_reg_t zer_domain1_regs[] = {
PDO_ENTRY(0,1,0)
PDO_ENTRY(0,2,1)
@@ -60,7 +75,25 @@ const static ec_pdo_entry_reg_t zer_domain1_regs[] = {
PDO_ENTRY(0,12,11)
{} // 结束标记
};
#else
const static ec_pdo_entry_reg_t zer_domain1_regs[] = {
YY_PDO_ENTRY(0,1,0)
PDO_ENTRY(0,2,1)
PDO_ENTRY(0,3,2)
PDO_ENTRY(0,4,3)
PDO_ENTRY(0,5,4)
PDO_ENTRY(0,6,5)
PDO_ENTRY(0,7,6)
PDO_ENTRY(0,8,7)
PDO_ENTRY(0,9,8)
PDO_ENTRY(0,10,9)
PDO_ENTRY(0,11,10)
PDO_ENTRY(0,12,11)
PDO_ENTRY(0,13,12)
{} // 结束标记
};
#endif
static ec_pdo_entry_info_t zer_device_pdo_entries[] = {
/*RxPdo 0x1600*/

5
ecrt_dev/src/ethercat_driver.cpp
View File

@@ -511,6 +511,7 @@ void EthercatDriver::readData(){
hw_joint_states_[i][0]=angle;
hw_joint_states_[i][1]=std::round(vel*count_to_speed*1000.0)/1000.0;
double amp=tv*rated_currents[i];
double effort=7.8*amp-19.19;
hw_joint_states_[i][2]=std::round(amp*10.0)/10.0;
}else{
hw_joint_states_[i][0]=9999;
@@ -647,6 +648,10 @@ void EthercatDriver::readData(){
csp_inited[i] = 1;
}
double down_pos=hw_joint_commands_[i][2]*rad_to_count;
if(i==2||i==8){
down_pos=-down_pos;
}
double target_pos=down_pos+pos_offsets[i];
//if(print_cnt%1000==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "value SET [%d],cur:%d,cmd:%.1f,%.1f,dst:%.1f",i,pos,hw_joint_commands_[i][2],down_pos,target_pos);

58
ecrt_dev/src/test_motor.cpp
View File

@@ -35,6 +35,7 @@ constexpr double count_to_rad=2*M_PI/524288.0;
void ControlLoop();
bool activateController(const std::string& controller_name);
void setJointValueTarget(double angle);
void setJointValueTarget(const std::vector<double> angles);
bool motor_drv_on=false;
bool all_motor_op=false;
private:
@@ -237,6 +238,41 @@ void TestMotor::motor_enable(int id,double enable)
gpioPub_->publish(posMsg_);
usleep(1000000);
}
void TestMotor::setJointValueTarget(std::vector<double> angles){
if(client_->wait_for_action_server(std::chrono::seconds(5))){
auto goal=control_msgs::action::FollowJointTrajectory::Goal();
goal.trajectory.joint_names={"joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7","joint_8","joint_9","joint_10","joint_11","joint_12"};
trajectory_msgs::msg::JointTrajectoryPoint point;
for(int i=0;i<12;i++){
std::string joint="joint_"+std::to_string(i+1);
double rad=angles.at(i)*M_PI/180.0;
point.positions.push_back(rad);
printf("D %s: %.2f/%.2f\n",joint.c_str(),curMap_[joint],point.positions[i]);
}
goal.trajectory.points.push_back(point);
auto send_goal_option=rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions();
send_goal_option.goal_response_callback=[this](auto res){
auto goal_handle=res.get();
if(goal_handle){
printf("goal has be accept!!!\n");
}
};
send_goal_option.feedback_callback=[this](auto,auto feedback){
for(int i=0;i<12;i++){
printf("%s,%.3f/%.3f\n",feedback->joint_names[i].c_str(),feedback->desired.positions[i],feedback->actual.positions[i]);
}
};
send_goal_option.result_callback=[this](auto ret){
if(ret.code==rclcpp_action::ResultCode::SUCCEEDED)
printf("action ret succeed\n");
else if(ret.code==rclcpp_action::ResultCode::ABORTED)
printf("action ret aborted\n");
};
client_->async_send_goal(goal,send_goal_option);
}else{
printf("wait action server error\n");
}
}
void TestMotor::setJointValueTarget(double angle){
if(client_->wait_for_action_server(std::chrono::seconds(5))){
auto goal=control_msgs::action::FollowJointTrajectory::Goal();
@@ -245,7 +281,7 @@ void TestMotor::setJointValueTarget(double angle){
for(int i=0;i<12;i++){
std::string joint="joint_"+std::to_string(i+1);
point.positions.push_back(angle);
//////printf("%s: %.2f/%.2f\n",joint.c_str(),curMap_[joint],point.positions[i]);
printf("S %s: %.2f/%.2f\n",joint.c_str(),curMap_[joint],point.positions[i]);
}
goal.trajectory.points.push_back(point);
auto send_goal_option=rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions();
@@ -375,13 +411,17 @@ int main(int argc,char* argv[]){
node->motor_enable(13,1);
std::cout<<"电机使能完毕!\n开始执行角度0...\n";
node->setJointValueTarget(0);
sleep(10);
std::cout<<"角度0执行完毕!\n开始执行角度0.785...\n";
node->setJointValueTarget(0.785);
sleep(10);
std::cout<<"角度0.785执行完毕!\n开始执行复位...\n";
node->setJointValueTarget(0);
sleep(10);
sleep(2);
while(1){
std::cout<<"开始执行角度1.05...\n";
std::vector<double> angles={-40, -40, -40, 0, -40, 0,40, 40, 40, 0, 0, 40};
node->setJointValueTarget(angles);
sleep(15);
std::cout<<"开始执行复位...\n";
node->setJointValueTarget(0);
sleep(15);
}
std::cout<<"复位完毕!\n禁能机械臂电机...\n";
node->motor_enable(13,0);
std::cout<<"禁能机械臂电机完毕!\n";
@@ -391,4 +431,4 @@ int main(int argc,char* argv[]){
rclcpp::shutdown();
spin_thread.join();
return 0;
}
}