EPSON机械臂TCP通讯，实现手眼标定(附EPSON代码)

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/Yong_Qi2015/article/details/86601878

	' 建立TCP_IP连接，机械手接收相机发送过来的数据从而到达对应的位置
	' 初始位置，查看标定板摆放位置是否正确

	Global String Camera_X$, Camera_Y$, Camera_Z$, Camera_U$, Camera_V$, Camera_W$
    ' 接收字符串
    Global String receiveCommond1$  '定义字符串变量
   
	Function main
		
	   '调试程序	
	   Call init '调用初始化
	   
	   Call NetWorldConnect '开始建立连接
	   
	   Do
	   Call ReceiveData '接收数据
	'  Print "相机发送的坐标系:", "X的值:", Camera_X$, "Y的值:", Camera_Y$, "Z的值:", Camera_Z$, "U的值:", Camera_U$, "V的值:", Camera_V$, "W的值:", Camera_W$
	   'Go XY(Val(Camera_X$), Val(Camera_Y$), Val(Camera_Z$), Val(Camera_U$), Val(Camera_V$), Val(Camera_W$))  '相机发的位置
	  
	  Loop
	 
    Fend
        Function init '初始化
    	Motor On
    	Power High
    	Speed 10
    	Accel 10, 10
       ' P000 = JA(0, 0, 0, 0, 0, 0)
       P000 = JA(58.086, 0, 0, 0, 0, 0)
    	Go P000 '回到起始点
    Fend
        Function NetWorldConnect '建立TCP连接
        	
    	SetNet #201, "192.168.0.1", 5000, CRLF, NONE, 0 '设置的服务器地址
    	
    	OpenNet #201 As Server '端口号对应pc机的端口号，如果控制器作为服务器，则201作为Server
    	
    	Print "等待连接"
    	
    	Print "201" + "端口已连接"
    	
    Fend
  Function ReceiveData '接收数据
    	AA: Integer NumChars '接收的字符个数
    	NumChars = ChkNet(201) '检查网络端口返回的字符个数
    	Print "接收数据数", NumChars
        If NumChars >= 0 Then '判断数据个数是否大于等于0
            Input #201, receiveCommond1$
            Print receiveCommond1$
            ' 接收到的是欧拉角
    	    If receiveCommond1$ = "Pos1" Then
			    Input #201, Camera_X$, Camera_Y$, Camera_Z$, Camera_U$, Camera_V$, Camera_W$
				Go XY(Val(Camera_X$), Val(Camera_Y$), Val(Camera_Z$), Val(Camera_U$), Val(Camera_V$), Val(Camera_W$))  '相机发的位置（关节角）
			     Print "EulerEnd"
			     Print #201, "END" '发送数据，向外发送   	     
    	    EndIf
            
            ' 接收到的是关节角
            If receiveCommond1$ = "Pos" Then
			    Input #201, Camera_X$, Camera_Y$, Camera_Z$, Camera_U$, Camera_V$, Camera_W$
				Go JA(Val(Camera_X$), Val(Camera_Y$), Val(Camera_Z$), Val(Camera_U$), Val(Camera_V$), Val(Camera_W$))  '相机发的位置（关节角）
			     Print "PosEnd"
			     Print #201, "END" '发送数据，向外发送   	     
    	    EndIf
    	    
    	 
    	    ' 发送当前的欧拉角及位姿
	        If receiveCommond1$ = "GetEulerPos" Then
		       ' 读取当前点的位姿
	             P002 = CurPos
	             Print CX(P002), CY(P002), CZ(P002), CU(P002), CV(P002), CW(P002)
	             Print #201, CX(P002), CY(P002), CZ(P002), CU(P002), CV(P002), CW(P002) '发送数据，向外发送
	        Print "GetEulerPosEnd"
	        EndIf
	        
	        
	        ' 发送当前的关节角
	        If receiveCommond1$ = "GetAxisPos" Then
		    ' 读取当前点的位姿
		    Print Agl(1), Agl(2), Agl(3), Agl(4), Agl(5), Agl(6)
		    Print #201, Agl(1), Agl(2), Agl(3), Agl(4), Agl(5), Agl(6)
	        Print "GetAxisPosEnd"
	        EndIf
	        
	        
	        ' 机器人接收到Start命令，则归位到指定位置
	        If receiveCommond1$ = "Start" Then
	        P002 = JA(58.086, 0, 0, 0, 0, 0)
	        Go P002
	        Print "StartEnd"
	        Print #201, "END"
	        EndIf
	        
	        
	        ' 设置速度为high
	        If receiveCommond1$ = "High" Then
		    ' 读取当前点的位姿
		    Speed 30
            Accel 30, 30
	        Print "Current speed is High"
	        Print #201, "END"
	        EndIf
	        
	        ' 设置速度为middle
	        If receiveCommond1$ = "Middle" Then
		    ' 读取当前点的位姿
		    Speed 20
            Accel 20, 20
	        Print "Current speed is Middle"
	        Print #201, "END"
	        EndIf
	        
	        ' 设置速度为Low
	        If receiveCommond1$ = "Low" Then
		    ' 读取当前点的位姿
		    Speed 10
            Accel 10, 10
	        Print "Current speed is Low"
	        Print #201, "END"
	        EndIf
    		Else
    			CloseNet #201 '关闭OpenNet打开的TCP/IP 201端口
    			OpenNet #201 As Server
    			Print "等待连接"
    			WaitNet #201
    			Print "连接成功"
    			GoTo AA
    	EndIf
    Fend
    

