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

split read write& solve data error

Browse Source
This commit is contained in:
Your Name
2026-01-21 10:57:14 +08:00
parent 3cc5a6a95f
commit c6deefe96d
6 changed files with 115 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
ecrt_dev/config/controllers.yaml
View File

@@ -111,36 +111,48 @@ gpio_command_controller:
left_arm_joint_1:
interfaces:
- fault
- enable
left_arm_joint_2:
interfaces:
- fault
- enable
left_arm_joint_3:
interfaces:
- fault
- enable
left_arm_joint_4:
interfaces:
- fault
- enable
left_arm_joint_5:
interfaces:
- fault
- enable
left_arm_joint_6:
interfaces:
- fault
- enable
right_arm_joint_1:
interfaces:
- fault
- enable
right_arm_joint_2:
interfaces:
- fault
- enable
right_arm_joint_3:
interfaces:
- fault
- enable
right_arm_joint_4:
interfaces:
- fault
- enable
right_arm_joint_5:
interfaces:
- fault
- enable
right_arm_joint_6:
interfaces:
- fault
- enable

1
ecrt_dev/description/motor_drive.ros2_control.xacro
View File

@@ -18,6 +18,7 @@
<state_interface name="velocity"/>
<state_interface name="effort"/>
<state_interface name="fault"/>
<state_interface name="enable"/>
<!-- 命令接口（位置控制/故障重置/使能） -->

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

@@ -125,11 +125,11 @@ private:
///bool activated_;
std::atomic<bool> activated_{false};
#define FREQUENCY 250
#define CSP_MAX_VEL_COUNTS_PER_S 25000//65536
#define CSP_MAX_VEL_COUNTS_PER_S 65536
#define CSP_POS_DEADBAND 10
#define CSP_DEADBAND1 100 //CSP允许的误差(计数),原来是10
#define CSP_DEADBAND2 300
#define CSP_SPEED1 30
#define CSP_DEADBAND1 100//100 //CSP允许的误差(计数),原来是10
#define CSP_DEADBAND2 1000 //300
#define CSP_SPEED1 20//30
#define CSP_SPEED2 100
#define CLOCK_TO_USE CLOCK_MONOTONIC
#define NSEC_PER_SEC (1000000000L)
@@ -146,6 +146,8 @@ private:
uint8_t op_states[NUM_SLAVES]={0};
double dst_locs[NUM_SLAVES]={0};
int32_t step_pos[NUM_SLAVES]={0};
int32_t cur_pos[NUM_SLAVES]={0};
double enable_arr[NUM_SLAVES]={0};
int8_t mode_cmd=8;
int inited = 0; //初始化
unsigned int counter = 0;

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

@@ -52,7 +52,8 @@ def generate_launch_description():
parameters=[robot_description, robot_controllers,
{
"lock_memory": True,
"thread_priority": 90
"thread_priority": 99,
"cpu_affinity": 6
}
],
output="both",

210
ecrt_dev/src/ethercat_driver.cpp
View File

