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.