IGH主站通信測(cè)試

linuxcnc配置基礎(chǔ)機(jī)器人控制LinuxCNC與EtherCAT介紹&&PDO&SDO，搭建環(huán)境步驟

需要配置IGH主站的查看這篇文章
linux系統(tǒng)學(xué)習(xí)筆記7——一次性安裝igh-ethercat主站

直接上代碼，這部分是直接控制使用csp模式控制一個(gè)從站運(yùn)動(dòng)
使能后直接運(yùn)動(dòng)，10s，每秒607a（目標(biāo)位置）增加100.
注意：急停 按下ESC

代碼分為兩部分，一個(gè)是通信線程 主要負(fù)責(zé)和伺服通信，使能伺服，讀取和寫入寄存器值。
第二個(gè)是操作線程， 負(fù)責(zé)修改位置的值，和監(jiān)控按鍵。

使用此代碼，首先根據(jù)手冊(cè)
1.修改PDO條目 ，要和自己的伺服一致
2.修改 PID VID 的值
3.根據(jù)自己?jiǎn)?dòng)模式，修改6040的控制字，比如筆者的是csp模式，6040：6-7-15 才能使能。

/*********************************************************************
 * ethercat通訊驅(qū)動(dòng)禾川伺服驅(qū)動(dòng)器X3EB**********************************
 * IGH主站PRMEET RT*************************************************
 * HUALAI機(jī)械臂******************************************************
 * DC同步模式
 * csp同步周期位置模式
 * 通信成功后運(yùn)動(dòng)，急停按下ESC
 * 作者：sf9090
 * 時(shí)間：2023/3/1
 * *********************************************************
 * ********************************************************************
**********************************************************************/
#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <sys/mman.h>
#include <pthread.h>
#include <math.h>
#include <malloc.h>
#include <sched.h> /* sched_setscheduler() */
/****************************************************************************/
#include "ecrt.h"
#include <time.h>
/*-------------------------------------監(jiān)聽鍵盤----------------------------------*/
#include <termios.h>
#include <fcntl.h> 
/****************************************************************************/
#include <stdbool.h>


#define FREQUENCY 1000
#define TASK_FREQUENCY          10 /*Hz*/

#define     Bool                              int
#define     false                             0
#define     true                              1
#define     ETHERCAT_STATUS_OP                0x08
#define     STATUS_SERVO_ENABLE_BIT           (0x04)

#define     CLOCK_TO_USE         CLOCK_REALTIME
#define     NSEC_PER_SEC         (1000000000L)
static int64_t  system_time_base = 0LL;
struct timespec wakeupTime;

#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY) /*本次設(shè)置周期PERIOD_NS為1ms*/

#define DIFF_NS(A, B)                                                          \
  (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + (B).tv_nsec - (A).tv_nsec)

#define TIMESPEC2NS(T) ((uint64_t)(T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)
//獲取當(dāng)前系統(tǒng)時(shí)間

//master status
typedef enum  _SysWorkingStatus
{
    SYS_WORKING_POWER_ON,
    SYS_WORKING_SAFE_MODE,
    SYS_WORKING_OP_MODE,
    SYS_WORKING_LINK_DOWN,
    SYS_WORKING_IDLE_STATUS       //系統(tǒng)空閑
}SysWorkingStatus;

typedef  struct  _GSysRunningParm
{
    SysWorkingStatus   m_gWorkStatus;
}GSysRunningParm;

GSysRunningParm    gSysRunning;

bool Isquit = true;/*判斷是否停止*/
bool flag = false;
const int slave = 0;/*從站號(hào)*/
int SendNumber = 0;/*發(fā)送的次數(shù)*/
int ecstate = 0;/*循環(huán)次數(shù)，用來初始化主站的變量*/
bool IsRun = false;/*判斷伺服是否使能*/
/****************************************************************************/

// EtherCAT
ec_master_t *master = NULL;
static ec_master_state_t master_state = {};

static ec_domain_t *domainServoInput = NULL;
static ec_domain_state_t domainServoInput_state = {};
static ec_domain_t *domainServoOutput = NULL;
static ec_domain_state_t domainServoOutput_state = {};

static uint8_t *domainOutput_pd = NULL;
static uint8_t *domainInput_pd = NULL;

static ec_slave_config_t *sc_estun;
static ec_slave_config_state_t sc_estun_state;
/****************************************************************************/
#define estun_Pos0 0, 0
#define estun  0x000116c7, 0x003e0402
// offsets for PDO entries
static unsigned int  errcode;

