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 enabled
else 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 Thread
pthread_create(&cycle_thread, NULL, cyclic_task, NULL);
}
void check_domain_state(void)
{
ec_domain_state_t ds = {};
// ec_domain_state_t ds1 = {};
// domainServoInput
ecrt_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 伺服电机