以上仅仅是EPSON端的实现代码，如果需要正常使用的话，仍需要配合C++的实现代码。

#include "Epson_robot.h"

#pragma comment(lib , "ws2_32.lib")

Epson::Epson() :
	host_(NULL)
{

}

Epson::~Epson()
{

}


bool Epson::Connect(const char * port, const char * ip)
{
	/*加载Winsock DLL*/
	if (WSAStartup(MAKEWORD(2, 2), &wsd_) != 0) {
		printf("Winsock    初始化失败!\n");
		WSACleanup();//释放套接字资源
		return false;
	}

	/*创建Socket*/
	sClient_ = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (sClient_ == INVALID_SOCKET) {
		printf("socket() 失败: %d\n", WSAGetLastError());
		WSACleanup();//释放套接字资源
		return false;
	}
	/*指定服务器地址*/
	server_.sin_family = AF_INET;
	port_ = atoi(port);
	server_.sin_port = htons(port_);
	server_.sin_addr.s_addr = inet_addr(ip);

	/*与服务器建立连接*/
	if (connect(sClient_, (struct sockaddr*)&server_,
		sizeof(SOCKADDR)) == SOCKET_ERROR) {
		printf("connect() 失败: %d\n", WSAGetLastError());
		WSACleanup();//释放套接字资源
		return false;
	}
	return true;
}


bool Epson::WritePose(EulerPose pose)
{
	char buffer[256] = { 0 };
	sprintf(buffer, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n",
		pose.Tx, pose.Ty, pose.Tz, pose.R1, pose.R2, pose.R3);

	char sent_string[256];
	sprintf(sent_string, "%s,\r\n", "Pos1");
	ret_ = send(sClient_, sent_string, strlen(sent_string), 0);
	Sleep(20);

	//向服务器发送消息
	ret_ = send(sClient_, buffer, strlen(buffer), 0);
	if (ret_ == 0) {
		return false;
	}
	else if (ret_ == SOCKET_ERROR) {
		printf("%d  send() 失败: %d\n", WSAGetLastError());
		WSACleanup();//释放套接字资源
		return false;
	}
	printf("Send %s\n", buffer);
	//判断机械手是否接收到数据
	char receive_buffer[BUFSIZE] = "\0";
	ret_ = recv(sClient_, receive_buffer, BUFSIZE, 0);
	std::string  receive_buffer_str = std::string(receive_buffer);
	if (receive_buffer_str != "END\r\n")
	{
		return false;
	}
	return true;
}


bool Epson::WritePose(AxisPose pose)
{
	char buffer[256] = { 0 };
	sprintf(buffer, "%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n",
		pose.A1, pose.A2, pose.A3, pose.A4, pose.A5, pose.A6);
	//向服务器发送消息

	char sent_string[256];
	sprintf(sent_string, "%s,\r\n", "Pos");
	ret_ = send(sClient_, sent_string, strlen(sent_string), 0);
	Sleep(20);
	ret_ = send(sClient_, buffer, strlen(buffer), 0);
	//ret_ = send(sClient_, "Image;X:377.9677;Y:0;Z:450s;A:0;B;90;C:180;Done$R$N", strlen("Image;X:377.9677;Y:0;Z:500;A:0;B;90;C:180;Done$R$N"), 0); 
	if (ret_ == 0) {
		return false;
	}
	else if (ret_ == SOCKET_ERROR) {
		printf("%d  send() 失败: %d\n", WSAGetLastError());
		WSACleanup();//释放套接字资源
		return false;
	}
	 printf("Send %s\n", buffer);

	 //判断机械手是否接收到数据
    char receive_buffer[BUFSIZE] = "\0";
	ret_ = recv(sClient_, receive_buffer, BUFSIZE, 0);
	std::string  receive_buffer_str = std::string(receive_buffer);
	if (receive_buffer_str != "END\r\n")
	{
		return false;
	}
	return true;
}


