当前位置： 首页 > article >正文

EtherCAT 伺服控制功能块实现

article 2024/11/15 11:12:32

        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_IF

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);
    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 <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;


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++)
    {
     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<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 伺服电机 .可以运行。在slave 类的基础上实现IEC61499 功能块相对比较容易。

        当使用etherCAT 运动器基础上，实现PLCopen 运动控制功能块库是否要容易一些呢？我对运动控制不熟悉。希望读者给我一些建议。


http://www.kler.cn/a/134796.html

相关文章：

  • 蓝桥杯c++算法学习【2】之搜索与查找（九宫格、穿越雷区、迷宫与陷阱、扫地机器人：：：非常典型的必刷例题！！！）
  • 专题十八_动态规划_斐波那契数列模型_路径问题_算法专题详细总结
  • TCP/IP协议，TCP和UDP区别
  • WPF学习之路，控件的只读、是否可以、是否可见属性控制
  • ABC334
  • 94个属于一区且接受医工交叉领域投稿的期刊汇总｜个人观点·24-11-13
  • 3D建模基础教程：编辑多边形功能命令快捷方式
  • SpringBoot 整合 Freemarker
  • 小程序判断是否授权位置信息和手动授权
  • 【每日一题】最大子数组和
  • 小程序商城免费搭建之java商城 电子商务Spring Cloud+Spring Boot+二次开发+mybatis+MQ+VR全景+b2b2c
  • 越南MIC新规针对ICT和ITE产品电气授权标准变更
  • 一起学docker系列之四docker的常用命令--系统操作docker命令及镜像命令
  • Springcloud可视化物联网智慧工地云SaaS平台源码 支持二开和私有化部署
  • 沸点 | Ultipa 图数据库金融应用场景优秀案例首批入选，金融街论坛年会发布
  • Chat GPT 用于论文润色，常用指令这里都全了
  • ts视频文件转为mp4(FFmpeg)
  • 『亚马逊云科技产品测评』活动征文｜基于next.js搭建一个企业官网
  • 每天一道算法题（五）——判断一组数字是否连续，出现连续数字的时候以‘-’输出
  • Flutter笔记：目录与文件存储以及在Flutter中的使用（上）
  • Git 提交竟然还能这么用？
  • css设置下划线
  • MCU内存基础知识
  • 下载node-sass
  • Vue 3.0 中重置 reactive 定义的响应式对象数据，恢复为初始值
  • grafana面板介绍