本程序在xenomai的基础上，用posix接口，编写了IGH的应用层代码，拓扑结构是一个master主站，两个slaves从站。主站通过PDO的方式，发送信息给从站。从站的MCU通过ethercat接口接收到信息后，控制伺服电机的运转。
#include <errno.h>
#include <mqueue.h>
#include <signal.h>
#include <pthread.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <limits.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <time.h>
#include <limits.h>
#include <rtdm/rtdm.h>
#include "ecrt.h"
#define USEC_PER_SEC 1000000000
static pthread_t cyclic_thread0;
static pthread_t cyclic_thread1;
static unsigned int cycle0_us = 800000; /* max 2000000 */
static unsigned int cycle1_us = 400000; /* max 2000000 */
static int run = 1;
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};
static ec_domain_t *domain0 = NULL;
static ec_domain_state_t domain0_state = {};
static ec_domain_t *domain1 = NULL;
static ec_domain_state_t domain1_state = {};
static ec_slave_config_t *slave_0;
static ec_slave_config_state_t slave_0_state = {};
static ec_slave_config_t *slave_1;
static ec_slave_config_state_t slave_1_state = {};
typedef struct shared_use_st
{
uint8_t slave0_stat;
uint8_t slave1_stat;
}shared_use_info;
int shmid ;
shared_use_info shared_use;
static uint8_t *domain0_pd = NULL;
static uint8_t *domain1_pd = NULL;
#define AX58100_0 0, 0
#define S0_VID_PID 0x00000009, 0x26483052
#define AX58100_1 0, 1
#define S1_VID_PID 0x00000009, 0x26483052
unsigned int off_bytes_board0_led1_val;
unsigned int off_bytes_board0_led2_val;
unsigned int off_bytes_board0_led3_val;
unsigned int off_bytes_board0_led4_val;
unsigned int off_bytes_board0_led5_val;
unsigned int off_bytes_board0_led6_val;
unsigned int off_bytes_board0_led7_val;
unsigned int off_bytes_board0_led8_val;
unsigned int off_bytes_board0_led9_val;
unsigned int off_bytes_board0_led10_val;
unsigned int off_bytes_board0_led11_val;
unsigned int off_bytes_board0_led12_val;
unsigned int off_bytes_board0_led13_val;
unsigned int off_bytes_board0_led14_val;
unsigned int off_bytes_board0_led15_val;
unsigned int off_bytes_board0_led16_val;
unsigned int off_bytes_board0_switch1_status;
unsigned int off_bytes_board0_switch2_status;
unsigned int off_bytes_board0_switch3_status;
unsigned int off_bytes_board0_switch4_status;
unsigned int off_bytes_board0_switch5_status;
unsigned int off_bytes_board0_switch6_status;
unsigned int off_bytes_board0_switch7_status;
unsigned int off_bytes_board0_switch8_status;
unsigned int off_bits_board0_led1_val;
unsigned int off_bits_board0_led2_val;
unsigned int off_bits_board0_led3_val;
unsigned int off_bits_board0_led4_val;
unsigned int off_bits_board0_led5_val;
unsigned int off_bits_board0_led6_val;
unsigned int off_bits_board0_led7_val;
unsigned int off_bits_board0_led8_val;
unsigned int off_bits_board0_led9_val;
unsigned int off_bits_board0_led10_val;
unsigned int off_bits_board0_led11_val;
unsigned int off_bits_board0_led12_val;
unsigned int off_bits_board0_led13_val;
unsigned int off_bits_board0_led14_val;
unsigned int off_bits_board0_led15_val;
unsigned int off_bits_board0_led16_val;
unsigned int off_bits_board0_switch1_status;
unsigned int off_bits_board0_switch2_status;
unsigned int off_bits_board0_switch3_status;
unsigned int off_bits_board0_switch4_status;
unsigned int off_bits_board0_switch5_status;
unsigned int off_bits_board0_switch6_status;
unsigned int off_bits_board0_switch7_status;
unsigned int off_bits_board0_switch8_status;
unsigned int off_bytes_board1_led1_val;
unsigned int off_bytes_board1_led2_val;
unsigned int off_bytes_board1_led3_val;
unsigned int off_bytes_board1_led4_val;
unsigned int off_bytes_board1_led5_val;
unsigned int off_bytes_board1_led6_val;
unsigned int off_bytes_board1_led7_val;
unsigned int off_bytes_board1_led8_val;
unsigned int off_bytes_board1_led9_val;
unsigned int off_bytes_board1_led10_val;
unsigned int off_bytes_board1_led11_val;
unsigned int off_bytes_board1_led12_val;
unsigned int off_bytes_board1_led13_val;
unsigned int off_bytes_board1_led14_val;
unsigned int off_bytes_board1_led15_val;
unsigned int off_bytes_board1_led16_val;
unsigned int off_bytes_board1_switch1_status;
unsigned int off_bytes_board1_switch2_status;
unsigned int off_bytes_board1_switch3_status;
unsigned int off_bytes_board1_switch4_status;
unsigned int off_bytes_board1_switch5_status;
unsigned int off_bytes_board1_switch6_status;
unsigned int off_bytes_board1_switch7_status;
unsigned int off_bytes_board1_switch8_status;
unsigned int off_bits_board1_led1_val;
unsigned int off_bits_board1_led2_val;
unsigned int off_bits_board1_led3_val;
unsigned int off_bits_board1_led4_val;
unsigned int off_bits_board1_led5_val;
unsigned int off_bits_board1_led6_val;
unsigned int off_bits_board1_led7_val;
unsigned int off_bits_board1_led8_val;
unsigned int off_bits_board1_led9_val;
unsigned int off_bits_board1_led10_val;
unsigned int off_bits_board1_led11_val;
unsigned int off_bits_board1_led12_val;
unsigned int off_bits_board1_led13_val;
unsigned int off_bits_board1_led14_val;
unsigned int off_bits_board1_led15_val;
unsigned int off_bits_board1_led16_val;
unsigned int off_bits_board1_switch1_status;
unsigned int off_bits_board1_switch2_status;
unsigned int off_bits_board1_switch3_status;
unsigned int off_bits_board1_switch4_status;
unsigned int off_bits_board1_switch5_status;
unsigned int off_bits_board1_switch6_status;
unsigned int off_bits_board1_switch7_status;
unsigned int off_bits_board1_switch8_status;
const static ec_pdo_entry_reg_t domain0_regs[] = {
{AX58100_0, S0_VID_PID, 0x7010, 0x01, &off_bytes_board0_led1_val,&off_bits_board0_led1_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x02, &off_bytes_board0_led2_val,&off_bits_board0_led2_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x03, &off_bytes_board0_led3_val,&off_bits_board0_led3_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x04, &off_bytes_board0_led4_val,&off_bits_board0_led4_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x05, &off_bytes_board0_led5_val,&off_bits_board0_led5_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x06, &off_bytes_board0_led6_val,&off_bits_board0_led6_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x07, &off_bytes_board0_led7_val,&off_bits_board0_led7_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x08, &off_bytes_board0_led8_val,&off_bits_board0_led8_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x09, &off_bytes_board0_led9_val,&off_bits_board0_led9_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x10, &off_bytes_board0_led10_val,&off_bits_board0_led10_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x11, &off_bytes_board0_led11_val,&off_bits_board0_led11_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x12, &off_bytes_board0_led12_val,&off_bits_board0_led12_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x13, &off_bytes_board0_led13_val,&off_bits_board0_led13_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x14, &off_bytes_board0_led14_val,&off_bits_board0_led14_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x15, &off_bytes_board0_led15_val,&off_bits_board0_led15_val},
{AX58100_0, S0_VID_PID, 0x7010, 0x16, &off_bytes_board0_led16_val,&off_bits_board0_led16_val},
{AX58100_0, S0_VID_PID, 0x6000, 0x01, &off_bytes_board0_switch1_status,&off_bits_board0_switch1_status},
{AX58100_0, S0_VID_PID, 0x6000, 0x02, &off_bytes_board0_switch2_status,&off_bits_board0_switch2_status},
{AX58100_0, S0_VID_PID, 0x6000, 0x03, &off_bytes_board0_switch3_status,&off_bits_board0_switch3_status},
{AX58100_0, S0_VID_PID, 0x6000, 0x04, &off_bytes_board0_switch4_status,&off_bits_board0_switch4_status},
{AX58100_0, S0_VID_PID, 0x6000, 0x05, &off_bytes_board0_switch5_status,&off_bits_board0_switch5_status},
{AX58100_0, S0_VID_PID, 0x6000, 0x06, &off_bytes_board0_switch6_status,&off_bits_board0_switch6_status},
{AX58100_0, S0_VID_PID, 0x6000, 0x07, &off_bytes_board0_switch7_status,&off_bits_board0_switch7_status},
{AX58100_0, S0_VID_PID, 0x6000, 0x08, &off_bytes_board0_switch8_status,&off_bits_board0_switch8_status},
{}
};
const static ec_pdo_entry_reg_t domain1_regs[] = {
{AX58100_1, S1_VID_PID, 0x7010, 0x01, &off_bytes_board1_led1_val, &off_bits_board1_led1_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x02, &off_bytes_board1_led2_val, &off_bits_board1_led2_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x03, &off_bytes_board1_led3_val, &off_bits_board1_led3_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x04, &off_bytes_board1_led4_val, &off_bits_board1_led4_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x05, &off_bytes_board1_led5_val, &off_bits_board1_led5_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x06, &off_bytes_board1_led6_val, &off_bits_board1_led6_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x07, &off_bytes_board1_led7_val, &off_bits_board1_led7_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x08, &off_bytes_board1_led8_val, &off_bits_board1_led8_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x09, &off_bytes_board1_led9_val, &off_bits_board1_led9_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x10, &off_bytes_board1_led10_val,&off_bits_board1_led10_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x11, &off_bytes_board1_led11_val,&off_bits_board1_led11_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x12, &off_bytes_board1_led12_val,&off_bits_board1_led12_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x13, &off_bytes_board1_led13_val,&off_bits_board1_led13_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x14, &off_bytes_board1_led14_val,&off_bits_board1_led14_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x15, &off_bytes_board1_led15_val,&off_bits_board1_led15_val},
{AX58100_1, S1_VID_PID, 0x7010, 0x16, &off_bytes_board1_led16_val,&off_bits_board1_led16_val},
{AX58100_1, S1_VID_PID, 0x6000, 0x01, &off_bytes_board1_switch1_status, &off_bits_board1_switch1_status},
{AX58100_1, S1_VID_PID, 0x6000, 0x02, &off_bytes_board1_switch2_status, &off_bits_board1_switch2_status},
{AX58100_1, S1_VID_PID, 0x6000, 0x03, &off_bytes_board1_switch3_status, &off_bits_board1_switch3_status},
{AX58100_1, S1_VID_PID, 0x6000, 0x04, &off_bytes_board1_switch4_status, &off_bits_board1_switch4_status},
{AX58100_1, S1_VID_PID, 0x6000, 0x05, &off_bytes_board1_switch5_status, &off_bits_board1_switch5_status},
{AX58100_1, S1_VID_PID, 0x6000, 0x06, &off_bytes_board1_switch6_status, &off_bits_board1_switch6_status},
{AX58100_1, S1_VID_PID, 0x6000, 0x07, &off_bytes_board1_switch7_status, &off_bits_board1_switch7_status},
{AX58100_1, S1_VID_PID, 0x6000, 0x08, &off_bytes_board1_switch8_status, &off_bits_board1_switch8_status},
{}
};
/***************************************************************************/
/*Config PDOs*/
static ec_pdo_entry_info_t slave_0_pdo_entries[] = {
//input (write)
{0x7010, 0x01, 16},
{0x7010, 0x02, 16},
{0x7010, 0x03, 16},
{0x7010, 0x04, 16},
{0x7010, 0x05, 16},
{0x7010, 0x06, 16},
{0x7010, 0x07, 16},
{0x7010, 0x08, 16},
{0x7010, 0x09, 16},
{0x7010, 0x10, 16},
{0x7010, 0x11, 16},
{0x7010, 0x12, 16},
{0x7010, 0x13, 16},
{0x7010, 0x14, 16},
{0x7010, 0x15, 16},
{0x7010, 0x16, 16},
//output (read)
{0x6000, 0x01, 8}, /* Switch 1 0x7e*/
{0x6000, 0x02, 8}, /* Switch 2 0x81 0x83 0x85 0x87 0x89*/
{0x6000, 0x03, 8}, /* Switch 3 正确0 错误1 位置1(00-07) */
{0x6000, 0x04, 8}, /* Switch 4 0x7e 位置2(08-15) */
{0x6000, 0x05, 8}, /* Switch 5 位置3(16-23) */
{0x6000, 0x06, 8}, /* Switch 6 位置4(24-31) */
{0x6000, 0x07, 8}, /* Switch 7 正确0 错误1 */
{0x6000, 0x08, 8}, /* Switch 8 0x7e*/
};
static ec_pdo_info_t slave_0_pdos[] = {
{0x1601, 16, slave_0_pdo_entries + 0}, /* DO RxPDO-Map */
{0x1a00, 8, slave_0_pdo_entries + 16}, /* DI TxPDO-Map */
};
static ec_sync_info_t slave_0_syncs[] = {
{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
{1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
{2, EC_DIR_OUTPUT, 1, slave_0_pdos + 0, EC_WD_ENABLE},
{3, EC_DIR_INPUT, 1, slave_0_pdos + 1, EC_WD_DISABLE},
{0xff}
};
/**************************************************************************/
/*Config PDOs*/
static ec_pdo_entry_info_t slave_1_pdo_entries[] = {
//input (write)
{0x7010, 0x01, 16}, /* LED 1 0x7e 标志*/
{0x7010, 0x02, 16}, /* LED 2 功能选择 0x80 0x82 0x84 0x86 0x88 */
{0x7010, 0x03, 16}, /* LED 3 位置低16 阶段加速1低16 */
{0x7010, 0x04, 16}, /* LED 4 位置高16 阶段加速1高16 */
{0x7010, 0x05, 16}, /* LED 5 速度低16 阶段加速2低16 */
{0x7010, 0x06, 16}, /* LED 6 速度高16 阶段加速2高16 */
{0x7010, 0x07, 16}, /* LED 7 加速度低16 阶段减速1低16 */
{0x7010, 0x08, 16}, /* LED 8 加速度高16 阶段减速1高16 */
{0x7010, 0x09, 16}, /* LED 9 减速度低16 阶段减速2低16 */
{0x7010, 0x10, 16}, /* LED 16 减速度高16 阶段减速2高16 */
{0x7010, 0x11, 16},
{0x7010, 0x12, 16},
{0x7010, 0x13, 16},
{0x7010, 0x14, 16},
{0x7010, 0x15, 16},
{0x7010, 0x16, 16},
//output (read)
{0x6000, 0x01, 8}, /* Switch 1 0x7e*/
{0x6000, 0x02, 8}, /* Switch 2 0x81 0x83 0x85 0x87 0x89*/
{0x6000, 0x03, 8}, /* Switch 3 正确0 错误1 位置1(00-07) */
{0x6000, 0x04, 8}, /* Switch 4 0x7e 位置2(08-15) */
{0x6000, 0x05, 8}, /* Switch 5 位置3(16-23) */
{0x6000, 0x06, 8}, /* Switch 6 位置4(24-31) */
{0x6000, 0x07, 8}, /* Switch 7 正确0 错误1 */
{0x6000, 0x08, 8}, /* Switch 8 0x7e*/
};
static ec_pdo_info_t slave_1_pdos[] = {
{0x1601, 16, slave_1_pdo_entries + 0}, /* DO RxPDO-Map */
{0x1a00, 8, slave_1_pdo_entries + 16}, /* DI TxPDO-Map */
};
static ec_sync_info_t slave_1_syncs[] = {
{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
{1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
{2, EC_DIR_OUTPUT, 1, slave_1_pdos + 0, EC_WD_ENABLE},
{3, EC_DIR_INPUT, 1, slave_1_pdos + 1, EC_WD_DISABLE},
{0xff}
};
/*************************************************************************/
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 check_domain0_state(void)
{
ec_domain_state_t ds;
ecrt_domain_state(domain0, &ds);
if (ds.working_counter != domain0_state.working_counter)
printf("Domain0: WC %u.\n", ds.working_counter);
if (ds.wc_state != domain0_state.wc_state)
printf("Domain0: State %u.\n", ds.wc_state);
domain0_state = ds;
}
void check_domain1_state(void)
{
ec_domain_state_t ds;
ecrt_domain_state(domain1, &ds);
if (ds.working_counter != domain1_state.working_counter)
printf("Domain1: WC %u.\n", ds.working_counter);
if (ds.wc_state != domain1_state.wc_state)
printf("Domain1: State %u.\n", ds.wc_state);
domain1_state = ds;
}
uint32_t check_slave0_config_states(void)
{
ec_slave_config_state_t s;
ecrt_slave_config_state(slave_0, &s);
if (s.al_state != slave_0_state.al_state)
printf("slave0: State 0x%02X.\n", s.al_state);
if (s.online != slave_0_state.online)
printf("slave0: %s.\n", s.online ? "online" : "offline");
if (s.operational != slave_0_state.operational)
printf("slave0: %soperational.\n\n\n", s.operational ? "" : "Not ");
slave_0_state = s;
return s.al_state;
}
uint32_t check_slave1_config_states(void)
{
ec_slave_config_state_t s;
ecrt_slave_config_state(slave_1, &s);
if (s.al_state != slave_1_state.al_state)
printf("slave1: State 0x%02X.\n", s.al_state);
if (s.online != slave_1_state.online)
printf("slave1: %s.\n", s.online ? "online" : "offline");
if (s.operational != slave_1_state.operational)
printf("slave1: %soperational.\n\n\n", s.operational ? "" : "Not ");
slave_1_state = s;
return s.al_state;
}
/*******************************************************************************/
void *cyclic_task0_thread(void *arg)
{
struct timespec next_period;
int cycle_counter = 0;
uint32_t status;
uint16_t pos1=0,pos2=0;
uint8_t step=0;
uint8_t status2;
uint8_t status3;
uint8_t status4;
uint8_t status5;
uint8_t status6;
uint8_t status7;
char *tip;
void *shm = NULL;
shared_use_info *slave_stat;
struct sched_param param = { .sched_priority = 83 };
pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m);
clock_gettime(CLOCK_REALTIME, &next_period);
shm = shmat(shmid, NULL, 0);
shm = shmat(shmid, 0, 0);
if (shm == (void *)-1) {
fprintf(stderr, "shmat failed\n");
exit(EXIT_FAILURE);
}
slave_stat = (shared_use_info *)shm;
slave_stat->slave0_stat=0;
printf("slave 1 time = %ld s\n", next_period.tv_sec);
printf("slave 1 time = %ld ns\n", next_period.tv_nsec);
while (run) {
next_period.tv_nsec += cycle0_us * 1000;
while (next_period.tv_nsec >= USEC_PER_SEC) {
next_period.tv_nsec -= USEC_PER_SEC;
next_period.tv_sec++;
}
clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &next_period, NULL);
cycle_counter++;
ecrt_master_receive(master);
ecrt_domain_process(domain0);
if (!(cycle_counter % 10)) {
check_domain0_state();
check_master_state();
}
//Check for slave configuration state(s)
status=check_slave0_config_states();
if(status != 0x8) {
ecrt_domain_queue(domain0);
ecrt_master_send(master);
slave_stat->slave0_stat=0;
continue;
}
else {
slave_stat->slave0_stat=1;
}
if(!(slave_stat->slave1_stat))
continue;
if(step == 0) {
pos2+=7;
EC_WRITE_U16(domain0_pd + off_bytes_board0_led1_val, 0x007e );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led2_val, 0x0080 );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led3_val, pos1 );//位置
EC_WRITE_U16(domain0_pd + off_bytes_board0_led4_val, pos2 );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led5_val, 0xff00 );//速度
EC_WRITE_U16(domain0_pd + off_bytes_board0_led6_val, 0x0fff );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led7_val, 0x0a00 );//加速度
EC_WRITE_U16(domain0_pd + off_bytes_board0_led8_val, 0x0000 );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led9_val, 0x00a0 );//减速度
EC_WRITE_U16(domain0_pd + off_bytes_board0_led10_val, 0x0000 );
step++;
printf("slave 0 time = %ld s\n", next_period.tv_sec);
printf("slave 0 time = %ld ns\n", next_period.tv_nsec);
}
else if(step == 1) {
EC_READ_U8(domain0_pd + off_bytes_board0_switch2_status);
EC_READ_U8(domain0_pd + off_bytes_board0_switch3_status);
step++;
}
else if(step == 2) {
EC_WRITE_U16(domain0_pd + off_bytes_board0_led1_val, 0x007e );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led2_val, 0x0082 );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led3_val, 0x0500 );//阶段加速1
EC_WRITE_U16(domain0_pd + off_bytes_board0_led4_val, 0x0000 );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led5_val, 0x0500 );//阶段加速2
EC_WRITE_U16(domain0_pd + off_bytes_board0_led6_val, 0x0000 );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led7_val, 0x0050 );//阶段减速1
EC_WRITE_U16(domain0_pd + off_bytes_board0_led8_val, 0x0000 );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led9_val, 0x0050 );//阶段减速2
EC_WRITE_U16(domain0_pd + off_bytes_board0_led10_val, 0x0000 );
step++;
}
else if(step == 3) {
EC_READ_U8(domain0_pd + off_bytes_board0_switch2_status);
EC_READ_U8(domain0_pd + off_bytes_board0_switch3_status);
step++;
}
else if(step == 4) {
EC_WRITE_U16(domain0_pd + off_bytes_board0_led1_val, 0x007e );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led2_val, 0x0084 );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led3_val, 0x007e );
step++;
}
else if(step == 5) {
EC_READ_U8(domain0_pd + off_bytes_board0_switch2_status);
EC_READ_U8(domain0_pd + off_bytes_board0_switch7_status);
step++;
}
else if(step == 6) {
EC_WRITE_U16(domain0_pd + off_bytes_board0_led1_val, 0x007e );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led2_val, 0x0086 );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led3_val, 0x007e );
step++;
}
else if(step == 7) {
EC_READ_U8(domain0_pd + off_bytes_board0_switch2_status);
EC_READ_U8(domain0_pd + off_bytes_board0_switch7_status);
step++;
}
else if(step == 8) {
EC_WRITE_U16(domain0_pd + off_bytes_board0_led1_val, 0x007e );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led2_val, 0x0088 );
EC_WRITE_U16(domain0_pd + off_bytes_board0_led3_val, 0x007e );
step++;
}
else if(step == 9) {
EC_READ_U8(domain0_pd + off_bytes_board0_switch2_status);
EC_READ_U8(domain0_pd + off_bytes_board0_switch7_status);
step++;
}
else {
step++;
}
if (step <= 10) {
ecrt_domain_queue(domain0);
ecrt_master_send(master);
}
if( (step == 3) || (step == 5) ) {
status2 = EC_READ_U8(domain0_pd + off_bytes_board0_switch2_status);
status3 = EC_READ_U8(domain0_pd + off_bytes_board0_switch3_status);
}
else if( (step == 7) || (step == 9) || (step == 11) ) {
status2 = EC_READ_U8(domain0_pd + off_bytes_board0_switch2_status);
status7 = EC_READ_U8(domain0_pd + off_bytes_board0_switch7_status);
}
if( (step == 3) ||(step == 5) ||(step == 7) ||(step == 9) ||(step == 11) ) {
printf("step %d board0 State 0x%x :",step, status2);
switch (status2) {
case 0x81: tip = status3 ? "fail" : "sucess" ; break;
case 0x83: tip = status3 ? "fail" : "sucess" ; break;
case 0x85: tip = status7 ? "fail" : "sucess" ; break;
case 0x87: tip = status7 ? "fail" : "sucess" ; break;
case 0x89: tip = status7 ? "fail" : "sucess" ; break;
default : tip = "fail";
}
printf("%s\n", tip);
}
if(step == 11){
step=0;
//return NULL;
}
}
return NULL;
}
void *cyclic_task1_thread(void *arg)
{
struct timespec next_period;
int cycle_counter = 0;
uint32_t status;
uint16_t pos1=0xffff,pos2=0xffff;
uint8_t step=0;
uint8_t status2;
uint8_t status3;
uint8_t status4;
uint8_t status5;
uint8_t status6;
uint8_t status7;
char *tip;
void *shm = NULL;
shared_use_info *slave_stat;
struct sched_param param = { .sched_priority = 82 };//数值越大，对应的优先级越大
pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m);
clock_gettime(CLOCK_REALTIME, &next_period);
shm = shmat(shmid, NULL, 0);
shm = shmat(shmid, 0, 0);
if (shm == (void *)-1) {
fprintf(stderr, "shmat failed\n");
exit(EXIT_FAILURE);
}
slave_stat = (shared_use_info *)shm;
slave_stat->slave1_stat=0;
printf("slave 1 time = %ld s\n", next_period.tv_sec);
printf("slave 1 time = %ld ns\n", next_period.tv_nsec);
while (run) {
next_period.tv_nsec += cycle1_us * 1000;
while (next_period.tv_nsec >= USEC_PER_SEC) {
next_period.tv_nsec -= USEC_PER_SEC;
next_period.tv_sec++;
}
clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &next_period, NULL);
cycle_counter++;
ecrt_master_receive(master);
ecrt_domain_process(domain1);
if (!(cycle_counter % 10)) {
check_domain1_state();
check_master_state();
}
//Check for slave configuration state(s)
status=check_slave1_config_states();
if(status != 0x8) {
ecrt_domain_queue(domain1);
ecrt_master_send(master);
slave_stat->slave1_stat=0;
continue;
}
else {
slave_stat->slave1_stat=1;
}
if(!(slave_stat->slave0_stat))
continue;
if(step == 0) {
pos2-=7;
EC_WRITE_U16(domain1_pd + off_bytes_board1_led1_val, 0x007e );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led2_val, 0x0080 );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led3_val, pos1 );//位置
EC_WRITE_U16(domain1_pd + off_bytes_board1_led4_val, pos2 );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led5_val, 0xff00 );//速度
EC_WRITE_U16(domain1_pd + off_bytes_board1_led6_val, 0x0fff );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led7_val, 0x0a00 );//加速度
EC_WRITE_U16(domain1_pd + off_bytes_board1_led8_val, 0x0000 );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led9_val, 0x00a0 );//减速度
EC_WRITE_U16(domain1_pd + off_bytes_board1_led10_val, 0x0000 );
step++;
printf("slave 1 time = %ld s\n", next_period.tv_sec);
printf("slave 1 time = %ld ns\n", next_period.tv_nsec);
}
else if(step == 1) {
EC_READ_U8(domain1_pd + off_bytes_board1_switch2_status);
EC_READ_U8(domain1_pd + off_bytes_board1_switch3_status);
step++;
}
else if(step == 2) {
EC_WRITE_U16(domain1_pd + off_bytes_board1_led1_val, 0x007e );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led2_val, 0x0082 );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led3_val, 0x0500 );//阶段加速1
EC_WRITE_U16(domain1_pd + off_bytes_board1_led4_val, 0x0000 );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led5_val, 0x0500 );//阶段加速2
EC_WRITE_U16(domain1_pd + off_bytes_board1_led6_val, 0x0000 );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led7_val, 0x0050 );//阶段减速1
EC_WRITE_U16(domain1_pd + off_bytes_board1_led8_val, 0x0000 );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led9_val, 0x0050 );//阶段减速2
EC_WRITE_U16(domain1_pd + off_bytes_board1_led10_val, 0x0000 );
step++;
}
else if(step == 3) {
EC_READ_U8(domain1_pd + off_bytes_board1_switch2_status);
EC_READ_U8(domain1_pd + off_bytes_board1_switch3_status);
step++;
}
else if(step == 4) {
EC_WRITE_U16(domain1_pd + off_bytes_board1_led1_val, 0x007e );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led2_val, 0x0084 );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led3_val, 0x007e );
step++;
}
else if(step == 5) {
EC_READ_U8(domain1_pd + off_bytes_board1_switch2_status);
EC_READ_U8(domain1_pd + off_bytes_board1_switch7_status);
step++;
}
else if(step == 6) {
EC_WRITE_U16(domain1_pd + off_bytes_board1_led1_val, 0x007e );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led2_val, 0x0086 );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led3_val, 0x007e );
step++;
}
else if(step == 7) {
EC_READ_U8(domain1_pd + off_bytes_board1_switch2_status);
EC_READ_U8(domain1_pd + off_bytes_board1_switch7_status);
step++;
}
else if(step == 8) {
EC_WRITE_U16(domain1_pd + off_bytes_board1_led1_val, 0x007e );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led2_val, 0x0088 );
EC_WRITE_U16(domain1_pd + off_bytes_board1_led3_val, 0x007e );
step++;
}
else if(step == 9) {
EC_READ_U8(domain1_pd + off_bytes_board1_switch2_status);
EC_READ_U8(domain1_pd + off_bytes_board1_switch7_status);
step++;
}
else {
step++;
}
if (step <= 10) {
ecrt_domain_queue(domain1);
ecrt_master_send(master);
}
if( (step == 3) || (step == 5) ) {
status2 = EC_READ_U8(domain1_pd + off_bytes_board1_switch2_status);
status3 = EC_READ_U8(domain1_pd + off_bytes_board1_switch3_status);
}
else if( (step == 7) || (step == 9) || (step == 11) ) {
status2 = EC_READ_U8(domain1_pd + off_bytes_board1_switch2_status);
status7 = EC_READ_U8(domain1_pd + off_bytes_board1_switch7_status);
}
if( (step == 3) ||(step == 5) ||(step == 7) ||(step == 9) ||(step == 11) ) {
printf("step %d board1 State 0x%x :",step, status2);
switch (status2) {
case 0x81: tip = status3 ? "fail" : "sucess" ; break;
case 0x83: tip = status3 ? "fail" : "sucess" ; break;
case 0x85: tip = status7 ? "fail" : "sucess" ; break;
case 0x87: tip = status7 ? "fail" : "sucess" ; break;
case 0x89: tip = status7 ? "fail" : "sucess" ; break;
default : tip = "fail";
}
printf("%s\n", tip);
}
if(step == 11){
step=0;
// if (shmdt(shm) == -1) {
// fprintf(stderr, "shmdt failed\n");
// exit(EXIT_FAILURE);
// }
//return NULL;
}
}
if (shmdt(shm) == -1) {
fprintf(stderr, "shmdt failed\n");
exit(EXIT_FAILURE);
}
return NULL;
}
void signal_handler(int sig)
{
printf("exit program...\n");
run = 0;
}
/****************************************************************************/
int main(int argc, char **argv)
{
int ret;
pthread_attr_t thattr;
signal(SIGTERM, signal_handler);
signal(SIGINT, signal_handler);
ret = mlockall(MCL_CURRENT | MCL_FUTURE);
if(ret)
printf("mlockall failure\n");
shmid = shmget(IPC_PRIVATE, sizeof(shared_use_info), IPC_CREAT|0600 ) ;
if ( shmid < 0 ) {
printf("get shm ipc_id error") ;
return -1 ;
}
master = ecrt_request_master(0);
if (!master) {
exit(EXIT_FAILURE);
}
domain0 = ecrt_master_create_domain(master);
if (!domain0) {
exit(EXIT_FAILURE);
}
domain1 = ecrt_master_create_domain(master);
if (!domain1) {
exit(EXIT_FAILURE);
}
printf("Creating slave configurations...\n");
if (!(slave_0 = ecrt_master_slave_config(master, AX58100_0, S0_VID_PID))) {
fprintf(stderr, "Failed to get slave configuration for slave0!\n");
exit(EXIT_FAILURE);
}
if (!(slave_1 = ecrt_master_slave_config(master, AX58100_1, S1_VID_PID))) {
fprintf(stderr, "Failed to get slave configuration for slave1!\n");
exit(EXIT_FAILURE);
}
// ecrt_slave_config_overlapping_pdos(slave_0, 1);
// ecrt_slave_config_overlapping_pdos(slave_1, 1);
printf("Configuring PDOs...\n");
if (ecrt_slave_config_pdos(slave_0, EC_END, slave_0_syncs)) {
fprintf(stderr, "Failed to configure slave0 PDOs!\n");
exit(EXIT_FAILURE);
}
if (ecrt_slave_config_pdos(slave_1, EC_END, slave_1_syncs)) {
fprintf(stderr, "Failed to configure slave1 PDOs!\n");
exit(EXIT_FAILURE);
}
if (ecrt_domain_reg_pdo_entry_list(domain0, domain0_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
exit(EXIT_FAILURE);
}
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
exit(EXIT_FAILURE);
}
printf("Activating master...\n");
if (ecrt_master_activate(master)) {
exit(EXIT_FAILURE);
}
else {
printf("*Master activated*\n");
}
if (!(domain0_pd = ecrt_domain_data(domain0))) {
exit(EXIT_FAILURE);
}
if (!(domain1_pd = ecrt_domain_data(domain1))) {
exit(EXIT_FAILURE);
}
pthread_attr_init(&thattr);
pthread_attr_setschedpolicy(&thattr, SCHED_FIFO);
ret = pthread_create(&cyclic_thread0, &thattr, &cyclic_task0_thread, NULL);
if (ret) {
printf("%s: pthread_create cyclic task0 failed\n", strerror(-ret));
return 1;
}
ret = pthread_create(&cyclic_thread1, &thattr, &cyclic_task1_thread, NULL);
if (ret) {
printf("%s: pthread_create cyclic task1 failed\n", strerror(-ret));
return 1;
}
pthread_join(cyclic_thread0, NULL);
pthread_join(cyclic_thread1, NULL);
pthread_attr_destroy(&thattr);
ecrt_release_master(master);
if (shmctl(shmid, IPC_RMID, 0) == -1) {
fprintf(stderr, "shmctl(IPC_RMID) failed\n");
exit(EXIT_FAILURE);
}
printf("End of Program\n");
return EXIT_SUCCESS;
}
