EtherCAT 伺服控制功能块实现

        EtherCAT 是运动控制领域主要的通信协议，开源EtherCAT 主站协议栈 IgH 和SOEM 两个项目，IgH 相对更普及一些，但是它是基于Linux 内核的方式，比SOEM更复杂一些。使用IgH 协议栈编写一个应用程序，控制EtherCAT 伺服电机驱动器是比较简单的。但是要实现一个通用的EtherCAT 组件库（例如IEC61131-3 ,或者IEC61499功能块）就复杂一些了，例如动态地加入一个从站驱动器，通过组件控制某一个从站。

本博文研究基于组件的EtherCAT 程序架构及其实现方法。

背景技术

CiA402 运动控制的CANopen 驱动器规范

        EtherCAT 的运动控制器是基于CANopen 的CiA402规范。这套配置文件规范标准化了伺服驱动器、变频器和步进电机控制器的功能行为。它定义了状态机，控制字，状态字，参数值，它们映射到过程数据对象（PDO）配置文件已在 IEC 61800-7 系列中部分实现国际标准化。

COE 协议

CANopen Over EtherCAT 协议被称为COE，它的架构为：

正是由于如此，基于EtherCAT 的运动控制器的控制方式，PDO 定义，控制方式都是类似的。

主要的一些数据对象

 

PLCopen 运动控制库

 最著名的运动控制的标准应当数PLCopen 运动控制库，它是PLC 上的一个功能块集。PLC 的应用程序通过这些功能块能够方便地实现运动控制。但是这些功能块如何实现，如何与硬件驱动结合。内部实现应该是比较复杂的。笔者看来，应该有两种方式：

•    PLC 内嵌运动控制模型
•    通过Ethercat 总线外接运动控制模块

两种结构的实现方法应该各不相同。是否有支持etherCAT 的PLCopen 功能块库？

EtherCAT 主站程序

        EtherCAT 协议是倍福公司提出的，从站通常使用专用ASIC 芯片，FPGA 实现，而主站使用通用Ethernet接口和软件实现。EtherCAT 主站协议有专业公司开发的商业化产品，也有开源代码，下面是两个比较流行的EtherCAT Master

• IgH
• SOEM

感觉IgH  更普及一点，于是我们选择IgH 协议栈。

EtherCAT 组件设计

IgH 主要实现Ethercat 协议数据帧的映射，以及通过Ethernet 发送和接收。如果设计成为组件库，许多参数需要可编程，比如：

•     多少从站
•    每个从站的位置
•    每个从站的操作模型，操作算法
•    每个从机的状态

        本项目的基本思路是构建一个从站类，每个物理从站对应一个虚拟从站，应用程序通过虚拟从站控制从站，将虚拟从站的参数映射到物理从站参数，通过Ethercat 网络发送和接收。

从站类（SevoController Class）与主站类（Master Class）

        为了实现动态的建立和控制从站，采用虚拟从站类。为每个物理的从站创建一个从站类（SevoController). 该类型中包含了物理伺服驱动控制器的参数和状态。应用程序可以通过修改SevoController 的参数，实现对物理伺服的驱动。

        为了相对于，我们同时设立一个Master 类（Master Class)。存放主站的参数。

系统架构

        从上图可见，使用Slaver 类作为应用程序和EtherCAT 底层的接口。EtherCAT 底层程序读取Slave 的参数，对EtherCAT 初始化，并且建立一个EtherCAT 线程，周期扫描各个从站。

从站类（slave class）

#ifndef _SEVOCONTROLLER_H
#define _SEVOCONTROLLER_H#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <sys/mman.h>
#include <stdint.h>
#include "ecrt.h"
#define PROFILE_POSITION 1
#define VEOLOCITY 2
#define PROFILE_VELOCITY 3
#define PROFILE_TORQUE 4
#define HOMING 6
#define CYCLICE_SYNC_POSITION 8
using namespace std;
struct pdo_offset
{unsigned int ctrl_word;unsigned int operation_mode;unsigned int target_velocity;unsigned int target_position;unsigned int profile_velocity;unsigned int status_word;unsigned int mode_display;unsigned int current_velocity;
};
class SevoController
{
public:pdo_offset offset;uint16_t position;uint32_t vendor_id;uint32_t product_code;uint32_t position_actual;uint32_t velocity_actual;uint32_t operation_modes;uint32_t target_velocity;uint32_t target_position;uint32_t profile_velocity;ec_slave_config_t *slave_config;void eventAction(string EventName);SevoController(uint32_t Position, uint32_t Vendor_id, uint32_t Product_cdode, uint32_t Modes_operation);
};
#endif

