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 伺服电机。