@@ -30,7 +30,7 @@ CallbackReturn EthercatDriver::on_init(const hardware_interface::HardwareInfo &
}
struct sched_param param;
param.sched_priority = 99;
param.sched_priority = 90;
if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &param) != 0) {
RCLCPP_ERROR(rclcpp::get_logger("EthercatDriver"),
@@ -166,11 +166,7 @@ RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints.size:%ld,%ld,%ld",info
std::cout << "Failed PDO config " << i << std::endl;
}
}
//ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 5000, 0, 0);
if(i==8)
ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 5000,2500, 0);
else
ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 5000,0, 0);
ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 20000, 0, 0);
//ecrt_slave_config_dc(sc[i], 0x0700, PERIOD_NS, 5000, PERIOD_NS/2, 0);
}
///ecrt_master_set_send_interval(master, PERIOD_NS);
@@ -205,7 +201,7 @@ EthercatDriver::export_state_interfaces()
// export joint state interface
for (uint j = 0; j < info_.joints.size(); j++) {
for (uint i = 0; i < info_.joints[j].state_interfaces.size(); i++) {
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"export:%s,%s",info_.joints[j].name.c_str(),info_.joints[j].state_interfaces[i].name.c_str());
////RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"export:%s,%s",info_.joints[j].name.c_str(),info_.joints[j].state_interfaces[i].name.c_str());
state_interfaces.emplace_back(
hardware_interface::StateInterface(
info_.joints[j].name,
@@ -255,7 +251,7 @@ RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_command_interfa
for (uint j = 0; j < info_.joints.size(); j++) {
////RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"size:%d\n",info_.joints[j].command_interfaces.size());
for (uint i = 0; i < info_.joints[j].command_interfaces.size(); i++) {
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"joints:%s,%s",info_.joints[j].name.c_str(),info_.joints[j].command_interfaces[i].name.c_str());
///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"joints:%s,%s",info_.joints[j].name.c_str(),info_.joints[j].command_interfaces[i].name.c_str());
command_interfaces.emplace_back(
hardware_interface::CommandInterface(
info_.joints[j].name,
@@ -358,13 +354,12 @@ hardware_interface::return_type EthercatDriver::read(
//printf("read data...\n");
//if (lock.owns_lock() && activated_) {
if (activated_.load(std::memory_order_relaxed)){
////master_.readData();
auto start=std::chrono::high_resolution_clock::now();
readData();
auto end=std::chrono::high_resolution_clock::now();
auto diff=end-start;
//if(diff.count()>500000)
// RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"diff:%ld",diff.count());
if(diff.count()>100000)
RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"rdiff:%ld",diff.count());
}
return hardware_interface::return_type::OK;
}
@@ -431,8 +426,6 @@ struct timespec EthercatDriver::timespec_add(struct timespec time1, struct times
return result;
}
#include<sstream>
static std::stringstream logStream;
void EthercatDriver::readData(){
static int print_cnt=0;
@@ -450,29 +443,17 @@ void EthercatDriver::readData(){
#endif
#if 1
wakeupTime = timespec_add(wakeupTime, cycletime);
////clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); //使用绝对时间，精确等待下一个周期，避免循环执行的时间造成周期漂移
// int ret = clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
// if (ret != 0) {
// // 睡眠失败（如wakeupTime落后于当前时间），重置唤醒时间
// clock_gettime(CLOCK_TO_USE, &wakeupTime); // 获取当前时间
// wakeupTime = timespec_add(wakeupTime, cycletime); // 重新计算下一个周期
// RCLCPP_WARN(rclcpp::get_logger("EthercatDriver"),
// "clock_nanosleep failed: %s, reset wakeupTime", strerror(ret));
// }
ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime)); // 更新主站应用时间
////clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime));
#endif
ecrt_master_receive(master); //接收 EtherCAT 帧
ecrt_domain_process(domain1); //处理域数据
check_domain1_state(); //检查域状态
ecrt_master_receive(master);
ecrt_domain_process(domain1);
check_domain1_state();
if (counter) {
counter--;
} else { // do this at 1 Hz
counter = FREQUENCY;
// check for master state (optional)
check_master_state(); //检查主站状态
check_master_state();
if(all_motor_op){
if(!has_clear_all){
has_clear_all=true;
@@ -501,11 +482,7 @@ void EthercatDriver::readData(){
}
inited = 1;
}
// if(print_cnt%1000==1){
// ////for(int i=0;i<NUM_SLAVES;i++)
// ///RCLCPP_INFO(rclcpp::get_logger("hehe"),"new cmd %d:%.1f",i,hw_joint_commands_[i][2]);
// }
logStream.str("");
for (int i = 0; i < NUM_SLAVES; ++i)
{
uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word);
@@ -514,11 +491,14 @@ void EthercatDriver::readData(){
int16_t tv = EC_READ_S16(domain1_pd + zer_offsets[i].torque_actual_value);
int8_t md = EC_READ_S8 (domain1_pd + zer_offsets[i].modes_of_operation_display);
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
double up_pos=pos-pos_offsets[i];
double angle=std::round(up_pos*count_to_rad * 1000.0) / 1000.0;
cur_pos[i]=pos;
double up_pos=cur_pos[i]-pos_offsets[i];
//double angle=std::round(up_pos*count_to_rad * 1000.0) / 1000.0;
if(i==2||i==8)
up_pos=-up_pos;
if(op_states[i]==1){
hw_joint_states_[i][0]=angle;
hw_joint_states_[i][1]=std::round(vel*count_to_speed*1000.0)/1000.0;
hw_joint_states_[i][0]=up_pos*count_to_rad;//angle;
hw_joint_states_[i][1]=vel*count_to_speed;//std::round(vel*count_to_speed*1000.0)/1000.0;
double amp=tv*rated_currents[i];
double effort=0.014*0.6*amp;
hw_joint_states_[i][2]=effort;//std::round(amp*10.0)/10.0;
@@ -529,37 +509,27 @@ void EthercatDriver::readData(){
}
hw_joint_states_[i][3]=err;
hw_joint_states_[i][4]=enable_arr[i];
//hw_joint_states_[i][2]=err;
// if(print_cnt%1000==0){
// logStream<<i<<":"<<pos<<"=>"<<up_pos<<","<<angle<<std::endl;
// // if(i==5){
// // std::cout<<"X "<<logStream.str()<<std::endl;
// // logStream.str("");
// // }else if(i==11){
// // std::cout<<"Z "<<logStream.str()<<std::endl;
// // logStream.str("");
// // }
// if(i==11){
// std::cout<<logStream.str()<<std::endl;
// logStream.str("");
// }
// }
//if(print_cnt%1000==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],p:%d,v:%d,0x%04x,%d",i,pos,vel,err,(sw&0x006f)==0x0008);
status[i] = sw; //侦测子站状态字变化
if (status[i] != last_status[i]) {
last_status[i] = status[i];
}
}
}
void EthercatDriver::writeData(){
static int print_cnt=0;
print_cnt+=1;
for (int i = 0; i < NUM_SLAVES; ++i)
{
#if 1
if ((status[i]&0x006f)==0x0008){
uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
if(err>0){
double fault=hw_joint_commands_[i][0];
if(print_cnt%1000==1){
RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],EE:0x%04x",i,err);
///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],EE:0x%04x",i,err);
}
if(fault==1.0||clear_cmd[i]==1){
if(print_cnt%1000==0)
@@ -594,7 +564,7 @@ void EthercatDriver::readData(){
errStaArr[i]=ERR_FIN_WAIT;
}else if(errStaArr[i]==ERR_FIN_WAIT){
//RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_FIN_WAIT %d",i,err,(sw&0x006f)==0x0008);
if((sw&0x006f)==0x0008){
if((status[i]&0x006f)==0x0008){
errStaArr[i]=ERR_ACK;
}else{
errStaArr[i]=ERR_NONE;
@@ -604,10 +574,10 @@ void EthercatDriver::readData(){
#endif
if(errStaArr[i]!=ERR_NONE||op_states[i]!=1)
continue;
double enable=hw_joint_commands_[i][1];
enable_arr[i]=hw_joint_commands_[i][1];
//if(print_cnt%500==0)
// RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"enable:%d,%.1f,%.2f",i,enable,hw_joint_commands_[i][2]);
if(enable!=1.0){
if(enable_arr[i]!=1.0){
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);
hw_joint_commands_[i][2]=0;
@@ -616,18 +586,17 @@ void EthercatDriver::readData(){
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;
step_pos[i]=pos;
step_pos[i]=cur_pos[i];
} 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);
step_pos[i]=pos;
step_pos[i]=cur_pos[i];
} else if ((status[i] & command[i]) == 0x0023) {
EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f); // Operation enabled
command[i] = 0x006F;
step_pos[i]=pos;
step_pos[i]=cur_pos[i];
} else if ((status[i] & command[i]) == 0x0027) {
if(true)
{
@@ -646,7 +615,7 @@ void EthercatDriver::readData(){
// 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
csp_cmd_pos[i] = pos; // 先对齐
csp_cmd_pos[i] = cur_pos[i]; // 先对齐
vmax_acc[i] = 0;
csp_inited[i] = 1;
}
@@ -654,59 +623,43 @@ void EthercatDriver::readData(){
if(i==2||i==8){
down_pos=-down_pos;
}
double target_pos=down_pos+pos_offsets[i];
if(print_cnt%1000==1){
//if(i==8)
// RCLCPP_INFO(rclcpp::get_logger("hehe"),"hw_joint_commands_[i][2]:%.3f,target_pos:%.3f",hw_joint_commands_[i][2],target_pos);
}
if(target_pos>550000)
continue;
dst_locs[i]=target_pos;
#if 1
#if 0
///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; // 保留余数到下周期
int32_t max_step=CSP_MAX_VEL_COUNTS_PER_S/FREQUENCY;
const int32_t goal=target_pos;
const int32_t err_cmd = goal -csp_cmd_pos[i];
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;
}
#else
const int32_t err_cmd=target_pos-pos;
if (abs(err_cmd)<CSP_DEADBAND1) {
step_pos[i] = target_pos;
reach_target[i]=1;
}else if(abs(err_cmd)<CSP_DEADBAND2){
if(err_cmd>0)
step_pos[i]+=CSP_SPEED1;
else
step_pos[i]-=CSP_SPEED1;
reach_target[i]=0;
} else {
if(reach_target[i]==1){
reach_target[i]=0;
if(err_cmd>0)
step_pos[i]+=CSP_SPEED1;
else
step_pos[i]-=CSP_SPEED1;
}else{
if(err_cmd>0)
step_pos[i]+=CSP_SPEED2;
else
step_pos[i]-=CSP_SPEED2;
}
}
#endif
#else
csp_cmd_pos[i]=target_pos;
#endif
#if 0
int32_t max_step=CSP_MAX_VEL_COUNTS_PER_S/FREQUENCY;
const int32_t goal=target_pos;
const int32_t err_cmd = goal -csp_cmd_pos[i];
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;
}
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]);
#else
const int32_t err_cmd=target_pos-cur_pos[i];
if (abs(err_cmd)<CSP_DEADBAND1) {
step_pos[i] = target_pos;
reach_target[i]=1;
}else if(abs(err_cmd)<CSP_DEADBAND2){
if(err_cmd>0) step_pos[i]+=(err_cmd>CSP_SPEED1) ? CSP_SPEED1:err_cmd;
else step_pos[i]+=(err_cmd<-CSP_SPEED1) ? -CSP_SPEED1:err_cmd;
// reach_target[i]=0;
//if(err_cmd>0) step_pos[i]+=CSP_SPEED1;
//else step_pos[i]-=CSP_SPEED1;
} else {
//if(err_cmd>0) step_pos[i]+=(err_cmd>CSP_SPEED2) ? CSP_SPEED2:err_cmd;
//else step_pos[i]+=(err_cmd<-CSP_SPEED2) ? -CSP_SPEED2:err_cmd;
if(err_cmd>0) step_pos[i]+=CSP_SPEED2;
else step_pos[i]-=CSP_SPEED2;
}
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, step_pos[i]);
#endif
// 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
// const int32_t follow_err = csp_cmd_pos[i] - pv;
// const int32_t FE_LIMIT = (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
@@ -714,11 +667,6 @@ void EthercatDriver::readData(){
// // 例如：冻结或减半 max_step，给驱动时间追上
// }
#if 1
// 下发“绝对目标”（不是增量）
//EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]);
EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, step_pos[i]);
#endif
break;
}
case CYCLIC_TORQUE: // 10
@@ -750,20 +698,11 @@ void EthercatDriver::readData(){
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);
}
ecrt_master_sync_slave_clocks(master);
#if 0
auto diff=std::chrono::high_resolution_clock::now()-rec_time;
rec_time=std::chrono::high_resolution_clock::now();
long ttv=diff.count()-4000000;
if(abs(ttv)>100000)
RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"exe :%ld,%ld",diff.count(),ttv);
#endif
// send process data
ecrt_domain_queue(domain1); //排队域数据
ecrt_master_send(master); //发送ETHERCAT帧
}
@@ -773,10 +712,13 @@ hardware_interface::return_type EthercatDriver::write(
{
// try to lock so we can avoid blocking the read/write loop on the lock.
/// const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
//printf("write data...\n");
///if (lock.owns_lock() && activated_)
{
//writeData();
if (activated_.load(std::memory_order_relaxed)){
auto start=std::chrono::high_resolution_clock::now();
writeData();
auto end=std::chrono::high_resolution_clock::now();
auto diff=end-start;
if(diff.count()>100000)
RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"wdiff:%ld",diff.count());
}
return hardware_interface::return_type::OK;
}

28
ecrt_dev/src/test_motor.cpp
View File

@@ -160,7 +160,7 @@ void TestMotor::pubTraj(const std::vector<double> angles1,const std::vector<doub
// skip_pub=false;
// }
point.positions.push_back(rad);
printf("D %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],point.positions[i]);
printf("D %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],rad);
}
for(int i=0;i<6;i++){
std::string jname = "right_arm_joint_" + std::to_string(i+1);
@@ -168,7 +168,7 @@ void TestMotor::pubTraj(const std::vector<double> angles1,const std::vector<doub
double rad=curMap_[jname];
rad=dstMap_[jname];
point.positions.push_back(rad);
printf("D %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],point.positions[i]);
printf("D %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],rad);
}
traj_msg.points.push_back(point);
//if(!skip_pub)
@@ -200,12 +200,12 @@ void TestMotor::pubTraj(const double angle){
// skip_pub=false;
// }
point.positions.push_back(angle);
printf("S %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],point.positions[i]);
printf("S %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],angle);
}
for(int i=0;i<6;i++){
std::string jname = "right_arm_joint_" + std::to_string(i+1);
point.positions.push_back(angle);
printf("S %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],point.positions[i]);
printf("S %s: %.2f/%.2f\n",jname.c_str(),curMap_[jname],angle);
}
traj_msg.points.push_back(point);
//if(!skip_pub)
@@ -498,12 +498,20 @@ int main(int argc,char* argv[]){
std::vector<double> angles1={0, 0, 0, 0, 0, 0};
std::vector<double> angles2={90,0, 40, 0, 0, 0};
node->is_reach=true;
static int loop_cnt=0;
while(rclcpp::ok()){
std::cout<<"开始执行角度1.05...\n";
//std::vector<double> angles={-40, -40, -40, 0, -40, 0,40, 40, 40, 0, 40, 0};
//angles={0, 0, 0, 0, 0, 0,40,0, 0, 0, 0, 00};
angles1={0, 0, 0, 0, 0, 0};
angles2={0, 0, 40,40, 0, 0};
loop_cnt+=1;
if(loop_cnt%2==0){
angles1={-40, -40, -40,-40,-40,-40};
angles2={-40,-40,-40, -40,-40,-40};
}else{
angles1={40, 40, 40,40,40,40};
angles2={40,40,40, 40,40,40};
}
//angles2={0,0,0, 40,0,0};
//angles1={-40,-40,-40, 0,-40, 0};
//angles2={40,40,40, 0,40,0};
// for(int i=0;i<12;i++){
// std::string jname = "joint_" + std::to_string(i+1);
// node->dstMap_[jname]=angles.at(i)*M_PI/180.0;
@@ -511,7 +519,7 @@ int main(int argc,char* argv[]){
// }
///node->dstMap_[jname]={-0.678,-0.678,-0.678,0,-0.678,0,0.678,0.678,0.678,0,0.678,0};
node->pubTraj(angles1,angles2);
sleep(15);
sleep(10);
std::cout<<"开始执行复位...\n";
// for(int i=0;i<12;i++){
// std::string jname = "joint_" + std::to_string(i+1);
@@ -521,7 +529,7 @@ int main(int argc,char* argv[]){
angles1={0, 0, 0, 0, 0, 0};
angles2={0, 0, 0,0, 0, 0};
node->pubTraj(angles1,angles2);
sleep(15);
sleep(10);
}
// std::cout<<"复位完毕!\n禁能机械臂电机...\n";