引言
這篇文章是筆者的第一篇文章,筆者作為一個機(jī)器人從業(yè)者,經(jīng)常要接觸到EtherCAT與ROS等相關(guān)內(nèi)容。目前市面上有的開源EtherCAT系統(tǒng)有Igh以及SOEM兩種,Igh在多年前已經(jīng)停止維護(hù),而截至日前SOEM依然維持更新,且SOEM已經(jīng)集成到ROS生態(tài)中,故筆者選擇SOEM進(jìn)行研究。
苦于網(wǎng)上資料較少,筆者在學(xué)習(xí)摸索期間遇到大大小小的坑,浪費(fèi)了不少時間。如今分享一下自己的一個例程,希望大家也能盡快掌握SOEM的使用。
開發(fā)環(huán)境
操作系統(tǒng):Ubuntu22.04
系統(tǒng)內(nèi)核:Linux 5.15.0-1022-realtime
注:Ubuntu在22.04版本已經(jīng)提供官方的實(shí)時補(bǔ)丁,不需要自己編譯實(shí)時內(nèi)核,其使用的是preempt-rt補(bǔ)丁,實(shí)時性與自己打的補(bǔ)丁一樣。可參考:Real-time Ubuntu 22.04 LTS Beta – Now Available
例程
介紹
該例程的作用為控制伺服電機(jī),通過SOEM與電機(jī)驅(qū)動器進(jìn)行通訊。
代碼
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <unistd.h>
#include <string.h>
#include <sys/time.h>
#include <time.h>
#define __USE_GNU
#include <pthread.h>
#include <math.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h> //PISOX中定義的標(biāo)準(zhǔn)接口
#include "ethercat.h"
#define NSEC_PER_SEC 1000000000
#define EC_TIMEOUTMON 500
#define EEP_MAN_SYNAPTICON (0x000022d2)
#define EEP_ID_SYNAPTICON (0x00000201)
struct sched_param schedp;
char IOmap[4096];
pthread_t thread1, thread2;
uint64_t diff, maxt, avg, cycle;
struct timeval t1, t2;
int dorun = 0;
int64 toff = 0;
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;
enum {
STATE_RESET,
STATE_INIT,
STATE_PREREADY,
STATE_READY,
STATE_ENABLE,
STATE_DISABLE
}
int state = STATE_RESET;
typedef struct PACKED
{
uint16_t Controlword;
int16_t TargetTor;
int32_t TargetPos;
int32_t TargetVel;
uint8_t ModeOp;
int16_t TorOff;
} Drive_Outputs;
typedef struct PACKED
{
uint16_t Statusword;
int32_t ActualPos;
int32_t ActualVel;
int16_t ActualTor;
uint8_t ModeOp;
int32_t SecPos;
} Drive_Inputs;
static int drive_write8(uint16 slave, uint16 index, uint8 subindex, uint8 value)
{
int wkc;
wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
return wkc;
}
static int drive_write16(uint16 slave, uint16 index, uint8 subindex, uint16 value)
{
int wkc;
wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
return wkc;
}
static int drive_write32(uint16 slave, uint16 index, uint8 subindex, int32 value)
{
int wkc;
wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);
return wkc;
}
// 該函數(shù)用于設(shè)置PDO映射表
int drive_setup(uint16 slave)
{
int wkc = 0;
printf("Drive setup\n");
wkc += drive_write16(slave, 0x1C12, 0, 0);
wkc += drive_write16(slave, 0x1C13, 0, 0);
wkc += drive_write16(slave, 0x1A00, 0, 0);
wkc += drive_write32(slave, 0x1A00, 1, 0x60410010); // Statusword
wkc += drive_write32(slave, 0x1A00, 2, 0x60640020); // Position actual value
wkc += drive_write32(slave, 0x1A00, 3, 0x606C0020); // Velocity actual value
wkc += drive_write32(slave, 0x1A00, 4, 0x60770010); // Torque actual value
wkc += drive_write32(slave, 0x1A00, 5, 0x60610008); // Modes of operation display
wkc += drive_write32(slave, 0x1A00, 6, 0x230A0020); // 2nd Pos
wkc += drive_write8(slave, 0x1A00, 0, 6);
wkc += drive_write8(slave, 0x1600, 0, 0);
wkc += drive_write32(slave, 0x1600, 1, 0x60400010); // Controlword
wkc += drive_write32(slave, 0x1600, 2, 0x60710010); // Target torque
wkc += drive_write32(slave, 0x1600, 3, 0x607A0020); // Target position
wkc += drive_write32(slave, 0x1600, 4, 0x60FF0020); // Target velocity
wkc += drive_write32(slave, 0x1600, 5, 0x60600008); // Modes of operation display
wkc += drive_write32(slave, 0x1600, 6, 0x60B20010); // Torque offset
wkc += drive_write8(slave, 0x1600, 0, 6);
wkc += drive_write16(slave, 0x1C12, 1, 0x1600);
wkc += drive_write8(slave, 0x1C12, 0, 1);
wkc += drive_write16(slave, 0x1C13, 1, 0x1A00);
wkc += drive_write8(slave, 0x1C13, 0, 1);
strncpy(ec_slave[slave].name, "Drive", EC_MAXNAME);
if (wkc != 22)
{
printf("Drive %d setup failed\nwkc: %d\n", slave, wkc);
return -1;
}
else
printf("Drive %d setup succeed.\n", slave);
return 0;
}
/* add ns to timespec */
void add_timespec(struct timespec *ts, int64 addtime)
{
int64 sec, nsec;
nsec = addtime % NSEC_PER_SEC;
sec = (addtime - nsec) / NSEC_PER_SEC;
ts->tv_sec += sec;
ts->tv_nsec += nsec;
if (ts->tv_nsec > NSEC_PER_SEC)
{
nsec = ts->tv_nsec % NSEC_PER_SEC;
ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
ts->tv_nsec = nsec;
}
}
/* PI calculation to get linux time synced to DC time */
void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
{
static int64 integral = 0;
int64 delta;
/* set linux sync point 50us later than DC sync, just as example */
delta = (reftime - 50000) % cycletime;
if (delta > (cycletime / 2))
{
delta = delta - cycletime;
}
if (delta > 0)
{
integral++;
}
if (delta < 0)
{
integral--;
}
*offsettime = -(delta / 100) - (integral / 20);
}
static inline int64_t calcdiff_ns(struct timespec t1, struct timespec t2)
{
int64_t tdiff;
tdiff = NSEC_PER_SEC * (int64_t)((int)t1.tv_sec - (int)t2.tv_sec);
tdiff += ((int)t1.tv_nsec - (int)t2.tv_nsec);
return tdiff;
}
static int latency_target_fd = -1;
static int32_t latency_target_value = 0;
/* 消除系統(tǒng)時鐘偏移函數(shù),取自cyclic_test */
static void set_latency_target(void)
{
struct stat s;
int ret;
if (stat("/dev/cpu_dma_latency", &s) == 0)
{
latency_target_fd = open("/dev/cpu_dma_latency", O_RDWR);
if (latency_target_fd == -1)
return;
ret = write(latency_target_fd, &latency_target_value, 4);
if (ret == 0)
{
printf("# error setting cpu_dma_latency to %d!: %s\n", latency_target_value, strerror(errno));
close(latency_target_fd);
return;
}
printf("# /dev/cpu_dma_latency set to %dus\n", latency_target_value);
}
}
void test_driver(char *ifname, int mode)
{
int cnt, i, j ;
Drive_Inputs *iptr;
Drive_Outputs *optr;
struct sched_param schedp;
cpu_set_t mask;
pthread_t thread;
int ht;
int chk = 2000;
int64 cycletime;
struct timespec ts, tnow;
CPU_ZERO(&mask);
CPU_SET(2, &mask);
thread = pthread_self();
pthread_setaffinity_np(thread, sizeof(mask), &mask);
memset(&schedp, 0, sizeof(schedp));
schedp.sched_priority = 99; /* 設(shè)置優(yōu)先級為99,即RT */
sched_setscheduler(0, SCHED_FIFO, &schedp);
printf("Starting Redundant test\n");
/* initialise SOEM, bind socket to ifname */
if (ec_init(ifname))
{
printf("ec_init on %s succeeded.\n", ifname);
/* find and auto-config slaves */
if (ec_config_init(FALSE) > 0)
{
printf("%d slaves found and configured.\n", ec_slavecount);
/* wait for all slaves to reach SAFE_OP state */
int slave_ix;
for (slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++)
{
ec_slavet *slave = &ec_slave[slave_ix];
slave->PO2SOconfig = drive_setup;
}
/* configure DC options for every DC capable slave found in the list */
ec_config_map(&IOmap); // 此處調(diào)用drive_setup函數(shù),進(jìn)行PDO映射表設(shè)置
ec_configdc(); // 設(shè)置同步時鐘,該函數(shù)必須在設(shè)置pdo映射之后;
// setup dc for devices
for (slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++)
{
ec_dcsync0(slave_ix, TRUE, 4000000U, 20000U);
// ec_dcsync01(slave_ix, TRUE, 4000000U, 8000000U, 20000U);
}
printf("Slaves mapped, state to SAFE_OP.\n");
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
/* read indevidual slave state and store in ec_slave[] */
ec_readstate();
for (cnt = 1; cnt <= ec_slavecount; cnt++)
{
printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
}
expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
printf("Calculated workcounter %d\n", expectedWKC);
printf("Request operational state for all slaves\n");
/* activate cyclic process data */
/* wait for all slaves to reach OP state */
ec_slave[0].state = EC_STATE_OPERATIONAL;
/* request OP state for all slaves */
ec_writestate(0);
clock_gettime(CLOCK_MONOTONIC, &ts);
ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */
ts.tv_nsec = ht * 1000000;
cycletime = 4000 * 1000; /* cycletime in ns */
/* 對PDO進(jìn)行初始化 */
for (i = 0; i < ec_slavecount; i++)
{
optr = (Drive_Outputs *)ec_slave[i + 1].outputs;
if(optr == NULL)
{
printf("optr is NULL.\n");
}
optr->Controlword = 0;
optr->TargetPos = 0;
optr->ModeOp = 0;
optr->TargetTor = 0;
optr->TargetVel = 0;
optr->TorOff = 0;
}
do
{
/* wait to cycle start */
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, NULL);
if (ec_slave[0].hasdc)
{
/* calulate toff to get linux time and DC synced */
ec_sync(ec_DCtime, cycletime, &toff);
}
wkc = ec_receive_processdata(EC_TIMEOUTRET);
ec_send_processdata();
add_timespec(&ts, cycletime + toff);
} while (chk-- && (wkc != expectedWKC));
/* 此處與SOEM官方例程不一樣,因?yàn)閑c_statecheck函數(shù)消耗的時間較多,有可能超過循環(huán)周期 */
if (wkc == expectedWKC)
{
printf("Operational state reached for all slaves.\n");
inOP = TRUE;
cnt = 0;
while (cnt<10)
{
/* 計(jì)算下一周期喚醒時間 */
add_timespec(&ts, cycletime + toff);
/* wait to cycle start */
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, NULL);
clock_gettime(CLOCK_MONOTONIC, &tnow);
if (ec_slave[0].hasdc)
{
/* calulate toff to get linux time and DC synced */
ec_sync(ec_DCtime, cycletime, &toff);
}
wkc = ec_receive_processdata(EC_TIMEOUTRET);
diff = calcdiff_ns(tnow, ts);
switch (state)
{
case STATE_RESET: /* 對驅(qū)動器清除故障 */
for (i = 0; i < ec_slavecount; i++)
{
optr = (Drive_Outputs *)ec_slave[i + 1].outputs;
optr->Controlword = 128;
}
state = 0;
break;
case STATE_INIT /* 初始化驅(qū)動器 */:
for (i = 0; i < ec_slavecount; i++)
{
optr = (Drive_Outputs *)ec_slave[i + 1].outputs;
iptr = (Drive_Inputs *)ec_slave[i + 1].inputs;
optr->Controlword = 0;
optr->TargetPos = iptr->ActualPos;
}
state = STATE_PREREADY;
break;
case STATE_PREREADY:
for (i = 0; i < ec_slavecount; i++)
{
optr = (Drive_Outputs *)ec_slave[i + 1].outputs;
optr->Controlword = 6;
}
state = STATE_READY;
break;
case STATE_READY /* 系統(tǒng)轉(zhuǎn)為準(zhǔn)備使能狀態(tài) */:
for (i = 0; i < ec_slavecount; i++)
{
optr = (Drive_Outputs *)ec_slave[i + 1].outputs;
optr->Controlword = 7;
optr->ModeOp = 8;
}
state = STATE_ENABLE;
break;
case STATE_ENABLE /* 驅(qū)動器使能 */:
for (i = 0; i < ec_slavecount; i++)
{
optr = (Drive_Outputs *)ec_slave[i + 1].outputs;
optr->Controlword = 15;
}
break;
case STATE_DISABLE:
/* 使電機(jī)失能并在10個循環(huán)之后退出循環(huán) */
for (i = 0; i < ec_slavecount; i++)
{
optr = (Drive_Outputs *)ec_slave[i + 1].outputs;
optr->ModeOp = 0;
optr->TargetVel = 0;
optr->Controlword = 6;
}
cnt ++;
break;
default:
break;
}
ec_send_processdata();
cycle++;
avg += diff;
if (diff > maxt)
maxt = diff;
for (j = 0; j < 1; j++)
{
iptr = (Drive_Inputs *)ec_slave[j + 1].inputs;
optr = (Drive_Outputs *)ec_slave[j + 1].outputs;
printf(" %d: CW: %d, status: %d, pos: %d", j + 1, optr->Controlword, iptr->Statusword, iptr->ActualPos);
}
printf(", MaxLatency: %lu, avg: %lu \r", maxt, avg / cycle);
fflush(stdout);
}
dorun = 0;
}
else /* ECAT進(jìn)入OP失敗 */
{
printf("Not all slaves reached operational state.\n");
ec_readstate();
for (i = 1; i <= ec_slavecount; i++)
{
if (ec_slave[i].state != EC_STATE_OPERATIONAL)
{
printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
}
}
}
/* 斷開ECAT通訊 */
printf("\nRequest safe operational state for all slaves\n");
ec_slave[0].state = EC_STATE_SAFE_OP;
/* request SAFE_OP state for all slaves */
ec_writestate(0);
ec_slave[0].state = EC_STATE_PRE_OP;
ec_writestate(0);
ec_slave[0].state = EC_STATE_INIT;
ec_writestate(0);
ec_readstate();
if (ec_statecheck(0, EC_STATE_SAFE_OP, 1000) == EC_STATE_INIT)
{
printf("ECAT changed into state init\n");
}
}
else
{
printf("No slaves found!\n");
}
printf("End driver test, close socket\n");
/* stop SOEM, close socket */
ec_close();
}
else
{
printf("No socket connection on %s\nExcecute as root\n", ifname);
}
}
// 檢測鍵盤輸入,如檢測到esc即關(guān)閉SOEM退出程序
OSAL_THREAD_FUNC scanKeyboard()
{
int in;
// int i;
// Drive_Outputs *optr;
struct sched_param schedp;
cpu_set_t mask;
pthread_t thread;
struct termios new_settings;
struct termios stored_settings;
CPU_ZERO(&mask);
CPU_SET(2, &mask);
thread = pthread_self();
pthread_setaffinity_np(thread, sizeof(mask), &mask);
memset(&schedp, 0, sizeof(schedp));
schedp.sched_priority = 20;
sched_setscheduler(0, SCHED_FIFO, &schedp);
tcgetattr(0, &stored_settings);
new_settings = stored_settings;
new_settings.c_lflag &= (~ICANON); //屏蔽整行緩存
new_settings.c_cc[VTIME] = 0;
/*這個函數(shù)調(diào)用把當(dāng)前終端接口變量的值寫入termios_p參數(shù)指向的結(jié)構(gòu)。
如果這些值其后被修改了,你可以通過調(diào)用函數(shù)tcsetattr來重新配置
調(diào)用tcgetattr初始化一個終端對應(yīng)的termios結(jié)構(gòu)
int tcgetattr(int fd, struct termios *termios_p);*/
tcgetattr(0, &stored_settings);
new_settings.c_cc[VMIN] = 1;
/*int tcsetattr(int fd , int actions , const struct termios *termios_h)
參數(shù)actions控制修改方式,共有三種修改方式,如下所示。
1.TCSANOW:立刻對值進(jìn)行修改
2.TCSADRAIN:等當(dāng)前的輸出完成后再對值進(jìn)行修改。
3.TCSAFLUSH:等當(dāng)前的輸出完成之后,再對值進(jìn)行修改,但丟棄還未從read調(diào)用返回的當(dāng)前的可用的任何輸入。*/
tcsetattr(0, TCSANOW, &new_settings);
in = getchar();
tcsetattr(0, TCSANOW, &stored_settings);
while (1)
{
if (in == 27)
{
state = STATE_DISABLE;
printf("the keyboard input is: \n");
putchar(in);
break;
}
osal_usleep(10000); //間隔10ms循環(huán)一次;
}
}
OSAL_THREAD_FUNC ecatcheck(void *ptr)
{
int slave;
(void)ptr; /* Not used */
struct sched_param schedp;
cpu_set_t mask;
pthread_t thread;
time_t terr;
/* 設(shè)定線程優(yōu)先級為20 */
CPU_ZERO(&mask);
CPU_SET(2, &mask);
thread = pthread_self();
pthread_setaffinity_np(thread, sizeof(mask), &mask);
memset(&schedp, 0, sizeof(schedp));
schedp.sched_priority = 21;
sched_setscheduler(0, SCHED_FIFO, &schedp);
while (1)
{
if (inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate))
{
time(&terr);
printf("wkc: %d, expwkc: %d, docheckstate: %d, error time: %s\n", wkc, expectedWKC, ec_group[0].docheckstate, ctime(&terr));
if (needlf)
{
needlf = FALSE;
printf("\n");
}
/* one ore more slaves are not responding */
ec_group[currentgroup].docheckstate = FALSE;
ec_readstate();
for (slave = 1; slave <= ec_slavecount; slave++)
{
if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
{
ec_group[currentgroup].docheckstate = TRUE;
if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
{
printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave);
ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
ec_writestate(slave);
}
else if (ec_slave[slave].state == EC_STATE_SAFE_OP)
{
printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
ec_slave[slave].state = EC_STATE_OPERATIONAL;
ec_writestate(slave);
}
else if (ec_slave[slave].state > EC_STATE_NONE)
{
if (ec_reconfig_slave(slave, EC_TIMEOUTMON))
{
ec_slave[slave].islost = FALSE;
printf("MESSAGE : slave %d reconfigured\n", slave);
}
}
else if (!ec_slave[slave].islost)
{
/* re-check state */
ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
if (ec_slave[slave].state == EC_STATE_NONE)
{
ec_slave[slave].islost = TRUE;
printf("ERROR : slave %d lost\n", slave);
}
}
}
if (ec_slave[slave].islost)
{
if (ec_slave[slave].state == EC_STATE_NONE)
{
if (ec_recover_slave(slave, EC_TIMEOUTMON))
{
ec_slave[slave].islost = FALSE;
printf("MESSAGE : slave %d recovered\n", slave);
}
}
else
{
ec_slave[slave].islost = FALSE;
printf("MESSAGE : slave %d found\n", slave);
}
}
}
if (!ec_group[currentgroup].docheckstate)
printf("OK : all slaves resumed OPERATIONAL.\n");
}
osal_usleep(10000);
}
}
#define stack64k (64 * 1024)
int main(int argc, char *argv[])
{
int mode;
printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n");
if (argc > 1)
{
dorun = 0;
set_latency_target(); // 消除系統(tǒng)時鐘偏移
/* create thread to handle slave error handling in OP */
osal_thread_create(&thread2, stack64k * 4, &ecatcheck, NULL);
osal_thread_create(&thread1, stack64k * 4, &scanKeyboard, NULL);
/* start acyclic part */
test_driver(argv[1]);
}
else
{
printf("Usage: red_test ifname1 Mode_of_operation\nifname = eth0 for example\n");
}
printf("End program\n");
return (0);
}
解析
本代碼在SOEM官方示例代碼red_test的基礎(chǔ)上加入了優(yōu)先級實(shí)現(xiàn),取消系統(tǒng)時間偏移,PDO映射表實(shí)現(xiàn),PDO、SDO傳輸?shù)裙δ堋?br>需要注意的是,其中在ECAT進(jìn)入OP狀態(tài)的時候使用了wkc與expectedWKC的相等條件進(jìn)行判斷,因?yàn)閑c_statecheck函數(shù)耗時過長,有可能導(dǎo)致錯誤,同樣ec_readstate函數(shù)消耗時間也是很長,還會對其他線程形成阻塞,注意不要添加在循環(huán)當(dāng)中。文章來源:http://www.zghlxwxcb.cn/news/detail-522860.html
結(jié)語
以上為筆者分享的關(guān)于SOEM的驅(qū)動電機(jī)的一個小例程,希望可以對感興趣的讀者產(chǎn)生啟發(fā),也歡迎大家前來交流溝通。文章來源地址http://www.zghlxwxcb.cn/news/detail-522860.html
到了這里,關(guān)于Ubuntu 22.04下以SOEM為EtherCAT主站的驅(qū)動電機(jī)例子的文章就介紹完了。如果您還想了解更多內(nèi)容,請?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章,希望大家以后多多支持TOY模板網(wǎng)!