控制代码

#include "ecrt.h"
#include "stdio.h"
#include <errno.h>
#include <sys/resource.h>
#include <list>
#include "SevoController.hpp"
#include <pthread.h>
void check_domain_state(void);
void check_slave_config_states(void);
pthread_t cycle_thread;
int cycles;
int Run = 1;
ec_master_t *master = NULL;
static ec_master_state_t master_state = {};static ec_domain_t *domainServo = NULL;
static ec_domain_state_t domainServo_state = {};
static uint8_t *domain_pd = NULL;
std::list<SevoController *> SevoList;
ec_pdo_entry_reg_t *domainServo_regs;
static ec_pdo_entry_info_t pdo_entries[] = {/*RxPdo 0x1600*/{0x6040, 0x00, 16},{0x6060, 0x00, 8 }, {0x60FF, 0x00, 32},{0x607A, 0x00, 32},{0x6081, 0x00, 32},/*TxPdo 0x1A00*/{0x6041, 0x00, 16},{0x6061, 0x00, 8},{0x606C, 0x00, 32}
};static ec_pdo_info_t Slave_pdos[] = {// RxPdo{0x1600, 5, pdo_entries + 0},// TxPdo{0x1A00, 3, pdo_entries + 5}};static ec_sync_info_t Slave_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_pdos + 0, EC_WD_DISABLE},{3, EC_DIR_INPUT, 1, Slave_pdos + 1, EC_WD_DISABLE},{0xFF}};
int ConfigPDO()
{domainServo = ecrt_master_create_domain(master);if (!domainServo){return -1;}//domainServo_regs = new ec_pdo_entry_reg_t[9];std::list<SevoController *>::iterator it;int index = 0;for (it = SevoList.begin(); it != SevoList.end(); it++){domainServo_regs[index++] = {0, (**it).position, (**it).vendor_id, (**it).product_code, 0x6040, 0x00, &((**it).offset.ctrl_word)};domainServo_regs[index++] = {0, (**it).position, (**it).vendor_id, (**it).product_code, 0x6060, 0x00, &((**it).offset.operation_mode)};domainServo_regs[index++] = {0, (**it).position, (**it).vendor_id, (**it).product_code, 0x60FF, 0x00, &((**it).offset.target_velocity)};domainServo_regs[index++] = {0, (**it).position, (**it).vendor_id, (**it).product_code, 0x607A, 0x00, &((**it).offset.target_position)};domainServo_regs[index++] = {0, (**it).position, (**it).vendor_id, (**it).product_code, 0x6081, 0x00, &((**it).offset.profile_velocity)};domainServo_regs[index++] = {0, (**it).position, (**it).vendor_id, (**it).product_code, 0x6041, 0x00, &((**it).offset.status_word)};domainServo_regs[index++] = {0, (**it).position, (**it).vendor_id, (**it).product_code, 0x6061, 0x00, &((**it).offset.mode_display)};domainServo_regs[index++] = {0, (**it).position, (**it).vendor_id, (**it).product_code, 0x606C, 0x00, &((**it).offset.current_velocity)};printf("product_code:%x\n", (**it).product_code);}domainServo_regs[index++] = {}; ////for (it = SevoList.begin(); it != SevoList.end(); it++){(**it).slave_config = ecrt_master_slave_config(master, 0, (**it).position, (**it).vendor_id, (**it).product_code);ecrt_slave_config_pdos((**it).slave_config, EC_END, Slave_syncs);}//if (ecrt_domain_reg_pdo_entry_list(domainServo, domainServo_regs)){printf("PDO entry registration failed!\n");return -1;}return 0;
}
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 *cyclic_task(void *arg)
{uint16_t status;//  int8_t      opmode;static uint16_t command = 0x004F;printf("Cycles Task Start\n");while (Run){ecrt_master_receive(master);ecrt_domain_process(domainServo);check_domain_state();check_master_state();check_slave_config_states();std::list<SevoController *>::iterator it;for (it = SevoList.begin(); it != SevoList.end(); it++){status = EC_READ_U16(domain_pd + (**it).offset.status_word);if ((status & command) == 0x0040){printf("Switch On disabled\n");EC_WRITE_U16(domain_pd + (**it).offset.ctrl_word, 0x0006);EC_WRITE_S8(domain_pd + (**it).offset.operation_mode, (**it).operation_modes);command = 0x006F;}/*Ready to switch On*/else if ((status & command) == 0x0021){EC_WRITE_U16(domain_pd + (**it).offset.ctrl_word, 0x0007);command = 0x006F;}/* Switched On*/else if ((status & command) == 0x0023){printf("Switched On\n");EC_WRITE_U16(domain_pd + (**it).offset.ctrl_word, 0x000f);if ((**it).operation_modes == PROFILE_VELOCITY){EC_WRITE_S32(domain_pd + (**it).offset.target_velocity, (**it).target_velocity);}else{EC_WRITE_S32(domain_pd + (**it).offset.target_position, (**it).target_position);EC_WRITE_S32(domain_pd + (**it).offset.profile_velocity, (**it).profile_velocity);}command = 0x006F;}// operation enabledelse if ((status & command) == 0x0027){printf("operation enabled:%d\n", cycles);if (cycles == 0)EC_WRITE_U16(domain_pd + (**it).offset.ctrl_word, 0x001f);if ((status & 0x400) == 0x400){printf("target reachedd\n");Run = 0;EC_WRITE_U16(domain_pd + (**it).offset.ctrl_word, 0x0180); // halt}cycles = cycles + 1;}}ecrt_domain_queue(domainServo);ecrt_master_send(master);usleep(10000);}return ((void *)0);
}
void ethercat_initialize()
{master = ecrt_request_master(0);ConfigPDO();if (ecrt_master_activate(master)){printf("Activating master...failed\n");return;}if (!(domain_pd = ecrt_domain_data(domainServo))){fprintf(stderr, "Failed to get domain data pointer.\n");return;}// 启动master Cycles Threadpthread_create(&cycle_thread, NULL, cyclic_task, NULL);
}
void check_domain_state(void)
{ec_domain_state_t ds = {};// ec_domain_state_t ds1 = {};// domainServoInputecrt_domain_state(domainServo, &ds);if (ds.working_counter != domainServo_state.working_counter){printf("domainServoInput: WC %u.\n", ds.working_counter);}if (ds.wc_state != domainServo_state.wc_state){printf("domainServoInput: State %u.\n", ds.wc_state);}domainServo_state = ds;
}
void check_slave_config_states(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;
}

主程序

/*****************************************************************************
sudo /etc/init.d/ethercat start
gcc testbyesm.c -Wall -I /opt/etherlab/include -l ethercat -L /opt/etherlab/lib -o testbyesm****************************************************************************/
#include  "time.h"
#include  "SevoController.hpp"
#include "ethercat.hpp"
#define Panasonic           0x0000066F,0x60380004
#define TASK_FREQUENCY          100 /*Hz*/
#define TIMOUT_CLEAR_ERROR  (1*TASK_FREQUENCY)  /*clearing error timeout*/
#define TARGET_VELOCITY         8388608 /*target velocity*/
#define PROFILE_VELOCITY            3   /*Operation mode for 0x6060:0*/
#define PROFILE_POSITION            1  
int main(){printf("EtherCAT Component Test\n");SevoController *Sevo1=new SevoController(0,Panasonic,PROFILE_POSITION);Sevo1->profile_velocity=TARGET_VELOCITY*100;Sevo1->target_velocity=TARGET_VELOCITY*10;Sevo1->target_position=TARGET_VELOCITY/2;SevoList.push_back(Sevo1);ethercat_initialize();while(1){sleep(10);}
}

小结

上面的程序基于松下A6 EtherCAT 伺服电机