IGH 控制伺服的一个例子（验证中）

1.建立相关文件，如下：

 

2.使用如下命令进行编译

$CC main.c check_state.c config_qn.c cycle_task.c ui_task.c -o ectest_mc -L ./include -lethercat -lpthread

其中$CC是交叉编译命令，跟gcc 命令类似。

ethercat IGH 在开发板上的库被放在include的文件见中，故使用./include 关联该库地址。使用-Iehercat引用该库。 使用线程的库，也用引用。

3.编译后提示如下：

没有报错，有一些警告，可以生产执行文件。

4.发送至开发板

scp ectest_mc root@192.168.58.1:/home/mc，使用该命令可以将编译生成的二进制文件发送给开发板。然后在开发板上运行。

5.相关代码如下

5.1 main.c

//$CC main.c check_state.c config_qn.c cycle_task.c ui_task.c -o ectest_mc -L ./include -lethercat -lpthread

//scp ectest_mc root@192.168.58.1:/home/mc

#include <pthread.h>
#include "include/check_state.h"
#include "include/config_qn.h"
#include "include/cycle_task.h"
#include "include/ui_task.h"

pthread_t id;        //unsigned long int
int i,ret;

int main(void)
{
    
    master = ecrt_request_master(0);
    if (!master) {
        return -1;
    }
    
    config_cooldrive();
    ret=pthread_create(&id,NULL,(int *)data_cycle_task(),NULL); 
    if (ret < 0) {
        fprintf(stderr, "xenomai Failed to create task: %s\n", strerror(-ret));
        return -1;
    }
    positon_control();
    
    pause();
    printf("release_master\n");
    ecrt_release_master(master);
    
    return 0;
}

 

5.2 Ui.c ui.h

#include "check_state.h"


typedef struct{
	int cntlwd;
	int tarpos;
	int status;
	int actpos;
	int ok;
	
}Aix_data;
void positon_control(void);
extern Aix_data A1_data;
/****************************************************************************/

void motor_enable();
void motor_disable();

#include "include/ui_task.h"

Aix_data A1_data;
void positon_control(){
	A1_data.ok=0;
	//int cntwrd,pos;
	printf("please input cntwrd and position(r/min)\n");
	scanf("%d %d",&A1_data.cntlwd,&A1_data.tarpos);
	printf("cntwrd:%d;position:%d\n",A1_data.cntlwd,A1_data.tarpos);
	
	while(!A1_data.ok){
		if(A1_data.ok){
			printf("status:%d;actpos:%d\n",A1_data.status,A1_data.actpos);
			break;
			A1_data.ok=0;
		}
	
	sleep(1);
	printf("wait data ok\n");
	
	}

}

void motor_enable(){
	
	do{
	A1_data.cntlwd=0x10;//enable operation
	sleep(0.1);
	A1_data.cntlwd=0x06;//shut down
	sleep(0.1);
	A1_data.cntlwd=0x07;//disable voltage,quik stop
	sleep(0.1);
	A1_data.cntlwd=0xf;// fault reset
	sleep(0.1);
	}
	while(!((A1_data.status& 0x06f) == 0x027));
	
	printf("enable servo success!\n");

}

void motor_disable(){
	
	do{
	A1_data.cntlwd=0x05;//disable operation
	sleep(0.1);
	A1_data.cntlwd=0x06;//shut down
	sleep(0.1);
	A1_data.cntlwd=0x07;//disable voltage,quik stop
	sleep(0.1);
	A1_data.cntlwd=0xf;//fault reset
	sleep(0.1);
	}
	while((A1_data.status& 0x06f) == 0x027);
	
	printf("disable servo success!\n");

}

 5.3 cycle.c cycle.h

#include "include/cycle_task.h"
#include "include/ui_task.h"
#include "include/config_qn.h"
#include "include/check_state.h"
struct timespec wakeup_time;

int data_cycle_task(){
    printf("xenomai Activating master...\n");
    if (ecrt_master_activate(master)) {
        printf("xenomai Activating master...failed\n");
        return -1;
    }
    /********************/
    if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
        fprintf(stderr, "xenomai Failed to get domain data pointer.\n");
        return -1;
    }
    if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
        fprintf(stderr, "xenomai Failed to get domain data pointer.\n");
        return -1;
    }
    printf("xenomai Activating master...success\n");
    while(1){
    	ecrt_master_receive(master);
    	EC_WRITE_U16(domainInput_pd+A1.cntlwd,A1_data.cntlwd);    	
    	ecrt_domain_process(domainServoInput);
    	check_domainInput_state();
    	
    	ecrt_domain_process(domainServoOutput);
    	A1_data.status=EC_READ_U16(domainOutput_pd+A1.status); 
    	check_domainOutput_state();
    	A1_data.ok=1;
    	
    	// send process data
    	ecrt_domain_queue(domainServoOutput);
    	ecrt_domain_queue(domainServoInput);
    	ecrt_master_send(master);
    	
    	wakeup_time.tv_nsec += PERIOD_NS;
        while (wakeup_time.tv_nsec >= NSEC_PER_SEC) {
            wakeup_time.tv_nsec -= NSEC_PER_SEC;
            wakeup_time.tv_sec++;
       }
    }
    
}

#include "check_state.h"

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


int data_cycle_task();

5.4 config_qn.h config_qn.c

#include "check_state.h"

typedef struct{
	unsigned int cntlwd;
	unsigned int tarpos;
	unsigned int modper;
	unsigned int DB1;
	unsigned int status;
	unsigned int actpos;
	unsigned int moddis;
	unsigned int DB2;
	unsigned int errcode;
	unsigned int errflo;

}Robot_Aix;


// process data
#define CoolDrivePos0  1, 0
#define CoolDrivePos1  2, 1

