WinSockets 网络编程及UR机器人通信

Posted 2021-03-29 马少爷

1) Windows Sockets初始化

在程序的开始，进行Windows Sockets初始化。WSAStartup必须是应用程序或DLL调用的第一个Windows Sockets函数。它允许应用程序或DLL指明Windows Sockets API的版本号及获得特定Windows Sockets实现的细节。应用程序或DLL只能在一次成功的WSAStartup()调用之后才能调用进一步的Windows Sockets API函数。

 WSADATA wsaData; if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) { printf("Failed to load Winsock"); return;  }

对应的，在程序结束前，调用以下语句

WSACleanup();

2) 建立和断开与服务器的连接

通过IP和端口与服务器建立Socket通信通道，对于UR机器人，缺省的IP和端口为192.168.0.77:30003。
首先建立连接：

 char* pIp = "192.168.0.77"; int nPort = 30003; SOCKADDR_IN addrSrv; addrSrv.sin_family = AF_INET; addrSrv.sin_port = htons(nPort); // addrSrv.sin_addr.S_un.S_addr = inet_addr(pIp); inet_pton(AF_INET, pIp, &addrSrv.sin_addr);
 //创建套接字 m_sockData = socket(AF_INET, SOCK_STREAM, 0); if (INVALID_SOCKET == m_sockData) { m_nError = WSAGetLastError(); return; }
 //向服务器发出连接请求 if (connect(m_sockData, (struct sockaddr*)&addrSrv, sizeof(addrSrv)) == INVALID_SOCKET) { m_nError = WSAGetLastError(); CString msg; msg.Format(_T("Connect Error=%d"), m_nError); AfxMessageBox(msg); return; } //启动接收线程 m_bThread=true; AfxBeginThread(Thread_RecvData, this, THREAD_PRIORITY_IDLE);

对应的，最后程序退出或不再使用此通道时，断开连接：

 if (m_sockData != INVALID_SOCKET) { shutdown(m_sockData, 0); closesocket(m_sockData); m_sockData = INVALID_SOCKET;  }

3) 数据接收

网络Sockets数据是异步传送的，且使用的接收函数recv()是阻塞式的，即在没有接收事件发生（正常接收到数据、发生错误、接收超时、……）前，该函数是不返回的，所以我把数据接收放在一个专门的接收线程里。

UINT CURCommDlg::Thread_RecvData(PVOID pParam){ CURCommDlg* pDlg = (CURCommDlg*)pParam; int nLen = 1024; char* recvBuf = new char[nLen]; memset(recvBuf, 0, nLen); while (pDlg->m_bThread) { // //接收数据 int ret = recv(pDlg->m_sockData, recvBuf, nLen, 0); if (ret>0) { pDlg->OnRecvData(recvBuf, ret); } else { //recv error pDlg->m_nError = WSAGetLastError(); closesocket(pDlg->m_sockData); break; } } delete[]recvBuf; pDlg->m_bThread = false; return 1;}

4) 数据内容解析

以上网络接收到的数据，发送到OnRecvData函数中进行数据解析。
这里按照UR机器人的30003端口返回数据格式进行解析

void CURCommDlg::OnRecvData(char* pData, int nLen){ DWORD dwPackLen; dwPackLen = GetDword((PBYTE)pData); double data1[54]; int n; for (n = 0; n < 54; n++) { data1[n] = GetDouble((PBYTE)(pData + 12 + n * 8)); } double data2[30]; for (n = 0; n < 30; n++) { data2[n] = GetDouble((PBYTE)(pData + 444 + n * 8)); } for (n = 0; n < 6; n++) m_dCurPos[n] = data2[n];}

其中，由于UR返回数据为Big-Endian，而计算机中的数据为Little-Endian，必须进行数据字节转换，所以编了以下两个函数完成，实际上以下的GetDword函数和htonl()函数功能一样。

double GetDouble(PBYTE pData){ double t; PBYTE p = (PBYTE)&t; int i; for (i = 0; i < 8; i++) { p[i] = pData[7 - i]; } return t;}
DWORD GetDword(PBYTE pData){ DWORD t; PBYTE p = (PBYTE)&t; int i; for (i = 0; i < 4; i++) { p[i] = pData[3 - i]; } return t;}

解析出来的数据就可以用于程序的其他用途了，例如以上解析函数中的m_dCurPos[6]就是机器人的当前实时位姿的六个数据。

使用30003端口来向机器人发送URScript脚本控制命令，并通过该端口接收实时返回数据。

5) 返回数据包频率和长度

由于30003端口返回的信息是最全的，包含了30001、30002端口的返回信息，因此，这里以30003端口信息解析。
30003端口返回数据频率一般是125Hz，即每8ms即返回一个数据包，最高频率能达到500Hz。
个人实测，返回数据包的字节长度一般是1108字节，按照文献1数据包是1044字节，文献2数据包是1108字节，分析可能UR后来扩充了UR返回数据信息。
实际上，这个字节规定是多少并不影响使用，因为在返回的数据包的前四个字节，即给出了数据包中包含信息的字节数。

6) 返回数据包格式

下表为30003实时反馈端口机器人信息1044字节数据格式，来自参考文献1。

字节顺序	内容
1-4	整个数据包的字节数
5-12	控制器通电时间，断电清零
13-444	关节目标位置、速度、加速度、电流、扭矩，实际位置、速度、电流、控制电流
445-684	TCP位置、速度、力，0目标位置、速度
685-692	输入位状态
693-740	电机温度
740-748	程序扫描时间
749-756	保留
757-820	机器人模式，关节模式，安全模式
821-868	保留
869-892	TCP加速度
893-940	保留
941-948	速度比例
949-956	机器人当前动量值
957-972	保留
973-996	控制板电压，机器人电压，机器人电流
997-1044	关节电压

下图为1108字节返回数据具体的各字节包含数据格式，来自参考文献2。

编写了TCP/IP程序控制UR机器人，并解析其在30003端口返回的数据包，实测结果：
每个数据包收到1108字节数据。
数据解析时要注意，收到数据包的每个数据的字节顺序是Big-Endian，即高位在前，而计算机中的字节顺序是Little-Endian，即低位在前，要注意进行字节顺序转换。
下图为接收到的数据包的一部分

以几个主要的数据解析为例：

• 2.466446,-0.586911,1.581819,-2.725837,4.662427,-0.580726；

• 0.405443,-0.164387,0.030460,2.077079,-2.308484,0.256067。
  收到数据与UR示教器上显示数据比对一致。

7) 参考文献

https://wenku.baidu.com/view/c78aa35c0722192e4436f61c.html
https://blog.csdn.net/seing128/article/details/89713207

https://blog.csdn.net/hangl_ciom/article/details/97612246

https://blog.csdn.net/hangl_ciom/article/details/104439042