零差云控关节模组

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-2_blogsobaiduweb~default-2-108712655.nonecase&spm=1018.2226.3001.4450

一、机械臂组装

图1 电源电压和电流额定值

图2 电源接线方式

图3 ethercat接线方式
建议：通信线使用双绞线，自己做JST接头，一根到底，防止接线不牢固出现通信问题。模组间连线时，in入out出，依次连接，最终端模组不做冗余时只需连接in即可。
组装前先将编码器外接电池安装上进行编码器复位，并将电机转动至编码器0位处，好像目前还不支持置零操作。

二、对两个关节模组控制

IGH-DC模式对两个关节模组进行控制，使用CSP模式。通过每个周期改变目标位置来实现运动。
注：下面的代码只是简单实现了两个电机的转动

#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h>
#include <sys/mman.h>
#include <malloc.h>
#include <sched.h> /* sched_setscheduler() *//****************************************************************************/
#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   /*Operation mode for 0x6060:0*/       /*csp模式*/      //位置模式/****************************************************************************/#define NSEC_PER_SEC (1000000000L)
#define PERIOD_NS (NSEC_PER_SEC/ FREQUENCY)    /*本次设置周期PERIOD_NS为1ms*/#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 = {};/****************111111111111***********************/
static ec_slave_config_t *sc[2] ;    //根据从站的个数定
static ec_slave_config_state_t sc_state[2] ;/****************************************************************************/
// process data
static uint8_t *domain1_pd = NULL;
/****************22222222222222******************/
#define ZeroErr        0,0                        /*EtherCAT address on the bus*/
#define ZeroErr1       0,1//上面是从站的地址，每个从站只有地址不一样#define VID_PID          0x5a65726f,0x00029252   //需要通过sudo ethercat cstruct -a 0命令查看电机的ID//上面是电机的VID_PID,相同品牌的电机上面两个相同/**********************以下五条对从站的PDO进行配置***Offsets for PDO entries *******************************/
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;
/******************333333333333333*******************/
//通过定义新的相同的结构体来进行不同从站各状态的定义,来对多个从站进行配置
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},/***********************4444444444444444*******************/{}
};
/*Config PDOs*****只需要在需要读取电机更多的状态的时候进行改写，所有从站共用一个*/
static ec_pdo_entry_info_t slave_0_pdo_entries[] = {/*RxPdo 0x1600*/{0x607a, 0x00, 32}, /* Target Position */{0x60fe, 0x00, 32}, /* Digital outputs */{0x6040, 0x00, 16}, /* Control Word *//*TxPdo 0x1A00*/{0x6064, 0x00, 32}, /* Position Actual Value */{0x60fd, 0x00, 32}, /* Digital inputs */{0x6041, 0x00, 16}, /* Status Word */{0x6078,0x00,16},  /*actual corrent*/{0x606c,0x00,32},  /*actual speed*/{0x6077,0x00,16},  /*actual torque*/
};
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}
};  //不需要修改
/*以上五条根据sudo ethercat cstruct -a 0得到的电机的信息进行配置。*/
/**********************************************************************************************************/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;
}/******************************5555555555555555*******************************/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_TIMINGstruct 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 timeclock_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_TIMINGclock_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 dataecrt_master_receive(master);  //主站从设备获取数据帧并处理报文ecrt_domain_process(domain1);  //判断数据域报文的状态// check process data state (optional)//判断数据域、主站、从站状态是否发生变化，有变化就提示信息check_domain1_state();  //数据域if (counter){counter--;} else{ // do this at 1 Hzcounter = FREQUENCY;// check for master state (optional)check_master_state();     //主站// check for slave configuration state(s)check_slave_config_states();   //从站#ifdef MEASURE_TIMING// output timing statsprintf("period     %10u ... %10u\n",period_min_ns, period_max_ns);printf("exec       %10u ... %10u\n",exec_min_ns, exec_max_ns);printf("latency    %10u ... %10u\n",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 datablink = !blink;}
/*//从站进OP,通过写控制字实现*///*******************从站00******************************///*Read state*/status = EC_READ_U16(domain1_pd + offset.status_word);//读取从站0状态字printf("status_word    %10u\n",status);   // write process data//DS402 CANOpen over EtherCAT status machineif( (status & command) == 0x0040 )  //开始写控制字，让电机使能{EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x0006 );EC_WRITE_S8(domain1_pd + offset.operation_mode, CYCLIC_POSITION);    //写入状态字  //set control modecommand = 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 enabledelse if( (status & command) == 0x0027){EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x001f );}//*******************从站01******************************//status1 = EC_READ_U16(domain1_pd + offset1.status_word);//读取从站1状态字printf("status_word1 %10u\n",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 modecommand1 = 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 enabledelse 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状态，电机使能{/*****从站00*****///读当前位置并写入目标位置actual_position = EC_READ_S32(domain1_pd +offset.position_actual_value);//读取当前位置 printf("position_actual_value %10u\n",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 %dmA\n",actual_current);//实际速度actual_speed = EC_READ_S32(domain1_pd+offset.actual_speed);printf("actual_speed %dcnt/sec\n",actual_speed);//实际力矩actual_torque = EC_READ_S32(domain1_pd+offset.actual_torque);actual_torque=actual_torque*10;printf("actual_torque %dmNm\n",actual_torque);/*****从站01******/actual_position1 = EC_READ_S32(domain1_pd +offset1.position_actual_value);//读取当前位置 printf("position1_actual_value %10u\n",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_TIMINGclock_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 couplersc[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;/****************************************************************///下面是确认两个从站是否配置好PDOprintf("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 slavefor (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;}/* Set priority */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, &param) == -1) {perror("sched_setscheduler failed");}printf("Starting cyclic function.\n");cyclic_task();return 0;
}

