https://www.zeroerr.cn/download/eRob.html 获取零差云控最新资料
第一章 单模组的使用
第二章 零差云控关节模组内部构造
第三章 IGH及preempt-rt的安装
第四章 命令行读取电机的信息
第五章 机械臂的组装及控制
文章目录
- 零差云控关节模组
- @[TOC](文章目录)
- 前言
- 一、机械臂组装
- 二、对两个关节模组控制
- 三、测试结果
在linux下使用vs code编写c程序，使用GCC进行编译。
没有写关于CIA402的状态机转换程序，只简单写了进OP的程序，这种方式进OP特别慢，并且没写退出OP的程序。
一、机械臂组装参考自：https://blog.csdn.net/weixin_40293570/article/details/108712655?ops_request_misc=&request_id=&biz_id=102&utm_term=igh+preempt_rt&utm_medium=distribute.pc_search_result.none-task-blog-2blogsobaiduweb~default-2-108712655.nonecase&spm=1018.2226.3001.4450
图1 电源电压和电流额定值
图2 电源接线方式
图3 ethercat接线方式
建议：通信线使用双绞线，自己做JST接头，一根到底，防止接线不牢固出现通信问题。模组间连线时，in入out出，依次连接，最终端模组不做冗余时只需连接in即可。
组装前先将编码器外接电池安装上进行编码器复位，并将电机转动至编码器0位处，好像目前还不支持置零操作。
IGH-DC模式对两个关节模组进行控制，使用CSP模式。通过每个周期改变目标位置来实现运动。
注：下面的代码只是简单实现了两个电机的转动
#include三、测试结果#include #include #include #include #include #include #include #include #include #include #include #include "../include/ecrt.h" #include "../include/ectty.h" // ../表示上一级文件夹 // Application parameters #define FREQUENCY 1000 #define CLOCK_TO_USE CLOCK_MONOTONIC #define MEASURE_TIMING #define CYCLIC_POSITION 8 //位置模式 #define NSEC_PER_SEC (1000000000L) #define PERIOD_NS (NSEC_PER_SEC/ FREQUENCY) #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) // 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[2] ; //根据从站的个数定 static ec_slave_config_state_t sc_state[2] ; // process data static uint8_t *domain1_pd = NULL; #define ZeroErr 0,0 #define ZeroErr1 0,1 //上面是从站的地址，每个从站只有地址不一样 #define VID_PID 0x5a65726f,0x00029252 //需要通过sudo ethercat cstruct -a 0命令查看电机的ID //上面是电机的VID_PID,相同品牌的电机上面两个相同 static struct{ unsigned int operation_mode; unsigned int target_position; unsigned int digital_outputs; unsigned int ctrl_word; unsigned int position_actual_value; unsigned int digital_inputs; unsigned int status_word; int actual_current; unsigned int actual_speed; int actual_torque; }offset,offset1; //通过定义新的相同的结构体来进行不同从站各状态的定义,来对多个从站进行配置 const static ec_pdo_entry_reg_t domain1_regs[] = { //电机0 {ZeroErr, VID_PID, 0x607A, 0, &offset.target_position}, {ZeroErr, VID_PID, 0x60FE, 0, &offset.digital_outputs}, {ZeroErr, VID_PID, 0x6040, 0, &offset.ctrl_word}, {ZeroErr, VID_PID, 0x6064, 0, &offset.position_actual_value}, {ZeroErr, VID_PID, 0x60FD, 0, &offset.digital_inputs}, {ZeroErr, VID_PID, 0x6041, 0, &offset.status_word}, {ZeroErr, VID_PID, 0x6078, 0, &offset.actual_current}, {ZeroErr, VID_PID, 0x606c, 0, &offset.actual_speed}, {ZeroErr, VID_PID, 0x6077, 0, &offset.actual_torque}, //电机1 {ZeroErr1, VID_PID, 0x607A, 0, &offset1.target_position}, {ZeroErr1, VID_PID, 0x60FE, 0, &offset1.digital_outputs}, {ZeroErr1, VID_PID, 0x6040, 0, &offset1.ctrl_word}, {ZeroErr1, VID_PID, 0x6064, 0, &offset1.position_actual_value}, {ZeroErr1, VID_PID, 0x60FD, 0, &offset1.digital_inputs}, {ZeroErr1, VID_PID, 0x6041, 0, &offset1.status_word}, {ZeroErr1, VID_PID, 0x6078, 0, &offset1.actual_current}, {ZeroErr1, VID_PID, 0x606c, 0, &offset1.actual_speed}, {ZeroErr1, VID_PID, 0x6077, 0, &offset1.actual_torque}, {} }; static ec_pdo_entry_info_t slave_0_pdo_entries[] = { {0x607a, 0x00, 32}, {0x60fe, 0x00, 32}, {0x6040, 0x00, 16}, {0x6064, 0x00, 32}, {0x60fd, 0x00, 32}, {0x6041, 0x00, 16}, {0x6078,0x00,16}, {0x606c,0x00,32}, {0x6077,0x00,16}, }; static ec_pdo_info_t slave_0_pdos[] = { {0x1600, 3, slave_0_pdo_entries + 0}, {0x1a00, 6, slave_0_pdo_entries + 3}, }; //其中第二行的参数需要根据上面的txpdo与rxpdo的个数进行修改 static ec_sync_info_t slave_0_syncs[] = { {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}, {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}, {2, EC_DIR_OUTPUT, 1, slave_0_pdos + 0, EC_WD_ENABLE}, {3, EC_DIR_INPUT, 1, slave_0_pdos + 1, EC_WD_DISABLE}, {0xff} }; //不需要修改 static unsigned int counter = 0; static unsigned int blink = 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; } 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; } 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; } void check_slave_config_states(void) { int i; ec_slave_config_state_t s[2]={}; //根据从站的个数进行修改************************ for(i=0;i<2;i++) { ecrt_slave_config_state(sc[i], &s[i]); if (s[i].al_state != sc_state[i].al_state) { printf("slave: State 0x%02X.n", s[i].al_state); } if (s[i].online != sc_state[i].online) { printf("slave: %s.n", s[i].online ? "online" : "offline"); } if (s[i].operational != sc_state[i].operational) { printf("slave: %soperational.n", s[i].operational ? "" : "Not "); } sc_state[i] = s[i]; } } void cyclic_task() //循环任务 { static uint16_t command=0x004F;//用来帮助判断状态字的值 static uint16_t command1=0x004F; uint16_t status; //读取伺服状态 uint32_t status1; uint16_t i=1; int32_t actual_position =0; int32_t actual_position1=0; int16_t actual_current; int16_t actual_torque; int32_t target_position; int32_t target_position1; int32_t actual_speed; 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 // get current time clock_gettime(CLOCK_TO_USE, &wakeupTime); while(1) { 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); //主站从设备获取数据帧并处理报文 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 ... %10un", period_min_ns, period_max_ns); printf("exec %10u ... %10un", exec_min_ns, exec_max_ns); printf("latency %10u ... %10un", latency_min_ns, latency_max_ns); 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 // calculate new process data blink = !blink; } // status = EC_READ_U16(domain1_pd + offset.status_word);//读取从站0状态字 printf("status_word %10un",status); // write process data //DS402 CANOpen over EtherCAT status machine if( (status & command) == 0x0040 ) //开始写控制字，让电机使能 { EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x0006 ); EC_WRITE_S8(domain1_pd + offset.operation_mode, CYCLIC_POSITION); //写入状态字 //set control mode command = 0x006F; } else if( (status & command) == 0x0021) { EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x0007 ); command = 0x006F; } else if( (status & command) == 0x0023) { EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x000f ); command = 0x006F; actual_position = EC_READ_S32(domain1_pd + offset.position_actual_value);//读取当前位置 target_position = actual_position; //把当前位置赋值为目标位置 EC_WRITE_S32(domain1_pd + offset.target_position, target_position ); //把目标位置写入对象字典 } //operation enabled else if( (status & command) == 0x0027) { EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x001f ); } // status1 = EC_READ_U16(domain1_pd + offset1.status_word);//读取从站1状态字 printf("status_word1 %10un",status1); if( (status1 & command1) == 0x0040 ) //开始写控制字，让电机使能 { EC_WRITE_U16(domain1_pd + offset1.ctrl_word, 0x0006 ); EC_WRITE_S8(domain1_pd + offset1.operation_mode, CYCLIC_POSITION); //set control mode command1 = 0x006F; } else if( (status1 & command1) == 0x0021) { EC_WRITE_U16(domain1_pd + offset1.ctrl_word, 0x0007 ); command1 = 0x006F; } else if( (status1 & command1) == 0x0023) { EC_WRITE_U16(domain1_pd + offset1.ctrl_word, 0x000f ); command1 = 0x006F; actual_position1 = EC_READ_S32(domain1_pd + offset1.position_actual_value);//读取当前位置 target_position1 = actual_position1; //把当前位置赋值为目标位置 EC_WRITE_S32(domain1_pd + offset1.target_position, target_position1 ); //把目标位置写入对象字典 } //operation enabled else if( (status1& command1) == 0x0027) { EC_WRITE_U16(domain1_pd + offset1.ctrl_word, 0x001f ); } if (sync_ref_counter) { sync_ref_counter--; } else { sync_ref_counter = 1; // sync every cycle 每个循环周期用来同步 clock_gettime(CLOCK_TO_USE, &time); ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); if(( (status & command) == 0x0027)&((status1 & command1) == 0x0027) ) //确认两从站为OP状态，电机使能 { //读当前位置并写入目标位置 actual_position = EC_READ_S32(domain1_pd +offset.position_actual_value);//读取当前位置 printf("position_actual_value %10un",actual_position); target_position = actual_position + 10; //把当前位置+10 赋值为下一目标位置 EC_WRITE_S32(domain1_pd + offset.target_position, target_position ); //把目标位置写入对象字典 //实际电流 actual_current = EC_READ_S16(domain1_pd+offset.actual_current); actual_current=actual_current*5; printf("actual_current %dmAn",actual_current); //实际速度 actual_speed = EC_READ_S32(domain1_pd+offset.actual_speed); printf("actual_speed %dcnt/secn",actual_speed); //实际力矩 actual_torque = EC_READ_S32(domain1_pd+offset.actual_torque); actual_torque=actual_torque*10; printf("actual_torque %dmNmn",actual_torque); actual_position1 = EC_READ_S32(domain1_pd +offset1.position_actual_value);//读取当前位置 printf("position1_actual_value %10un",actual_position1); target_position1 = actual_position1 -10; //把当前位置+10 赋值为下一目标位置 EC_WRITE_S32(domain1_pd + offset1.target_position, target_position1 ); //把目标位置写入对象字典 } } ecrt_master_sync_slave_clocks(master); ecrt_domain_queue(domain1);//将数据域的所有报文插入到主站的报文序列 ecrt_master_send(master);//将主站所有报文发送到传输序列 #ifdef MEASURE_TIMING clock_gettime(CLOCK_TO_USE, &endTime); #endif } } int main(int argc, char **argv) { if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) { perror("mlockall failed"); return -1; } master = ecrt_request_master(0); //请求EtherCAT主机进行实时操作。 if (!master) return -1; domain1 = ecrt_master_create_domain(master); // 创建新的进程数据域 if (!domain1) return -1; // Create configuration for bus coupler sc[0] = ecrt_master_slave_config(master, ZeroErr, VID_PID);//获取从站配置 if (!sc[0]) //第一个参数是所请求的主站实例，第二个包括主站的别名和位置，第三个包括供应商码和产品码 return -1; //加从站则需要添加 sc[1] = ecrt_master_slave_config(master, ZeroErr1, VID_PID);//获取从站配置 if (!sc[1]) //第一个参数是所请求的主站实例，第二个包括主站的别名和位置，第三个包括供应商码和产品码 return -1; //下面是确认两个从站是否配置好PDO printf("Configuring PDOs...n"); for (int i = 0; i < 2; i++) { if (ecrt_slave_config_pdos(sc[i], EC_END, slave_0_syncs)) //指定完整的PDO配置。第一个参数是获取的从站配置，第二个参数表示同步管理器配置数，EC_END=~u0,第三个参数表示同步管理器配置数组 { fprintf(stderr, "Failed to configure slave PDOs!n"); exit(EXIT_FAILURE); } else { printf("*Success to configuring slave PDOs*n"); } } if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) //为进程数据域注册一组PDO项。参数一：创建的进程数据域，参数二：pdo注册数组 { fprintf(stderr, "PDO entry registration failed!n"); exit(EXIT_FAILURE); } // configure SYNC signals for this slave for (int i = 0; i < 2; i++) { ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 4400000, 0, 0); } printf("Activating master...n"); if (ecrt_master_activate(master)) // 以上激活主站 return -1; if (!(domain1_pd = ecrt_domain_data(domain1))) {//返回域的进程数据 return -1; } struct sched_param param = {}; param.sched_priority = sched_get_priority_max(SCHED_FIFO); printf("Using priority %i.", param.sched_priority); if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { perror("sched_setscheduler failed"); } printf("Starting cyclic function.n"); cyclic_task(); return 0; }
在组装了六轴机械臂后，对整个机械臂在CSP模式下进行了匀速运动实验。下面是实验中记录的第二个关节的位置、速度、力矩的实验图，仅供大家参考。
图一 关节二位置数据图
图二 关节二速度数据图
图三 关节二力矩数据图
注：图三中在20000数据后力矩的波动是因为其他关节对其影响，力矩是根据电流IQ乘以力矩系数 得到的。模组中电流采集可设置滤波深度
