Hi,

You may use Xenomai Event (https://xenomai.org/documentation/xenomai-2.6/html/api/group__event.html) to synchronize the tasks.

Please refer to my example below.

#include <native/event.h>
RT_EVENT syncEvent;
unsigned long evtMask = 0x1; // xenomai event mask
unsigned long numTicks = 4; // 4 times of 1ms cycle
unsigned long remainTicks;
void masterRun(void *arg)
{
    uint64_t* cycleTask = (uint64_t* ) arg;
    rt_task_set_periodic(nullptr, TM_NOW, *cycleTask);
    while (1)
    {
        rt_task_wait_period(nullptr);
        if (remainTicks == 0)
        {
            remainTicks = numTicks - 1; // remainTicks gets value from 0 to 3            
            rt_event_signal(&syncEvent, evtMask); // trigger rt event with the mask
        }
        else
        {
            remainTicks -= 1; // counting down
        }
        
        /* TODO: Implementation */
    }
}
void longRun(void *arg)
{
    while (1)
    {
        // Wait for master event        
        unsigned long evtValue;
        int res = rt_event_wait(&syncEvent, evtMask, &evtValue, EV_ANY, TM_INFINITE);
        rt_event_clear(&syncEvent, evtMask, nullptr);
        /* TODO: Implementation */
    }
}
int main()
{
    // create sync event    
    if (rt_event_create (&syncEvent, "SyncEventName", 0x0, EV_PRIO) != 0)
    {
        printf("Could not create timing event!\n");
        return -1;
    }
    
    
    // bind sync event    
    if (rt_event_bind (&syncEvent, "SyncEventName", TM_NONBLOCK) != 0)
    {
        printf("Could not find timing event!\n");
        return -1;
    }
    
    // start master thread
    uint64_t cycleTaskPeriod = 1000000; // 1ms
    rt_task_create(&master_task, "MasterTask", 0, 99, 0);
    rt_task_start(&master_task, &masterRun, &cycleTaskPeriod);
    
    // start long-time task
    rt_task_create(&long_task, "LongTask", 0, 95, 0);
    rt_task_start(&long_task, &longRun, nullptr);
}

Thanks.

Hi Mr. Quang,

Could you please zip your project and send it to my email at thach.do@neuromeka.com?

Then, I will check the project setting and fix it for you.

Thach

Hi Mr. Quang,

Here are my answers for your questions.

1. Please find the EtherCAT Tool at this URL (http://works.do/xuka73).

2. At first, please follow our instructions here (http://docs.neuromeka.com/2.3.0/en/STEP/section2b/)

If you still have problem, please let me know.

Thanks.

Thach

Add a new object into a PDO

@Dongun-Lee

Thanks for your explanation.

At first, please be sure that the EPOS4 supports torque-control mode. Then, you can add other objects (e.g. TarTor, ActTor, etc.) manually like below.

Please note that some PDOs allow to add more objects but some PDOs do not. Ideally, it should be mentioned in the servo's manual. 

In addition, in order to compile XenomaiTest project in Eclipse, please follow the instructions at Neuromeka's document site.

http://docs.neuromeka.com/2.3.0/kr/STEP/section2b/#build-example-project

@Dongun-Lee said:

각 2개씩만 추가되는것으로 확인 되고 있습니다.혹시 제가 모르는 상위버전의 NRMKECatTool 버전이 존재하는지요?만약에 그렇다면 기존 버전의 ECatTool은 어떻게 삭제하고 다시 넣어주나요?아니면 ECatTool에 호환가능한 EPOS4 Compact EtherCAT Module이 존재한다면 해당 ESI 파일 부탁드려도 되겠습니까?확인 부탁드리겠습니다.감사합니다.

 Dear Dongun,

Please find the latest ECatTool here at http://works.do/xuka73

After generating EtherCAT C++ project, you will see the below successful message. Unless, please copy and send us the error message.

Dear Tian,

I just implemented below code samples for Winsock-based socket communication with the generated EtherCAT app. 

EtherCATConnector.h

#pragma once
#include <sys types.h="">
#include <string>
#include <sstream>
#include <iostream>
#include <memory>
#include <array>
#include <limits>
#include <thread>
#include <mutex>
#define _WINSOCK_DEPRECATED_NO_WARNINGS
#include <winsock2.h>
#include <ws2tcpip.h>
#include <stdio.h>
#pragma comment(lib, "Ws2_32.lib")
class EtherCATConnector
{
public:
    enum {
        SERVER_PORT = 6868,
        DEFAULT_TIMEOUT = 10,
        NUM_MOTORS = 1,
        NUM_BYTES_NUMMOTOR = 1,    // UINT8
        NUM_BYTES_MODEOP = 1,    // UINT8
        NUM_BYTES_STATUS = 2,    // UINT16
        NUM_BYTES_ACTPOS = 4,    // INT32
        NUM_BYTES_ACTVEL = 4,    // INT32
        NUM_BYTES_ACTTOR = 2,    // INT16
        NUM_BYTES_DATA_LENGTH = NUM_BYTES_NUMMOTOR + NUM_MOTORS * (NUM_BYTES_MODEOP + NUM_BYTES_STATUS + NUM_BYTES_ACTPOS + NUM_BYTES_ACTVEL + NUM_BYTES_ACTTOR),
        NUM_BYTES_TARVAL = 4,    // UINT32
        NUM_BYTES_MAXVEL = 4,    // UINT32
        NUM_BYTES_MAXACC = 4,    // UINT32
        NUM_BYTES_MAXJERK = 4,    // UINT32
        NUM_BYTES_CMD_LENGTH = NUM_BYTES_NUMMOTOR + NUM_MOTORS * (NUM_BYTES_MODEOP + NUM_BYTES_TARVAL + NUM_BYTES_MAXVEL + NUM_BYTES_MAXACC + NUM_BYTES_MAXJERK),
    };
public:
    EtherCATConnector(const std::string & serverIP);
    virtual ~EtherCATConnector();
public:
    bool connect();
    void disconnect();
    bool isConnected() { return _connected; }
    bool sendMotionCommand(INT8 const * const ModeOp, float const * const TarVal, float const * const MaxVel, float const * const MaxAcc, float const * const MaxJerk);
    bool getMotionData(INT8 * ModeOp, UINT16 * Status, INT32 * ActPos, INT32 * ActVel, INT16 * ActTor);
private:
    bool _sendMessage(unsigned char const* buffer, size_t size);
    bool _recvMessage(unsigned char* buffer, size_t size);
private:
    const std::string _serverIP;
    WSADATA wsaData;
    SOCKET _sockFd;
    std::mutex _mtx;
    bool _connected;
};

EtherCATConnector.cpp

#include "EtherCATConnector.h"
EtherCATConnector::EtherCATConnector(const std::string& serverIP)
: _serverIP(serverIP)
, _sockFd(INVALID_SOCKET)
, _connected(false)
{
    // Initialize Winsock    
    int iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
    if (iResult != 0) {
        printf("WSAStartup failed: %d\n", iResult);
        return;
    }
}
EtherCATConnector::~EtherCATConnector() {
    disconnect();
}
bool EtherCATConnector::connect() {
    if (_connected)
        disconnect();
    _mtx.lock();
    _sockFd = INVALID_SOCKET;
    struct addrinfo* result = NULL, * ptr = NULL, hints;    
    int iResult;
    ZeroMemory(&hints, sizeof(hints));
    hints.ai_family = AF_UNSPEC;
    hints.ai_socktype = SOCK_STREAM;
    hints.ai_protocol = IPPROTO_TCP;
    // Resolve the server address and port
    //iResult = getaddrinfo(argv[1], DEFAULT_PORT, &hints, &result);
    iResult = getaddrinfo(_serverIP.c_str(), std::to_string(SERVER_PORT).c_str(), &hints, &result);
    if (iResult != 0) {
        printf("getaddrinfo failed with error: %d\n", iResult);
        WSACleanup();
        _mtx.unlock();
        return false;
    }
    // Attempt to connect to an address until one succeeds
    for (ptr = result; ptr != NULL; ptr = ptr->ai_next) {
        // Create a SOCKET for connecting to server
        _sockFd = socket(ptr->ai_family, ptr->ai_socktype,
            ptr->ai_protocol);
        if (_sockFd == INVALID_SOCKET) {
            printf("socket failed with error: %ld\n", WSAGetLastError());
            WSACleanup();
            _mtx.unlock();
            return false;
        }
        // Connect to server.
        iResult = ::connect(_sockFd, ptr->ai_addr, (int)ptr->ai_addrlen);
        if (iResult == SOCKET_ERROR) {
            closesocket(_sockFd);
            _sockFd = INVALID_SOCKET;
            continue;
        }
        break;
    }
    freeaddrinfo(result);
    if (_sockFd == INVALID_SOCKET) {
        printf("Unable to connect to server!\n");
        WSACleanup();
        _mtx.unlock();
        return false;
    }
    _mtx.unlock();
    unsigned char connectReqKey = 'O';
    _sendMessage(&connectReqKey, 1);
    
    _connected = true;
    return true;
}
void EtherCATConnector::disconnect() {
    _mtx.lock();
    if (_sockFd != INVALID_SOCKET)
    {
        try {
            ::closesocket(_sockFd);
        }
        catch (std::exception) {}
    }
    _sockFd = INVALID_SOCKET;
    _connected = false;
    _mtx.unlock();
}
bool EtherCATConnector::sendMotionCommand(INT8 const* const ModeOp, float const* const TarVal, float const* const MaxVel, float const* const MaxAcc, float const* const MaxJerk) {
    unsigned char _cmdBuf[NUM_BYTES_CMD_LENGTH + 4];
    int bufIdx = 0;
    memcpy(_cmdBuf, "NS", 2);                                //2 bytes, START_tokens "NS"
    bufIdx += 2;
    unsigned char _numMotors = NUM_MOTORS;
    memcpy(_cmdBuf + 2, &_numMotors, NUM_BYTES_NUMMOTOR);    //1 byte, num joints
    bufIdx += NUM_BYTES_NUMMOTOR;
    for (unsigned int i = 0; i < _numMotors; i++)
    {
        memcpy(_cmdBuf + bufIdx, &ModeOp[i], NUM_BYTES_MODEOP);                                        // Mode Operation
        bufIdx += NUM_BYTES_MODEOP;
        memcpy(_cmdBuf + bufIdx, &TarVal[i], NUM_BYTES_TARVAL);                                        // Status Word
        bufIdx += NUM_BYTES_TARVAL;
        memcpy(_cmdBuf + bufIdx, &MaxVel[i], NUM_BYTES_MAXVEL);                                        // Actual Position
        bufIdx += NUM_BYTES_MAXVEL;
        memcpy(_cmdBuf + bufIdx, &MaxAcc[i], NUM_BYTES_MAXACC);                                        // Actual Velocity
        bufIdx += NUM_BYTES_MAXACC;
        memcpy(_cmdBuf + bufIdx, &MaxJerk[i], NUM_BYTES_MAXJERK);                                        // Actual Torque
        bufIdx += NUM_BYTES_MAXJERK;
    }
    memcpy(_cmdBuf + bufIdx, "NE", 2);    //2 bytes, END_token
    bufIdx += 2;
    // Send Data
    return _sendMessage(_cmdBuf, bufIdx);
}
bool EtherCATConnector::getMotionData(INT8* ModeOp, UINT16* Status, INT32* ActPos, INT32* ActVel, INT16* ActTor) {
    unsigned char _dataBuf[NUM_BYTES_DATA_LENGTH + 4];
    int bufIdx = 0;
    if (!_recvMessage(_dataBuf, NUM_BYTES_DATA_LENGTH + 4)) return false;
    if ((_dataBuf[0] != 'N') || (_dataBuf[1] != 'S'))  return false;
    bufIdx += 2;
    unsigned char _numJoints = 0;
    memcpy(&_numJoints, _dataBuf + bufIdx, NUM_BYTES_NUMMOTOR);
    bufIdx += NUM_BYTES_NUMMOTOR;
    for (int i = 0; i < _numJoints; i++)
    {
        memcpy(&ModeOp[i], _dataBuf + bufIdx, NUM_BYTES_MODEOP);                                        // Mode Operation
        bufIdx += NUM_BYTES_MODEOP;
        memcpy(&Status[i], _dataBuf + bufIdx, NUM_BYTES_STATUS);                                        // Target Value
        bufIdx += NUM_BYTES_STATUS;
        memcpy(&ActPos[i], _dataBuf + bufIdx, NUM_BYTES_ACTPOS);                                        // Maximum Velocity
        bufIdx += NUM_BYTES_ACTPOS;
        memcpy(&ActVel[i], _dataBuf + bufIdx, NUM_BYTES_ACTVEL);                                        // Maximum Acceleration
        bufIdx += NUM_BYTES_ACTVEL;
        memcpy(&ActTor[i], _dataBuf + bufIdx, NUM_BYTES_ACTTOR);                                    // Maximum Jerk
        bufIdx += NUM_BYTES_ACTTOR;
    }
    return true;
}
bool EtherCATConnector::_sendMessage(unsigned char const* buffer, size_t size) {
    size_t curr = 0;
    while (curr < size)
    {
        _mtx.lock();
        int sendLen = ::send(_sockFd, reinterpret_cast<const char*="">(buffer) + curr, size - curr, 0);
        int err = WSAGetLastError();
        _mtx.unlock();
        if (sendLen == -1)
        {
            switch (err)
            {
            case WSAEINVAL: case WSAEFAULT: case WSAENETRESET: case WSAECONNRESET: case WSAECONNABORTED:
            case EINVAL: case EBADF: case ECONNRESET: case ENXIO: case EPIPE:
            {
                disconnect();
                std::cout << "Send Data Error " << '(' << err << ')' << "!\n";
            }
            case WSA_IO_INCOMPLETE: case WSAENETUNREACH: case WSAENETDOWN:
            case EFBIG: case EIO: case ENETDOWN: case ENETUNREACH: case ENOSPC:
            {
                disconnect();
                std::cout << "Send Resource Failure " << '(' << err << ')' << "!\n";
            }
            case WSAEINTR:
            case EINTR:    //interrupt...
            {
                Sleep(10);    //10ms
                continue;
            }
            case WSATRY_AGAIN: case WSA_IO_PENDING: case WSAEWOULDBLOCK: case EWOULDBLOCK:
            case EAGAIN:    //temp error
            {
                Sleep(10);    //10ms
                continue;
            }
            default:
            {
                disconnect();
                std::cout << "Send Data Failure " << '(' << err << ')' << "!\n";
            }
            }
        }
        else if (sendLen == 0)
        {
            Sleep(10);    //10ms
            continue;
        }
        curr += sendLen;
        Sleep(5);    //5ms
    }
    return true;
}
bool EtherCATConnector::_recvMessage(unsigned char* buffer, size_t size) {
    size_t curr = 0;
    while (curr < size)
    {
        _mtx.lock();
        int recvLen = ::recv(_sockFd, reinterpret_cast<char*>(buffer) + curr, size - curr, 0);
        int err = WSAGetLastError();
        _mtx.unlock();
        if (recvLen == 0)
        {
            //std::cout << "No Data Arrived!\n";
        }
        if (recvLen == -1)
        {
            int err = WSAGetLastError();
            switch (err)
            {
            case WSAEINVAL: case WSAEFAULT: case WSAENETRESET: case WSAECONNRESET: case WSAECONNABORTED:
            case EINVAL: case EBADF: case ECONNRESET: case ENXIO: case EPIPE:
            {
                disconnect();
                std::cout << "Receiving Error " << '(' << err << ")!\n";
            }
            case WSA_IO_INCOMPLETE: case WSAENETUNREACH: case WSAENETDOWN:
            case EFBIG: case EIO: case ENETDOWN: case ENETUNREACH: case ENOSPC:
            {
                disconnect();
                std::cout << "Receiving Resource Failure " << '(' << err << ")!\n";
            }
            case WSAEINTR:
            case EINTR:    //interrupt...
            {
                Sleep(10);    //10ms
                continue;
            }
            case WSATRY_AGAIN: case WSA_IO_PENDING: case WSAEWOULDBLOCK: case EWOULDBLOCK:
            case EAGAIN:    //temp error
            {
                Sleep(10);    //10ms
                continue;
            }
            default:    //else err code
            {
                disconnect();
                std::cout << "Receiving Failed " << '(' << err << ")!\n";
            }
            }
        }
        curr += recvLen;
        Sleep(5);    //5ms
    }
    return true;
}

main.cpp

#include <thread> 
#include "EtherCATConnector.h"
EtherCATConnector ecatClient("192.168.1.13");
void readEtherCATData(INT8* modeOpDisp, UINT16* status, INT32* actPos, INT32* actVel, INT16* actTor)
{
    if (ecatClient.isConnected())
        ecatClient.getMotionData(modeOpDisp, status, actPos, actVel, actTor);
    Sleep(100);
}
int main(int argc, char* argv[])
{
    INT8 modeOpDisp[EtherCATConnector::NUM_MOTORS] = { 0 };
    UINT16 status[EtherCATConnector::NUM_MOTORS] = { 0 };
    INT32 actPos[EtherCATConnector::NUM_MOTORS] = { 0 };
    INT32 actVel[EtherCATConnector::NUM_MOTORS] = { 0 };
    INT16 actTor[EtherCATConnector::NUM_MOTORS] = { 0 };
    INT8 modeOp[EtherCATConnector::NUM_MOTORS] = { 0 };
    float tarval[EtherCATConnector::NUM_MOTORS] = { 0 };
    float maxvel[EtherCATConnector::NUM_MOTORS] = { 0 };
    float maxacc[EtherCATConnector::NUM_MOTORS] = { 0 };
    float maxjerk[EtherCATConnector::NUM_MOTORS] = { 0 };
    if (ecatClient.connect())
        std::cout << "EtherCAT Client Connected Successfully\n";
    else
        std::cout << "EtherCAT Client Connected Failure\n";
    std::thread recvThread(readEtherCATData, modeOpDisp, status, actPos, actVel, actTor);
    modeOp[0] = 0; // 8 : CSP | 9 : CSV | 10 : CST
    tarval[0] = 2;
    maxvel[0] = 3; // encoder/sec
    maxacc[0] = 4; // encoder/sec2
    maxjerk[0] = 5; // encoder/sec3
    Sleep(1000);
    if (ecatClient.sendMotionCommand(modeOp, tarval, maxvel, maxacc, maxjerk))
        std::cout << "Send Command Successfully!\n";
    else
        std::cout << "Send Command Failure!\n";
    while (ecatClient.isConnected()) {
        if (ecatClient.sendMotionCommand(modeOp, tarval, maxvel, maxacc, maxjerk))
            std::cout << "Send Command Successfully!\n";
        else
            std::cout << "Send Command Failure!\n";
        std::cout << "Received Data: \n";
        std::cout << "\t" << (int)modeOpDisp[0] << "\n";
        std::cout << "\t" << status[0] << "\n";
        std::cout << "\t" << actPos[0] << "\n";
        std::cout << "\t" << actVel[0] << "\n";
        std::cout << "\t" << actTor[0] << "\n";
        Sleep(1000);
    }
    ecatClient.disconnect();
    return 0;
}