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 功能块库？
PLCopen 的功能块分为管理功能块和运动功能块。管理功能块不会引起轴的运动。
管理功能块
| 功能块 | 功能说明 |
| MC_Power | |
| MC_ReadStatus | |
| MC_ReadDigitalInput | |
| MC_ReadDigitalOutput |
运动功能块
| 功能块 | 功能说明 |
| MC_Home | 复位到原点 |
| MC_Stop | 被控轴减速并停止 |
| MC_MoveAbsolute | 以原点为基础，以绝对位置为目标的运动 |
| MC_MoveRelative | 以运动点为基础，相对距离为目标的运动 |
| MC_MoveVelocity | 对未终止的运动以规定的速度运动 |
| MC_TorqueControl | 被控轴施加规定的转矩或者力，达到该值后以该值恒定施加。 |
| MC_PositionProfile | 位置配置 |
| MC_VelocityProfile | 速度配置 |
一个开源项目GitHub - i5cnc/plcopen: plcopen中的运动控制功能块。
| 功能块 | 描述 |
|---|---|
| MC_Power | 控制电源（开或关）。 |
| MC_Home | 命令轴执行“search home”序列。 |
| MC_Stop | 命令受控运动停止并将轴传递到“停止”状态。 |
| MC_Halt | 命令受控运动停止并将轴传递到“静止”状态。 |
| MC_MoveAbsolute | 命令受控运动到指定的绝对位置。 |
| MC_MoveRelative | 命令相对于执行时设定的位置具有指定距离的受控运动。 |
| MC_MoveAdditive | 命令除最近命令位置之外的指定相对距离的受控运动。 |
| MC_MoveVelocity | 以指定速度指挥永无止境的受控运动。 |
| MC_ReadStatus | 详细返回所选轴的状态图的状态。 |
| MC_ReadMotionState | 详细返回轴相对于当前正在进行的运动的状态。 |
| MC_ReadAxisError | 读取有关轴的信息，如模式、与轴直接相关的输入以及某些状态信息。 |
| MC_EmergencyStop | 命令轴立即停止，并将轴转换为状态“ErrorStop”。 |
| MC_Reset | 通过重置所有与内部轴相关的错误，从状态“ErrorStop”转换为“Standstill”或“Disabled”。 |
| MC_ReadActualPosition | 返回实际位置。 |
| MC_ReadCommandPosition | 返回命令位置。 |
| MC_ReadActualVelocity | 返回实际速度。 |
| MC_ReadCommandVelocity | 返回命令速度。 |
PLCopen 的多轴运动规范
PLCopen Part4 Coordinated Motion 规范是针对多轴的坐标运动。适合类似机器人，机械臂的多轴运动控制。
多轴运动控制的基本思想是将多个轴组成组（group），以3D空间坐标和轨迹来控制运动。下面是多轴运动控制功能块列表：
IEC61499 运动功能块库
PLCopen 运动控制功能块库是基于IEC131-3 功能块定义的，将IEC61131-3 功能块转换为IEC61499 基于事件的功能块并非难事。下面是一个例子：
在具体实现中，功能块要调用相关的EtherCAT 控制功能，通过EtherCAT /CANopen 控制伺服电机的运动控制。
注：Axis_Ref 用作运动功能块的输入，用于指定要用作此功能块的主轴或从轴的轴。
使用功能块的一个例子：
PROGRAM MOTION_PRG
VAR
iStatus: INT;
Power: MC_Power;
MoveAbsolute: MC_MoveAbsolute;
p:REAL:=100;
END_VAR
CASE iStatus OF
// initialization of the axis
0:
Power(Enable:=TRUE, bRegulatorOn:=TRUE, bDriveStart:=TRUE, Axis:=Drive);
IF Power.Status THEN
iStatus := iStatus + 1;
END_IF
// Move the axis to position p by use of the MC_MoveAbsolute function block
1:
MoveAbsolute(Execute:=TRUE, Position:= p, Velocity:=100, Acceleration:=100, Deceleration:=100, Axis:=Drive);
IF MoveAbsolute.Done THEN
MoveAbsolute(Execute:=FALSE, Axis:=Drive);
iStatus := iStatus + 1;
END_IF
// Move the axis back to position 0 by use of the MC_MoveAbsolute function block:
2:
MoveAbsolute(Execute:=TRUE, Position:= 0, Velocity:=100, Acceleration:=100, Deceleration:=100, Axis:=Drive);
IF MoveAbsolute.Done THEN
MoveAbsolute(Execute:=FALSE, Axis:=Drive);
iStatus := 1;
END_IFEtherCAT 主站程序
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
#include
#include
#include
#include
#include
#include
#include
#include
#include
#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);
int SlaveConfig(ec_master_t *master,ec_pdo_entry_reg_t *domainServo_regs,int index);
SevoController(uint32_t Position, uint32_t Vendor_id, uint32_t Product_cdode, uint32_t Modes_operation);
private:
ec_pdo_entry_info_t pdo_entries[8] = {
/*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}
};
ec_pdo_info_t Slave_pdos[2] = {
// RxPdo
{0x1600, 5, pdo_entries + 0},
// TxPdo
{0x1A00, 3, pdo_entries + 5}};
ec_sync_info_t Slave_syncs[5] = {
{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}};
};
#endif slave class cpp
#include "SevoController.hpp"
SevoController::SevoController(uint32_t Position, uint32_t Vendor_id, uint32_t Product_code, uint32_t Modes_operation)
{
target_position = Position;
vendor_id = Vendor_id;
product_code = Product_code;
operation_modes = Modes_operation;
}
int SevoController::SlaveConfig(ec_master_t *master, ec_pdo_entry_reg_t *domainServo_regs, int index)
{
domainServo_regs[index++] = {0, position, vendor_id, product_code, 0x6040, 0x00, &(offset.ctrl_word)};
domainServo_regs[index++] = {0, position, vendor_id, product_code, 0x6060, 0x00, &(offset.operation_mode)};
domainServo_regs[index++] = {0, position, vendor_id, product_code, 0x60FF, 0x00, &(offset.target_velocity)};
domainServo_regs[index++] = {0, position, vendor_id, product_code, 0x607A, 0x00, &(offset.target_position)};
domainServo_regs[index++] = {0, position, vendor_id, product_code, 0x6081, 0x00, &(offset.profile_velocity)};
domainServo_regs[index++] = {0, position, vendor_id, product_code, 0x6041, 0x00, &(offset.status_word)};
domainServo_regs[index++] = {0, position, vendor_id, product_code, 0x6061, 0x00, &(offset.mode_display)};
domainServo_regs[index++] = {0, position, vendor_id, product_code, 0x606C, 0x00, &(offset.current_velocity)};
slave_config = ecrt_master_slave_config(master, 0, position, vendor_id, product_code);
ecrt_slave_config_pdos(slave_config, EC_END, Slave_syncs);
return index;
}
void SevoController::eventAction(string EventName)
{
// under development IEC61499 FB's eventAction
}控制代码
#include "ecrt.h"
#include "stdio.h"
#include
#include
#include
#include "SevoController.hpp"
#include
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 SevoList;
ec_pdo_entry_reg_t *domainServo_regs;
int ConfigPDO()
{
domainServo = ecrt_master_create_domain(master);
if (!domainServo)
{
return -1;
}
//
domainServo_regs = new ec_pdo_entry_reg_t[9];
std::list::iterator it;
int index = 0;
for (it = SevoList.begin(); it != SevoList.end(); it++)
{
index= (**it).SlaveConfig(master,domainServo_regs,index);
}
domainServo_regs[index++] = {};
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::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);
}
}基于PLCopen 的机械臂控制
对运动控制来说，通常三个领域：CNC、RC（Robot Control）和 GMC（General Motion Control）。现在，通用运动控制器，PLC 的界线越来越模糊，CNC和RC 仍然以专有控制器为主。
小结
上面的程序基于松下A6 EtherCAT 伺服电机 .可以运行。在slave 类的基础上实现IEC61499 功能块相对比较容易。
当使用etherCAT 运动器基础上，实现PLCopen 运动控制功能块库是否要容易一些呢？我对运动控制不熟悉。希望读者给我一些建议。