#define CoolDrive 0x00000748, 0x0000000a
//offsets for PDO entries

extern Robot_Aix A1,A2,A3,A4;

void config_cooldrive();

#include "include/config_qn.h"


Robot_Aix A1,A2,A3,A4;

void config_cooldrive(){
    printf("step1:request_master sucess\n");
    check_master_state();
    
    printf("step2:master get slave config\n");
    sc_cooldrive0=ecrt_master_slave_config(master,CoolDrivePos0,CoolDrive); 
    sc_cooldrive1=ecrt_master_slave_config(master,CoolDrivePos1,CoolDrive);       
    //check_slave_config_states();
    printf("step2.1:master_reset,Retry configuring slaves.\n");
    ecrt_master_reset(master); 
    printf("step2.2:check salve config states.\n");
    check_slave0_config_states();
    check_slave1_config_states();
    
    
    //pause();
    printf("step3:create domain.\n");
    domainServoOutput = ecrt_master_create_domain(master);
    domainServoInput = ecrt_master_create_domain(master);
    printf("step3.1:domain reg pdo entry.\n");
    
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive0,0x6040, 0x00,domainServoInput,&A1.cntlwd);
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive0,0x6041, 0x00,domainServoOutput,&A1.status);
    printf("step3.2:check domain.\n");
    check_domainInput_state();
    check_domainOutput_state();
    printf("step4:configure SYNC signals.\n");
    // configure SYNC signals for this slave
    ecrt_slave_config_dc(sc_cooldrive0, 0x0300, 1000000, 0, 0, 0);
    
    ecrt_master_reset(master); 


}

 5.5 check_state.h check_state.c

#include<stdio.h>
#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 <time.h> /* clock_gettime() */
#include <sys/mman.h> /* mlockall() */
#include <sched.h> /* sched_setscheduler() */
#include <sys/mman.h>
#include <pthread.h>
#include <math.h>
#define PI 3.1415926535898
#include"ecrt.h"

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

// EtherCAT
extern ec_master_t *master;
extern ec_master_state_t master_state;

extern ec_domain_t *domainServoInput ;
extern ec_domain_state_t domainServoInput_state ;
extern ec_domain_t *domainServoOutput ;
extern ec_domain_state_t domainServoOutput_state ;

extern uint8_t *domainOutput_pd ;
extern uint8_t *domainInput_pd ;

extern ec_slave_config_t *sc_cooldrive0 ;
extern ec_slave_config_state_t sc_cooldrive0_state;
extern ec_slave_config_t *sc_cooldrive1 ;
extern ec_slave_config_state_t sc_cooldrive1_state ;
/*************************************************************************/


void check_master_state(void);
void check_slave0_config_states(void);
void check_slave1_config_states(void);

void check_domainInput_state(void);
void check_domainOutput_state(void);

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

/** Task period in ns. */
#define PERIOD_NS   (1000000)

#define MAX_SAFE_STACK (8 * 1024) /* The maximum stack size which is
                                     guranteed safe to access without
                                     faulting */

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

/* Constants */
#define NSEC_PER_SEC (1000000000)
#define FREQUENCY (NSEC_PER_SEC / PERIOD_NS)

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

#include "include/check_state.h"


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

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

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

ec_slave_config_t *sc_cooldrive0 =NULL;
ec_slave_config_state_t sc_cooldrive0_state={};
ec_slave_config_t *sc_cooldrive1 =NULL;
ec_slave_config_state_t sc_cooldrive1_state={} ;
/****************************************************************************/

void check_master_state(void)
{
    ec_master_state_t ms;

    ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding) {
        printf("found %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_slave0_config_states(void)
{
    ec_slave_config_state_t s;

    ecrt_slave_config_state(sc_cooldrive0, &s);

    if (s.al_state != sc_cooldrive0_state.al_state) {
        printf("CoolDrive0: State 0x%02X.\n", s.al_state);
    }
    if (s.online != sc_cooldrive0_state.online) {
        printf("CoolDrive0: %s.\n", s.online ? "online" : "offline");
    }
    if (s.operational != sc_cooldrive0_state.operational) {
        printf("CoolDrive0: %soperational.\n", s.operational ? "" : "Not ");
    }

    sc_cooldrive0_state = s;
}
/*****************************************************************************/

void check_slave1_config_states(void)
{
    ec_slave_config_state_t s;

    ecrt_slave_config_state(sc_cooldrive1, &s);

    if (s.al_state != sc_cooldrive1_state.al_state) {
        printf("CoolDrive1: State 0x%02X.\n", s.al_state);
    }
    if (s.online != sc_cooldrive1_state.online) {
        printf("CoolDrive1: %s.\n", s.online ? "online" : "offline");
    }
    if (s.operational != sc_cooldrive1_state.operational) {
        printf("CoolDrive1: %soperational.\n", s.operational ? "" : "Not ");
    }

    sc_cooldrive1_state = s;
}
/*****************************************************************************/

void check_domainInput_state(void)
{
    ec_domain_state_t ds;

    ecrt_domain_state(domainServoInput, &ds);

    if (ds.working_counter != domainServoInput_state.working_counter) {
        printf("Domain1: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoInput_state.wc_state) {
        printf("Domain1: State %u.\n", ds.wc_state);
    }

    domainServoInput_state = ds;
}
/*****************************************************************************/

void check_domainOutput_state(void)
{
    ec_domain_state_t ds;

    ecrt_domain_state(domainServoOutput, &ds);

    if (ds.working_counter != domainServoOutput_state.working_counter) {
        printf("Domain1: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoOutput_state.wc_state) {
        printf("Domain1: State %u.\n", ds.wc_state);
    }

    domainServoOutput_state = ds;
}
/*****************************************************************************/