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