CExample

The following example is a simple implementation of TCP/IP socket communication with the Meca500 robot in TCP/IP on Windows system


socket_comm.h/.c contains all the socket/TCP-IP/Communication part of this example


socket_comm.h

#include#include#include//目前只保留socket,但可以添加更多内容//当程序需要更多功能时。typedef struct{SOCKET SOCKET}tcpip\u connection;///连接TCP套接字。处理windows init socket///返回0表示成功,返回1表示失败。int tcpip_connect(const char*address,const char*port,tcpip_connection*connection);///断开连接void tcpip_Disconnect(tcpip_connection*connection);///将数据发送到缓冲区///如果成功返回0,如果失败返回1,如果整个数据不适合TCP缓冲区,则返回实际发送的大小//。在这种情况下,您应该///发送剩余的。int tcpip_send(const char*buffer,int len,tcpip_connection*connection);///尝试接收数据的len字节。///对于关闭的连接,返回0;对于失败,返回1;或者对于接收到的数据,返回nb。int tcpip\u receive(字符*缓冲区,int len,tcpip\u连接*连接);


socket_comm.c

#include#include#include“socket_comm.h”#define DEBUG 0//保留要初始化/关闭库的用户计数。static int socket\u system\u user\u count=0;//windows套接字所需的WSData。static WSADATA WSADATA;///用于在套接字系统未初始化时退出函数的实用程序。#如果(socket\u SYSTEM\u user\u count==0){return-1;}///需要时初始化套接字库,则在\u SYSTEM\u NOT \u INITED()上定义EXIT\u。int init_socket_system(){if(socket_system_user_count==0){printf(“init WSA\r\n”);int result=WSAStartup(MAKEWORD(2,2),&wsaData);if(result!=0){printf(“WSAStartup失败,错误:%d\r\n”,result);return-1;}}socket\u system\u user\u count++;return 0;}///如果没有其他用户,请关闭socket库。void cleanup_socket_system(){socket_system_user_count--;if(socket_system_user_count==0){printf(“Closing WSA\r\n”);WSACleanup();}//=====================================================================================//int tcpip_connect(const char*address,const char*port,tcpip_connection*connection){int result=0;result=init_socket_system();if(结果!=0){return-1;}struct addrinfo hints,*addr\u result;ZeroMemory(&hints,sizeof(hints));艾乌家族=afu unsec;提示.ai\u socktype=短波流;hints.ai\u协议=IPPROTO\u TCP;//获取连接结果的地址信息=getaddrinfo(address,port,&hints,&addr\u result);if(result!=0){printf(“getaddrinfo failed with error:%d\r\n”,result);cleanup\u socket\u system();return-1;}//创建用于连接到服务器连接的套接字->socket\u=socket(addr\u result->ai\u family,addr\u result->ai\u socktype,addr\u result->ai\u protocol);if(connection->socket\u==INVALID\r\n){printf(“socket failed with error:%ld”,WSAGetLastError());cleanup_socket_system();return-1;}//连接到服务器。result=connect(connection->socket,addr\u result->ai\u addr,(int)addr\u result->ai\u addrlen);if(result==socket\u ERROR){closesocket(connection->socket\u);cleanup\u socket\u system();connection->socket\u result=INVALID\u socket;return-1;}//完成套接字创建。freeaddrinfo(addr\u result);返回0;}//=====================================================================================//void tcpip\u disconnect(tcpip\u connection*connection){if(socket\u system\u user\u count==0){return;}closesocket(connection->socket\u);cleanu socket\u system();}//=======================================================================================================//int tcpip\u send(const char*buffer,int len,tcpip\u connection*connection){退出\u SYSTEM\u NOT \u INITED();//发送初始缓冲区#ifdef DEBUG printf(“发送:%s\r\n”,buffer);#endif int send\u result=send(connection->socket,buffer,len,0);如果(send_result==SOCKET_ERROR){printf(“send failed with ERROR:%d\r\n”,WSAGetLastError());closesocket(connection->SOCKET_);cleanup_SOCKET_system();return-1;}//未发送所有内容,则返回已发送的数据量。如果(发送结果!=len){return send\u result;}//所有内容都已发送并返回0;}//=====================================================================================//int tcpip\u receive(char*buffer,int len,tcpip\u connection*connection){退出\u SYSTEM\u NOT INITED();int recv\u result=recv(connection->socket\u,buffer,len,0);if(recv\u result==0)//关闭连接案例{printf(“connection closed\r\n”);closesocket(connection->socket};cleanup}socket|system();return 0;}else if(recv|result<0)//错误案例{printf(“recv failed with Error:%d\r\n”,WSAGetLastError());return-1;}ifdef DEBUG printf(“Receiving:%s\r\n,buffer);#endif//包含接收的数据量。返回recv_result;}


