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



  • 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!



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


  • Thanks!It's very useful and timely.


Log in to reply