三、测试结果

在组装了六轴机械臂后，对整个机械臂在CSP模式下进行了匀速运动实验。下面是实验中记录的第二个关节的位置、速度、力矩的实验图，仅供大家参考。

图一关节二位置数据图

图二关节二速度数据图

图三关节二力矩数据图
注：图三中在20000数据后力矩的波动是因为其他关节对其影响，力矩是根据电流IQ乘以力矩系数得到的。模组中电流采集可设置滤波深度

第五章机械臂的组装及控制相关推荐

ROS笔记（27）机械臂的组装
ROS笔记(27) 机械臂的组装 1. 机械臂模型 2. 声明模型的宏 3. 创建机械臂模型 4. 加入Gazebo属性 5. 显示机械臂模型 1. 机械臂模型这里就是用模拟的一个六轴机械臂--HH ...
Matlab机器人工具箱（3-3）：五自由度机械臂（动力学）
动力学主要分为牛顿-欧拉法和拉格朗日法牛顿-欧拉法: 向外递推速度与角速度,向内迭代计算力与力矩拉格朗日方程: 根据能量思想,从标量(拉格朗日方程)得到动力学方程先计算动能与势能,再构造拉格朗日 ...
五轴机械臂实现视觉抓取--realsense深度相机和五自由度机械臂
前言:要实现视觉抓取,首先需要实现机械臂的驱动,深度相机的目标识别,能够反馈位置. 1.实现机械臂在ROS层的控制 2.基于深度相机目标物体的空间坐标反馈,需要知道摄像头中物体的像素坐标系到大地坐标系 ...
五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）
五自由度机械臂建模学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 机械臂还是原来的机械臂,之前用ROS做底层驱动,不需要写正逆运动学和相关算法就能得到一些简单的 ...
使用ROS MoveIt!控制真实五自由度机械臂
文章目录使用ROS MoveIt!控制diy五自由度机械臂写在前面修改功能包中相关文件编写myrobot_controllers.yaml文件修改launch文件 myrobot_drive ...
Matlab机器人工具箱（3-1）：五自由度机械臂（正逆运动学）
01 正运动学:DH表示法 1955年, Denavit和Hartenberg在"ASME Journal of Applied Mechanic"发表了一篇论文,这篇论文介绍了一 ...
柔性机械臂_CSR论文精选 | 基于视觉的双连杆柔性机械臂末端位置跟踪控制
05基于视觉的双连杆柔性机械臂末端位置跟踪控制 Umesh Kumar Sahu; Dipti Patra; Bidyadhar Subudhi 文章精读英文标题: Vision-based tip ...
Matlab机器人工具箱（3-4）：五自由度机械臂（计算力矩控制方法与roblocks）
01 roblocks使用方法在命令行输入roblocks 打开机器人工具箱的模块库使用'roblocks'命令打开simulink 机器人模块时提示版本过低的解决办法: ① 打开帮助–选择小齿轮 ...
Matlab机器人工具箱（3-2）：五自由度机械臂（轨迹规划）
轨迹规划可以分为关节空间的轨迹规划和笛卡尔空间的轨迹规划关节空间规划:用时小,计算量少 [q,qt,qtt]=jtraj(q1,q2,t); 返回关节的位置.速度.加速度使用五次多项式插值笛卡尔 ...
【机器人工具箱学习笔记】第七章机械臂运动学
函数库 import %装载 ETS2.* %二维空间 ETS3.* %三维空间 Rz() %绕z轴旋转 Tx() %沿着x轴平移 .fkine() %正运动学 .teach %示教 .structu ...

第五章机械臂的组装及控制

零差云控关节模组

文章目录

前言

一、机械臂组装

二、对两个关节模组控制

三、测试结果

第五章机械臂的组装及控制相关推荐

最新文章

热门文章

第五章 机械臂的组装及控制

零差云控关节模组

文章目录

前言

一、机械臂组装

二、对两个关节模组控制

三、测试结果

第五章 机械臂的组装及控制相关推荐

最新文章

热门文章

第五章机械臂的组装及控制

第五章机械臂的组装及控制相关推荐