robot_interface.h/.c contains the code that handles sending the robot the correct command

and parsing the return messages

robot_interface.h

# include“socket_comm。h”/ /包含信息;n about a robot's connection. // Members should not be access individually typedef struct { tcpip_connection robot_connection_; int connected_; }Meca500; /// Connect to a meca500 to the control port int connect_robot(char* address, Meca500 *robot); /// Disconnect a meca500 int disconnect_robot(Meca500 *robot); /// Wait for a specified return code. /// Return 0 if code was received. /// Return 1 if an error code ( between 1000 and 2000) was received /// Return -1 if there was a network error. int wait_for_return_code(int code, Meca500 *robot); /// Wait for End of Block flag. /// Return 0 if code was received. /// Return 1 if an error code ( between 1000 and 2000) was received /// Return -1 if there was a network error. int wait_for_EOB(Meca500 *robot); /// Send an ActivateRobot command and wait for it to be completed. /// return 0 for success /// return -1 for failure int activate_robot(Meca500 *robot); /// Send an DeactivateRobot command and wait for it to be completed. /// return 0 for success /// return -1 for failure int deactivate_robot(Meca500 *robot); /// Send an activatesim command and wait for it to be completed. /// return 0 for success /// return -1 for failure int activate_sim(Meca500 *robot); /// Send an deactivatesim command and wait for it to be completed. /// return 0 for success /// return -1 for failure int deactivate_sim(Meca500 *robot); /// Send an home command and wait for it to be completed. /// return 0 for success /// return -1 for failure int home(Meca500 *robot); /// Sends a MoveJoint command. Do not wait for completion. /// return 0 for success /// return -1 for failure int movejoints(Meca500 *robot, float joints[6]); /// Sends a MovePose command. Do not wait for completion. /// return 0 for success /// return -1 for failure int movepose(Meca500 *robot, float euler[6]); /// Sends a MoveJoint command to go to zero. Do not wait for completion. /// return 0 for success /// return -1 for failure int movetozero(Meca500 *robot); /// Sends a MoveJoint command for the shipping position. Do not wait for completion. /// return 0 for success /// return -1 for failure int movetoshipping(Meca500 *robot);


robot_interface.c

