Using the visual studio 2012 project to make the Socket communication between the SETP2 and the control PC

[tian]

Now I want to using  the visual studio 2012 to make the Socket communication between the SETP2 and the control PC, to dubug the GUI control,  I have find the QT demo. Can you giving me a visual studio demo  or a socket lib is ok. thanks!

[thach.do]

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

[tian]

Thanks！It's very useful and timely.