static unsigned int  cntlwd;/*控制字*/
static unsigned int  ipData;/*目標(biāo)位置*/
static unsigned int  modes_of_operation;/*模式*/
static unsigned int  status;/*狀態(tài)字*/
static unsigned int  actpos;/*實(shí)際位置*/
static unsigned int  modes_of_operation_display;/*讀取模式*/
static unsigned int current_velocity;/*當(dāng)前速度*/
static int cur_status;/*當(dāng)前狀態(tài)字*/
static int cur_mode;/*當(dāng)前模式*/
static int curpos = 0;/*當(dāng)前位置*/
int curpos_offset = 0;/*設(shè)置當(dāng)前位置*/
// process data
static unsigned int counter = 0;
static unsigned int blink = 0;
static unsigned int sync_ref_counter = 0;
const struct timespec cycletime = {0, PERIOD_NS};
// 


ec_pdo_entry_reg_t domainServoOutput_regs[] = {

    {estun_Pos0, estun, 0x6040, 0x00, &cntlwd, NULL},
    {estun_Pos0, estun, 0x6060, 0x00, &modes_of_operation, NULL},           //6060 模式選擇
    {estun_Pos0, estun, 0x607A, 0x00, &ipData, NULL},    
    {}

};
ec_pdo_entry_reg_t domainServoInput_regs[] = {
     {estun_Pos0, estun, 0x6041, 0x00, &status, NULL},
      {estun_Pos0, estun, 0x606C, 0x00, &current_velocity, NULL},
    {estun_Pos0, estun, 0x6064, 0x00, &actpos, NULL},
   
    {}
};
/*****************************************************************************/

// 兩個(gè)時(shí)間相加
struct timespec timespec_add(struct timespec time1, struct timespec time2) {
  struct timespec result;

  if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
    result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
    result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
  } else {
    result.tv_sec = time1.tv_sec + time2.tv_sec;
    result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
  }

  return result;
}
/*********************************************************
 * PDO條目根據(jù)伺服驅(qū)動(dòng)器說明書上相應(yīng)的模式設(shè)置
 * 查看ethercat cstruct
**********************************************************/
ec_pdo_entry_info_t estun_pdo_entries[] = {


    /*RxPdo 0x1600*/
    {0x6040, 0x00, 16},
    {0x6060, 0x00, 8 }, 
    {0x607a, 0x00, 32},
    /*TxPdo 0x1A00*/
    {0x6041, 0x00, 16},
    {0x606C, 0x00, 32},
    {0x6064, 0x00, 32}

};

ec_pdo_info_t estun_pdos[] = {
    {0x1600, 3, estun_pdo_entries + 0}, /* CoE CSP Mode (RX) */
    {0x1a00, 3, estun_pdo_entries + 3}, /* CoE CSP Mode (TX) */

};

ec_sync_info_t estun_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, estun_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 1, estun_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