#include“robot_interface.h”#include“stdio.h”#include“stdlib.h”#include“string.h”///meca500静态常量char*control_port=“10000”的控制端口;///如果未连接robot,则退出函数的实用程序#define exit ON robot not connected(connected \u flag)\if(connected \u flag==0)\{\printf(“robot not connected.\r\n”);\return-1;\}//============================================================================================================================///从消息中提取代码(“[3000][…”->3000”)并作为int返回。///如果找不到,则返回0;int Extract \u code(const char*message){int code=0;char*first \u bracket=NULL;char*second \u bracket=NULL;do{first \u bracket=strhr(message,[');第二个括号=strhr(消息,']');if((int)(第二个括号-第一个括号)==5){code=1000*(第一个括号[1]-'0')+100*(第一个括号[2]-'0')+10*(第一个括号[3]-'0')+1*(第一个括号[4]-'0');}}while(code==0 | |第一个括号==NULL | |第二个括号==NULL);返回代码;}=====================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================返回返回返回返回返回返回代码代码代码(int代码代码,国际代码,国际代码,国际代码,METACOCOCOCOCOCOCOCOCOCOCOCOCOCOCOCOCOCOCOCOCOCOCOU,MEP,MEP,MECADEDEDEDEP,MEMACOCOCOCOCOCOCO如果(recv\u result<0){return-1;}code\u received=extract\u code(recv\u buffer);found\u requested\u code=(code\u received==code);found\u error\u code=code\u received>=1000&&code\u received<2000;}while(!找到\u请求的\u代码&&!found\u error\u code);if(found\u error\u code){return 1;}if(found\u requested\u code){return 0;}//不应出现在此处。。。=============================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================================//int connect \u robot(char*address,Meca500*robot){int result=0;//连接到好的端口结果=tcpip\u connect(address,CONTROL\u port,&robot->robot\u connection);if(result==-1){printf(“robot connection failed on network error.\r\n”);return-1;}//等待robot返回数据。char recv_buffer[512]={0};result=tcpip_receive(recv_buffer,512,&robot->robot_connection);if(result==-1){printf(“robot connection failed on network error(recv)”;return-1;}else if(result==0){printf(“robot connection failed because robot closed the connection.\r\n”);return-1;}//检查连接是否已确认从机器人那里。char*search_result=strstrstr(recv_buffer,“[3000]”);if(search_result==NULL){printf(“Robot连接失败,因为Robot不接受控制。\r\n”);printf(“%s\r\n”,recv_buffer);return-1;}Robot->connected_=1;printf(“连接到Meca500的地址为%s\r\n”,address);return 0;}//=================================================================================================//int disconnect\u robot(Meca500*robot){tcpip\u disconnect(&robot->robot\u connection);robot->connected\u0;}//==============================================================================================//int activate\u robot(Meca500*robot){退出未连接的robot\u(robot->CONNECTED);char*msg=“ActivateRobot\0”;tcpip\u send(msg,strlen(msg)+1,&robot->robot\u connection);int result=wait\u等待返回代码(2000,robot);if(result!=0){return-1;}return 0;}//===============================================================================//int deactivate\u robot(Meca500*robot){退出未连接的\u robot(robot->CONNECTED);char*msg=“DeactivateRobot\0”;tcpip\u send(msg,strlen(msg)+1,&robot->robot\u connection);int result=wait\u for\u return\u code(2004,robot);如果(结果)!=0){return-1;}return 0;}//===============================================================================//int activate\u sim(Meca500*robot){//退出未连接的robot\u(robot->CONNECTED);char*msg=“ActivateSim\0”;int result=tcpip\u send(msg,strlen(msg)+1,&robot->robot->robot\u connection);if(result!=0){return-1;}result=等待返回代码(2045,robot);if(result!=0){return-1;}return 0;}//===============================================================================//int deactivate\u sim(Meca500*robot){退出未连接的robot\u(robot->CONNECTED);char*msg=“DeactivateSim\0”;int result=tcpip\u send(msg,strlen(msg)+1,&robot->robot\u connection);if(result!=0){return-1;}result=等待返回代码(2046,robot);if(result!=0){return-1;}return 0;}//==================================================================================//int home(Meca500*robot){在未连接的robot上退出(robot->CONNECTED);char*msg=“home\0”;int result=tcpip\u send(msg,strlen(msg)+1,&robot->robot\u connection);if(result!=0){return-1;}result=等待返回代码(2002,robot);if(result!=0){return-1;}返回0;}//============================================================================= // int movejoints(Meca500 *robot, float joints[6]) { EXIT_ON_ROBOT_NOT_CONNECTED(robot->connected_); char msg[100] = {0}; sprintf(msg, "MoveJoints(%10.4f, %10.4f, %10.4f, %10.4f, %10.4f, %10.4f)\0", joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]); int result = tcpip_send(msg, strlen(msg) + 1, &robot->robot_connection_); if (result != 0) { return -1; } return 0; } //============================================================================= // int movepose(Meca500 *robot, float euler[6]) { EXIT_ON_ROBOT_NOT_CONNECTED(robot->connected_); char msg[100] = {0}; sprintf(msg, "MovePose(%10.4f, %10.4f, %10.4f, %10.4f, %10.4f, %10.4f)\0", euler[0], euler[1], euler[2], euler[3], euler[4], euler[5]); int result = tcpip_send(msg, strlen(msg) + 1, &robot->robot_connection_); if (result != 0) { return -1; } return 0; } //============================================================================= // int movetozero(Meca500 *robot) { float joints[6] = {0}; return movejoints(robot, joints); } //============================================================================= // int movetoshipping(Meca500 *robot) { float joints[6] = {0, -60, 60, 0, 0, 0}; return movejoints(robot, joints); }


主cis the main program that calls the function from the robot's interface.


主c

#define WIN32_LEAN_AND_MEAN #include  #include  #include "robot_interface.h" // Need to link with Ws2_32.lib, Mswsock.lib, and Advapi32.lib #pragma comment (lib, "Ws2_32.lib") #pragma comment (lib, "Mswsock.lib") #pragma comment (lib, "AdvApi32.lib") /// Little example to show usage of the Meca500 in C /// This example Connect to the robot, activate the simulation, /// Activate/home the robot and do some movement. /// Then, it deactivate the robot and the simulation and disconnect. int main(int argc, char **argv) { Meca500 robot; // Connect int result = connect_robot("192.168.0.100", &robot); if( result != 0) { return -1; } /// For visual clarity, will no check for the return value of the function. activate_sim(&robot); activate_robot(&robot); home(&robot); float pose_1 [6] = {250,0,250,0,90,0}; float pose_2 [6] = {150,0,150,180,00,180}; movepose(&robot, pose_1); movepose(&robot, pose_2); movetozero(&robot); movetoshipping(&robot); /// Movement function to not wait for completion before returning from their call /// So we wait eob here. wait_for_EOB(&robot); deactivate_robot(&robot); deactivate_sim(&robot); disconnect_robot(&robot); return 0; }



你觉得有用吗?Yes

发送反馈
Sorry we couldn't be helpful. Help us improve this article with your feedback.