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;
}
/*****************************************************************************/