EulerPose Epson::ReadPose()
{
	EulerPose pose;
	pose.eulerT = XS_ZYX;
	char receive_buffer[BUFSIZE] = "\0";

	char sent_string[256];
	sprintf(sent_string, "%s,\r\n","GetEulerPos");
	ret_ = send(sClient_, sent_string, strlen(sent_string), 0);
	Sleep(20);
	//接收从服务器来的消息
	ret_ = recv(sClient_, receive_buffer, BUFSIZE, 0);
	EulerPose pose0(0, 0, 0, 0, 0, 0);
	if (ret_ == 0) {
		return pose0;
	}
	else if (ret_ == SOCKET_ERROR) {
		printf("recv() 失败: %d\n", WSAGetLastError());
		WSACleanup();//释放套接字资源           
		return pose0;
	}
	std::cout << receive_buffer << std::endl;
	sscanf_s(receive_buffer, " %f %f %f %f %f %f\r\n", &pose.Tx,&pose.Ty, &pose.Tz, &pose.R1, &pose.R2, &pose.R3);
	return pose;
}


AxisPose Epson::ReadAxisPose()
{
	AxisPose pose;
	char receive_buffer[BUFSIZE] = "\0";

	char sent_string[256];
	sprintf(sent_string, "%s,\r\n", "GetAxisPos");
	ret_ = send(sClient_, sent_string, strlen(sent_string), 0);
	Sleep(20);

	//接收从服务器来的消息
	ret_ = recv(sClient_, receive_buffer, BUFSIZE, 0);
	AxisPose pose0(0, 0, 0, 0, 0, 0);
	if (ret_ == 0) {
		return pose0;
	}
	else if (ret_ == SOCKET_ERROR) {
		printf("recv() 失败: %d\n", WSAGetLastError());
		WSACleanup();//释放套接字资源           
		return pose0;
	}
	sscanf_s(receive_buffer, "%f %f %f %f %f %f\r\n", &pose.A1, &pose.A2, &pose.A3, &pose.A4, &pose.A5, &pose.A6);

	return pose;
}

bool Epson::SetSpeed(RobotSpeed speed)
{
	std::string command_str = "\0";
	bool is_has_command = false;
	switch (speed)
	{
	case HIGH:
		command_str = "High";
		is_has_command = true;
		break;
	case MIDDLE:
		command_str = "Middle";
		is_has_command = true;
		break;
	case LOW:
		command_str = "Low";
		is_has_command = true;
		break;
	default:
		break;
	}
	if (!is_has_command)
	{
		return false;
	}
	char sent_string[256];
	sprintf(sent_string, "%s,\r\n", command_str.c_str());
	//向服务器发送消息
	ret_ = send(sClient_, sent_string, strlen(sent_string), 0);

	if (ret_ == 0) {
		return false;
	}
	else if (ret_ == SOCKET_ERROR) {
		printf("send() 失败: %d\n", WSAGetLastError());
		WSACleanup();//释放套接字资源
		return false;
	}
	printf("Send %d bytes\n", ret_);
	Sleep(20);
	char receive_buffer[BUFSIZE] = "\0";
	ret_ = recv(sClient_, receive_buffer, BUFSIZE, 0);
	if (std::string(receive_buffer) != "END\r\n")
	{
		return false;
	}
	return true;
}


bool Epson::WriteCommand(RobotCommand command)
{
	std::string command_str = "\0";

	bool is_has_command = false;

	switch (command)
	{
	case SCEND_SATRT:
		command_str = "Start";
		is_has_command = true;
		break;
	case SECND_SPEAD:
		command_str = "Spead";
		is_has_command = true;
		break;
	default:
		break;
	}
	if (!is_has_command)
	{
		return false;
	}
	char sent_string[256];
	sprintf(sent_string, "%s,\r\n", command_str.c_str());

	//向服务器发送消息
	ret_ = send(sClient_, sent_string, strlen(sent_string), 0);
	Sleep(20);
	if (ret_ == 0) {
		return false;
	}
	else if (ret_ == SOCKET_ERROR) {
		printf("send() 失败: %d\n", WSAGetLastError());
		WSACleanup();//释放套接字资源
		return false;
	}
	printf("Send %d bytes\n", ret_);
	//判断机械手是否接收到数据
	char receive_buffer[BUFSIZE] = "\0";
	ret_ = recv(sClient_, receive_buffer, BUFSIZE, 0);
	std::string  receive_buffer_str = std::string(receive_buffer);
	if (receive_buffer_str != "END\r\n")
	{
		return false;
	}
	return true;

}

void Epson::Disconnect()
{
	//用完了，关闭socket句柄(文件描述符)
	closesocket(sClient_);
	WSACleanup();    //清理
}

个人公众号：视觉IMAX。
知识星球：视觉IMAX。