| EtherCAT 伺服控制功能块实现 | 您所在的位置：网站首页 › plc怎么控制伺服驱动器程序 › EtherCAT 伺服控制功能块实现 |
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 运动控制库，它是PLC 上的一个功能块集。PLC 的应用程序通过这些功能块能够方便地实现运动控制。但是这些功能块如何实现，如何与硬件驱动结合。内部实现应该是比较复杂的。笔者看来，应该有两种方式： PLC 内嵌运动控制模型 通过Ethercat 总线外接运动控制模块两种结构的实现方法应该各不相同。是否有支持etherCAT 的PLCopen 功能块库？ PLCopen 的功能块分为管理功能块和运动功能块。管理功能块不会引起轴的运动。 管理功能块 功能块功能说明MC_PowerMC_ReadStatusMC_ReadDigitalInputMC_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空间坐标和轨迹来控制运动。下面是多轴运动控制功能块列表：
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_IF EtherCAT 主站程序EtherCAT 协议是倍福公司提出的，从站通常使用专用ASIC 芯片，FPGA 实现，而主站使用通用Ethernet接口和软件实现。EtherCAT 主站协议有专业公司开发的商业化产品，也有开源代码，下面是两个比较流行的EtherCAT Master IgHSOEM感觉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}}; }; #endifslave 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 运动控制功能块库是否要容易一些呢？我对运动控制不熟悉。希望读者给我一些建议。 |
【本文地址】
公司简介
联系我们