/****************************************************************************/
int ConfigPDO()
{
    /********************/
    printf("  Configuring PDOs...\n");
    domainServoOutput = ecrt_master_create_domain(master);
    if (!domainServoOutput) {
        return -1;
    }
    domainServoInput = ecrt_master_create_domain(master);
    if (!domainServoInput) {
        return -1;
    }
    /********************/
    printf("  Creating slave configurations...\n");
    sc_estun =
        ecrt_master_slave_config(master, estun_Pos0, estun);
    if (!sc_estun) {
        fprintf(stderr, "Failed to get slave configuration.\n");
        return -1;
    }
    /********************/
    if (ecrt_slave_config_pdos(sc_estun, EC_END, estun_syncs)) {
        fprintf(stderr, "Failed to configure PDOs.\n");
        return -1;
    }
    /********************/
    if (ecrt_domain_reg_pdo_entry_list(domainServoOutput, domainServoOutput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    if (ecrt_domain_reg_pdo_entry_list(domainServoInput, domainServoInput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }

    fprintf(stderr, "Creating SDO requests...\n");
    //有些伺服沒有sdo功能的不需要配置
    ecrt_slave_config_sdo8(sc_estun, 0x6060, 0, 8);
    ecrt_slave_config_sdo8(sc_estun, 0x60C2, 1, 1);

    return 0;
}


/*****************************************************************************
 * Realtime task
 ****************************************************************************/

void rt_check_domain_state(void)
{
    ec_domain_state_t ds = {};
    ec_domain_state_t ds1 = {};
    //domainServoInput
    ecrt_domain_state(domainServoInput, &ds);
    if (ds.working_counter != domainServoInput_state.working_counter) {
        printf("domainServoInput: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoInput_state.wc_state) {
        printf("domainServoInput: State %u.\n", ds.wc_state);
    }
    domainServoInput_state = ds;
    //domainServoOutput
    ecrt_domain_state(domainServoOutput, &ds1);
    if (ds1.working_counter != domainServoOutput_state.working_counter) {
        printf("domainServoOutput: WC %u.\n", ds1.working_counter);
    }
    if (ds1.wc_state != domainServoOutput_state.wc_state) {
        printf("domainServoOutput: State %u.\n", ds1.wc_state);
    }
    domainServoOutput_state = ds1;
}

/****************************************************************************/

void rt_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 check_slave_config_states(void)
{
    ec_slave_config_state_t s;
    ecrt_slave_config_state(sc_estun,&s);
    if (s.al_state != sc_estun_state.al_state)
        printf("sc_estun_state: State 0x%02X.\n", s.al_state);
    if (s.online != sc_estun_state.online)
        printf("sc_estun_state: %s.\n", s.online ? "online" : "offline");
    if (s.operational != sc_estun_state.operational)
        printf("sc_estun_state: %soperational.\n",s.operational ? "" : "Not ");
    sc_estun_state = s;


}
/****************************************************************************/
void ReleaseMaster()
{
    if(master)
    {
        printf("  End of Program, release master\n");
        ecrt_release_master(master);
        master = NULL;
    }
}
/****************************************************************************/
int ActivateMaster()
{
    int ret;
    printf("  Requesting master...\n");
    if(master)
        return 0;
    master = ecrt_request_master(0);
    if (!master) {
        return -1;
    }

    ConfigPDO();

    // configure SYNC signals for this slave
    ecrt_slave_config_dc(sc_estun, 0x0300, 1000000, 0, 0, 0);
/********************************************************
 *       配置從站 sync0 和 sync1 信號(hào)
 * 最后四個(gè)參數(shù)表示設(shè)置 sync 0和1 的周期和相應(yīng)的偏移量，單位都是 ns。
 *  sync0_cycle即為sync0的循環(huán)周期，和主棧的周期任務(wù)的循環(huán)周期保持一致。這個(gè)是很重要的一點(diǎn)，需要注意。

  * 1、查看 esi 文件，不支持 sync1 同步，所以需要設(shè)置成 0x0300
  * 0x300: 0x0981的 0,1 位 置 1,其他位 置 0,表示激活運(yùn)行周期， 激活 sync0
 * 0x700: 0x0981的 0,1,2 位 置 1,其他位 置 0,表示激活運(yùn)行周期， 激活 sync0，和 sync1 
  * 2、查看 ethercat upload 0x1c32 0x0004 中 bit5-6 為零，代表不支持 shift time，因此第三個(gè)參數(shù)設(shè)置為 0
 *  3、一般不使用sync1同步信號(hào)，最后兩個(gè)參數(shù)可設(shè)置為0。
  * 同步周期設(shè)置成 1ms
 * ********************************************************/



    
    // ecrt_master_application_time(master, system_time_ns());
    ret = ecrt_master_select_reference_clock(master, NULL);
    if (ret < 0) {
        fprintf(stderr, "  Failed to select reference clock: %s\n",
                strerror(-ret));
        return ret;
    }

    /********************/
    printf("  Activating master...\n");
    if (ecrt_master_activate(master)) {
        printf("  Activating master...failed\n");
        return -1;
    }
    /********************/
    if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
        fprintf(stderr, "  Failed to get domain data pointer.\n");
        return -1;
    }
    if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
        fprintf(stderr, "  Failed to get domain data pointer.\n");
        return -1;
    }
    printf("  Activating master...success\n");
    return 0;
}
/******************************************************************************
 * 判斷是否有按鍵按下
******************************************************************************/
int kbhit()
{
        struct termios oldt, newt;

        int ch;

        int oldf;

        tcgetattr(STDIN_FILENO, &oldt);

        newt = oldt;

        newt.c_lflag &= ~(ICANON | ECHO);

        tcsetattr(STDIN_FILENO, TCSANOW, &newt);

        oldf = fcntl(STDIN_FILENO, F_GETFL, 0);

        fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);

        ch = getchar();

        tcsetattr(STDIN_FILENO, TCSANOW, &oldt);

        fcntl(STDIN_FILENO, F_SETFL, oldf);

        if(ch != EOF)

        {

            ungetc(ch, stdin);

            return 1;

        }

        return 0;

}
/*******************************************************************************/


/****************************************************************************
 * 按鍵定義
 * ESC：退出程序
 * 
 ***************************************************************************/
void listen_button()
{

        
    char key;
    if(kbhit())//檢測(cè)是否有鍵盤輸入
    {

        key=getchar();
        
        printf(" key = %d \n",key);
        if(key==27)//esc鍵的ASCII為27
        { 
            printf("退出程序\n");
           curpos_offset  = 0;
            Isquit = false;

        }
     
    }
    
}
/*********************************************
 * 操作線程
*********************************************/
void *OperationThread(void * arg)
{
    while(1)
    {
        usleep(10000);
        listen_button();
        if(!Isquit){
            IsRun  = false;
            ReleaseMaster();
            exit(0);
        }
        if ( IsRun){//如過伺服驅(qū)動(dòng)器使能，這里就開始運(yùn)行，知道10s后會(huì)停止運(yùn)動(dòng)，這里運(yùn)動(dòng)是通過修改607a的值實(shí)現(xiàn)
            if(SendNumber >= 10000)
                curpos_offset  = 0;
           else
                curpos_offset=100.0;
        }      
    }
}
/*******************************************************************************/

/****************************************************************************/


/****************************************************************************/
 void DriverEtherCAT()
{
    static int i = 0;
    int status1= 0;
   
    //處于剛開機(jī)(需要等待其他操作完成），返回等待下次周期
    if(gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
        return ;

    static int cycle_counter = 0;
    cycle_counter++;
    if(cycle_counter >= 120*1000){
        cycle_counter = 0;

    }

    // receive EtherCAT frames
    ecrt_master_receive(master);
    ecrt_domain_process(domainServoOutput);
    ecrt_domain_process(domainServoInput);
    rt_check_domain_state();

    if (!(cycle_counter % 500)) {
        rt_check_master_state();
        check_slave_config_states();
    }
    curpos = EC_READ_S32(domainInput_pd + actpos);  
    //狀態(tài)機(jī)操作
    switch (gSysRunning.m_gWorkStatus)
    {
    case SYS_WORKING_SAFE_MODE:{
        //檢查主站是否處于 OP 模式, 若不是，則調(diào)整為 OP 模式
        rt_check_master_state();
        check_slave_config_states();
        if((master_state.al_states & ETHERCAT_STATUS_OP))
        {
            int tmp = true;

            if(sc_estun_state.al_state != ETHERCAT_STATUS_OP)
            {
                tmp = false;
                break ;
            }

            if(tmp)
            {
                ecstate = 0;
                gSysRunning.m_gWorkStatus = SYS_WORKING_OP_MODE;
                printf("  SYS_WORKING_OP_MODE\n");
            }
        }
    }break;

    case SYS_WORKING_OP_MODE:
    {
        ecstate++;
        //使能伺服
        if(ecstate <= 900)
        {
            switch (ecstate){
            case 1:
                EC_WRITE_U8(domainOutput_pd + modes_of_operation, 8);
                break;
            case 200:
            //    EC_WRITE_U16(domainOutput_pd + cntlwd, 0x80);    //錯(cuò)誤復(fù)位  
                cur_status=  EC_READ_U16(domainOutput_pd+ cntlwd);
                printf(" my status_world 0X6040 = %d\n",cur_status);
                cur_status = EC_READ_U16(domainInput_pd + status);
                printf(" my status_world 0x6041 = %d\n",cur_status);
         
                break;
            case 300:
               curpos = EC_READ_S32(domainInput_pd + actpos);  
               EC_WRITE_S32(domainOutput_pd + ipData, curpos);
                printf("x@rtITP >>> Axis  current position = %d\n", curpos);

                break;
            case 400:
                EC_WRITE_U16(domainOutput_pd + cntlwd, 0x06);
               status1=  EC_READ_U16(domainInput_pd+ status);
                printf(" my status_world 0X6041 = %d \n",status1);
                break;
            case 500:
                EC_WRITE_U16(domainOutput_pd + cntlwd, 0x07);
                status1=  EC_READ_U16(domainInput_pd+ status);
                printf(" my status_world 0X6041 = %d \n",status1);
                break;
            case 600:

                EC_WRITE_U16(domainOutput_pd + cntlwd, 0xF);
                status1=  EC_READ_U16(domainInput_pd+ status);
                printf(" my status_world 0X6041 = %d \n",status1);
                break;              
            }

        }
        else {
         
            printf("enable servo success!\n");

            int tmp  = true;
            if((EC_READ_U16(domainInput_pd + status) & (STATUS_SERVO_ENABLE_BIT)) == 0)
            {
                tmp = false;
                printf("EC_READ_U16(domainInput_pd + status) & (STATUS_SERVO_ENABLE_BIT)==0\n");
                break ;
            }
            if(tmp)
            {
                ecstate = 0;
                gSysRunning.m_gWorkStatus = SYS_WORKING_IDLE_STATUS;
                printf("  SYS_WORKING_IDLE_STATUS\n");              
            }
        }
    }break;
    default://  在周期任務(wù)里，判斷當(dāng)電機(jī)切換為使能狀態(tài)后，讀取電機(jī)當(dāng)前位置，并把這個(gè)數(shù)值加數(shù)據(jù)域設(shè)置目標(biāo)作為目標(biāo)位置寫入電機(jī)
    {
       
        IsRun = true;       
        EC_WRITE_S32(domainOutput_pd + ipData,curpos+curpos_offset );//  下發(fā)位置命令
        SendNumber++;
    }break;
 
    }
 
    // write application time to master

    // ecrt_master_application_time(master, system_time_ns());
    ecrt_master_sync_reference_clock(master);  //將指定的時(shí)間發(fā)送給參考時(shí)鐘，這個(gè)函數(shù)將最近一次從ecrt_master_application_time傳入的時(shí)間戳發(fā)送給參考時(shí)鐘。
      // 這就是官方example中除了rtai_rtdm_dc這個(gè)例子使用的默認(rèn)dc同步方式，即所謂的以主站作為參考時(shí)鐘的方式。

    ecrt_master_sync_slave_clocks(master);          //將DC時(shí)鐘漂移補(bǔ)償數(shù)據(jù)報(bào)排隊(duì)發(fā)送，讓所有從站時(shí)鐘與基準(zhǔn)時(shí)鐘同步

    // send process data
    ecrt_domain_queue(domainServoOutput);
    ecrt_domain_queue(domainServoInput);
    ecrt_master_send(master);
}
/****************************************************************************/

void *InterpolationThread(void * arg)
{
   clock_gettime(CLOCK_TO_USE, &wakeupTime);
    while (1) {
        usleep(10000);
        wakeupTime = timespec_add(wakeupTime, cycletime);

    // TIMER_ABSTIME： 代表是采用的絕對(duì)時(shí)間
    // 最后一個(gè)參數(shù) rept的意思： 該函數(shù)將會(huì)使得調(diào)用進(jìn)程處于掛起狀態(tài)，直到請(qǐng)求的時(shí)間到達(dá)或者是被信號(hào)中斷，參數(shù)reqtp指定了需要睡眠的秒數(shù)與納秒數(shù)。
    // 如果在睡眠中途被信號(hào)中斷，且進(jìn)程沒有終止的話，timespec結(jié)構(gòu)指針remtp指向的結(jié)構(gòu)將保存剩余的睡眠時(shí)間，如果我們對(duì)于未睡眠時(shí)間不感興趣的話，我們可以把這一時(shí)間設(shè)置為NULL.
    // 如果系統(tǒng)不支持納秒時(shí)間精度的話，請(qǐng)求時(shí)間將被向上取整，因?yàn)楹瘮?shù)nanosleep并不涉及任何信號(hào)的產(chǎn)生，我們可以放心大膽地使用它而不用擔(dān)心與其他函數(shù)相互影響。
         clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);
         ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime));
        //Delay the calling task (absolute).Delay the execution of the calling task until a given date is reached.
 
        DriverEtherCAT();
    }
}



/****************************************************************************
 * Main function
 ***************************************************************************/

int main(int argc, char *argv[])
{
    int ret;

   mlockall(MCL_CURRENT | MCL_FUTURE);

    gSysRunning.m_gWorkStatus = SYS_WORKING_POWER_ON;
    if(gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
    {
        ActivateMaster();
        ecstate = 0;
        gSysRunning.m_gWorkStatus = SYS_WORKING_SAFE_MODE;
        printf("  SYS_WORKING_SAFE_MODE\n");
    }


    printf("Starting InterpolationTask...\n");

    pthread_t id[3];
    int i = 0;
    pthread_create(&id[0],NULL,InterpolationThread,NULL);//通訊線程
    pthread_create(&id[1],NULL,OperationThread,NULL);//操作線程
    pthread_join(id[0],NULL);
    pthread_join(id[1],NULL);
    printf("  Deleting realtime InterpolationTask task...\n");

    return 0;
}

參考：https://blog.csdn.net/weixin_40293570/article/details/108712655文章來源地址http://www.zghlxwxcb.cn/news/detail-793727.html

到了這里，關(guān)于IGH主站通信測(cè)試csp模式（DC同步 preemrt）連通一從站并實(shí)現(xiàn)控制的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！