贴出完整代码,适合新手入门学习demo
cmake_minimum_required(VERSION 3.27)
project(demo)
set(CMAKE_CXX_STANDARD 17)
find_package(EtherCAT REQUIRED)
add_executable(demo main.cpp)
target_link_libraries(demo PUBLIC EtherLab::ethercat)
#include <iostream>
#include <algorithm>
#include <csignal>
#include "ecrt.h"
using namespace std;
/****************************************************************************/
/** 任务周期(ns)。 */
#define PERIOD_NS (1000000) // 1ms
/****************************************************************************/
/****************************************************************************/
/* 常量 */
#define NSEC_PER_SEC (1000000000)
/****************************************************************************/
/****************************************************************************/
// EtherCAT
static ec_master_t *master = nullptr;
static ec_master_state_t master_state = {};
static ec_domain_t *domain1 = nullptr;
static ec_domain_state_t domain1_state = {};
static ec_slave_config_t *sc;
static ec_slave_config_state_t sc_state = {};
/****************************************************************************/
/****************************************************************************/
/*处理数据*/
static uint8_t *domain1_pd = nullptr;
/*总线耦合器位置*/
#define BusCouplerPos 0, 0
/*从站位置*/
#define Detail_ASD_A2_E_1_Pos 0x000001dd,0x10305070 /*Vendor ID, Product code*/
/****************************************************************************/
/****************************************************************************/
/*PDO 条目的偏移量*/
static struct {
unsigned int operation_mode;
unsigned int ctrl_word;
unsigned int target_velocity;
unsigned int status_word;
unsigned int current_velocity;
unsigned int current_position;
} offset;
const static ec_pdo_entry_reg_t domain1_regs[] = {
{BusCouplerPos, Detail_ASD_A2_E_1_Pos, 0x6040, 0, &offset.ctrl_word},
{BusCouplerPos, Detail_ASD_A2_E_1_Pos, 0x6041, 0, &offset.status_word},
{BusCouplerPos, Detail_ASD_A2_E_1_Pos, 0x6064, 0, &offset.current_position},
{BusCouplerPos, Detail_ASD_A2_E_1_Pos, 0x607A, 0, &offset.target_velocity},
{}
};
/****************************************************************************/
/****************************************************************************/
/*Config PDOs*/
static ec_pdo_entry_info_t device_pdo_entries[] = {
/*RxPdo 0x1600*/
{0x6040, 0x00, 16}, /*Controlword*/
{0x607A, 0x00, 32}, /*TargetPosition*/
/*TxPdo 0x1A00*/
{0x6041, 0x00, 16}, /*Statusword*/
{0x6064, 0x00, 32}, /*ActualPosition*/
};
static ec_pdo_info_t device_pdos[] = {
//RxPdo
{0x1601, 2, device_pdo_entries + 0},
//TxPdo
{0x1A01, 2, device_pdo_entries + 2}
};
static ec_sync_info_t device_syncs[] = {
{0, EC_DIR_OUTPUT, 0, nullptr, EC_WD_DISABLE},
{1, EC_DIR_INPUT, 0, nullptr, EC_WD_DISABLE},
{2, EC_DIR_OUTPUT, 1, device_pdos + 0, EC_WD_DISABLE},
{3, EC_DIR_INPUT, 1, device_pdos + 1, EC_WD_DISABLE},
{0xFF}
};
/****************************************************************************/
void check_domain1_state() {
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;
}
void check_master_state() {
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() {
ec_slave_config_state_t s;
ecrt_slave_config_state(sc, &s);
if (s.al_state != sc_state.al_state) {
printf("slave: State 0x%02X.\n", s.al_state);
}
if (s.online != sc_state.online) {
printf("slave: %s.\n", s.online ? "online" : "offline");
}
if (s.operational != sc_state.operational) {
printf("slave: %soperational.\n", s.operational ? "" : "Not ");
}
sc_state = s;
}
template<typename type>
void sdoWrite(uint16_t slave, uint16_t index, uint8_t subIndex, const type value) {
uint32_t abort_code;
if (ecrt_master_sdo_download(master, slave, index, subIndex, (uint8_t *) &value, sizeof(type), &abort_code))
cout << "ERROR: ecrt_master_sdo_download: " << abort_code << endl;
}
template<typename type>
type sdoRead(uint16_t slave, uint16_t index, uint8_t subIndex) {
type value;
size_t result_size;
uint32_t abort_code;
if (ecrt_master_sdo_upload(master, slave, index, subIndex, (uint8_t *) &value, sizeof(type), &result_size,
&abort_code) < 0) {
cout << "ERROR: ecrt_master_sdo_upload: " << abort_code << endl;
// 如果出现错误,可能需要返回一个默认值或者采取其他适当的错误处理措施
}
return value;
}
template<>
// 特化模板
std::string sdoRead<string>(uint16_t slave, uint16_t index, uint8_t subIndex) {
const size_t maxStringLength = 256;
uint8_t data[maxStringLength]; // 缓冲区用于存储字符串数据
size_t resultSize;
uint32_t abortCode;
// 读取字符串数据
if (ecrt_master_sdo_upload(master, slave, index, subIndex, data, maxStringLength, &resultSize, &abortCode) < 0) {
std::cerr << "ERROR: ecrt_master_sdo_upload: " << abortCode << std::endl;
}
// 将字节流转换为字符串
std::string str(data, data + resultSize);
// 替换换行符为空字符
str.erase(std::remove_if(str.begin(), str.end(), [](char c) { return c == '\0'; }), str.end());
return str;
}
void cyclic_task() {
/*接收过程数据*/
ecrt_master_receive(master);
ecrt_domain_process(domain1);
/*检查过程数据状态(可选)*/
check_domain1_state();
//检查主状态
check_master_state();
//检查从站配置状态
check_slave_config_states();
/*Read Position*/
//position = EC_READ_U32(domain1_pd + offset.current_position);
//fprintf(stderr, "Position: %d\n", position);
static uint16_t command = 0x004F;//用来帮助判断状态字的值
int position;
int status;
status = EC_READ_U16(domain1_pd + offset.status_word);
//EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x06);
//EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x07);
//EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x0f);
// 延时1ms
// cout << hex << (status & 0x4f) << endl;
if ((status & 0x4F) == 0x40) {
EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x06);
//printf("Status: 0x40\n");
command = 0x006F;
} else if ((status & 0x6F) == 0x21) {
EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x07);
//printf("Status: 0x21\n");
command = 0x006F;
} else if ((status & 0x6F) == 0x23) {
EC_WRITE_U16(domain1_pd + offset.ctrl_word, 0x0f);
//printf("Status: 0x23\n");
command = 0x006F;
} else if((status & command) == 0x27){
position = EC_READ_S32(domain1_pd + offset.current_position);
position += 25600;
EC_WRITE_S32(domain1_pd + offset.target_velocity, position);
}
// 发送PDO
/*发送过程数据*/
ecrt_domain_queue(domain1);
ecrt_master_send(master);
}
struct timespec calculate_next_wakeup(const struct timespec ¤t_time, long period_ms) {
struct timespec next_wakeup = current_time;
long sec_to_add = period_ms / 1000;
long nsec_to_add = (period_ms % 1000) * 1000000;
next_wakeup.tv_sec += sec_to_add;
next_wakeup.tv_nsec += nsec_to_add;
// Adjust tv_sec and tv_nsec if necessary
if (next_wakeup.tv_nsec >= 1000000000) {
next_wakeup.tv_sec += 1;
next_wakeup.tv_nsec -= 1000000000;
}
return next_wakeup;
}
int main() {
printf("Requesting master...\n");
master = ecrt_request_master(0);
if (!master) {
exit(EXIT_FAILURE);
}
// 这里可以配置1C12,1C13等配置
//int slave = 0;
/* *//*RxPDO*//*
sdoWrite<int8_t>(slave, 0x1C12, 0, 0);
*//*TxPDO*//*
sdoWrite<int8_t>(slave, 0x1C13, 0, 0);
*//*RxPDO*//*
sdoWrite<int8_t>(slave, 0x1600, 0, 0);
sdoWrite<int32_t>(slave, 0x1600, 1, 0x60400010); // Controlword
sdoWrite<int32_t>(slave, 0x1600, 2, 0x60710010); // Target torque
sdoWrite<int32_t>(slave, 0x1600, 3, 0x607A0020); // Target position
sdoWrite<int32_t>(slave, 0x1600, 4, 0x60FF0020); // Target velocity
sdoWrite<int32_t>(slave, 0x1600, 5, 0x60600008); // Modes of operation display
sdoWrite<int32_t>(slave, 0x1600, 6, 0x60B20010); // Torque offset
sdoWrite<int8_t>(slave, 0x1600, 0, 6);
*//*TxPDO*//*
sdoWrite<int16_t>(slave, 0x1A00, 0, 0);
sdoWrite<int32_t>(slave, 0x1A00, 1, 0x60410010); // Statusword
sdoWrite<int32_t>(slave, 0x1A00, 2, 0x60640020); // Position actual value
sdoWrite<int32_t>(slave, 0x1A00, 3, 0x606C0020); // Velocity actual value
sdoWrite<int32_t>(slave, 0x1A00, 4, 0x60770010); // Torque actual value
sdoWrite<int32_t>(slave, 0x1A00, 5, 0x60610008); // Modes of operation display
sdoWrite<int8_t>(slave, 0x1A00, 0, 5);
*//* write 0x1C12 and 0x1C13 (RxPDO and TxPDO) *//*
sdoWrite<int16_t>(slave, 0x1C12, 1, 0x1600);
sdoWrite<int8_t>(slave, 0x1C12, 0, 1);
sdoWrite<int16_t>(slave, 0x1C13, 1, 0x1A00);
sdoWrite<int8_t>(slave, 0x1C13, 0, 1);*/
//sdoWrite<int16_t>(0, 0x6040, 0, 0x80); //reset faluts
//sdoWrite<int16_t>(0, 0x6040, 7, 0x1); //reset faluts
//sdoWrite<int8_t>(0, 0x6060, 0, 8); //Modes of Operation: 8 [cyclic synchronous position mode]
//sdoWrite<int8_t>(0, 0x60C2, 1, 4000/100);
//sdoWrite<int8_t>(0, 0x60C2, 2, -4);
//string deviceName = sdoRead<string>(slave, 0x1008, 0); // Device name
//cout << "Device name: " << deviceName << endl;
//sdoWrite<uint8_t>(0, 0x6060, 0, 8);
//sdoWrite<uint8_t>(0, 0x60C2, 1, 1);
domain1 = ecrt_master_create_domain(master);
if (!domain1) {
exit(EXIT_FAILURE);
}
if (!(sc = ecrt_master_slave_config(master, BusCouplerPos, Detail_ASD_A2_E_1_Pos))) {
fprintf(stderr, "Failed to get slave configuration for slave!\n");
exit(EXIT_FAILURE);
}
printf("Configuring PDOs...\n");
if (ecrt_slave_config_pdos(sc, EC_END, device_syncs)) {
fprintf(stderr, "Failed to configure slave PDOs!\n");
exit(EXIT_FAILURE);
} else {
printf("*Success to configuring slave PDOs*\n");
}
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
fprintf(stderr, "PDO entry registration failed!\n");
exit(EXIT_FAILURE);
}
ecrt_slave_config_dc(sc, 0x0300, 1000000, 0, 0, 0);
ecrt_master_select_reference_clock(master, nullptr);
//ecrt_master_application_time(master, 1);
//ecrt_master_sync_reference_clock(master);
//ecrt_master_sync_slave_clocks(master);
ecrt_slave_config_sdo8(sc, 0x6040, 0, 0x80);
ecrt_slave_config_sdo8(sc, 0x60C2, 1, 1);
ecrt_slave_config_sdo8(sc, 0x6060, 0, 8);
printf("Activating master...\n");
if (ecrt_master_activate(master)) {
exit(EXIT_FAILURE);
} else {
printf("*Master activated*\n");
}
if (!(domain1_pd = ecrt_domain_data(domain1))) {
exit(EXIT_FAILURE);
}
printf("*It's working now*\n");
struct timespec wakeup_time{};
clock_gettime(CLOCK_MONOTONIC, &wakeup_time);
const long PERIOD_MS = 1; // 1ms
while (true) {
//usleep(1000);
//cyclic_task();
wakeup_time = calculate_next_wakeup(wakeup_time, PERIOD_MS);
//printf("Next wakeup: %ld.%09ld\n", wakeup_time.tv_sec, wakeup_time.tv_nsec);
cyclic_task();
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &wakeup_time, nullptr);
//cyclic_task();
}
ecrt_release_master(master);
return EXIT_SUCCESS;
}