键盘控制UR3机械臂运动的小Demo

**

键盘控制UR3机械臂运动的小Demo

**

这一段时间研究机械臂搭配机械手的相关问题。目前只做到了同通过键盘来简单的操作机械臂的移动，利用的是机械臂的movep指令的移动方式。下面来让我们一起看代码。转载请注明出处。

这里是包含的相关的头文件

#include <WINSOCK2.H>   
#include <stdio.h>     
#include "string"
#include <WS2tcpip.h>
#include <iostream>
#include <sstream>
#include <conio.h>

下面是建立socket通信的代码

using namespace std;     
#define SERVER_ADDRESS "192.168.3.22" //服务器端IP地址      
#define PORT           30003         //服务器的端口号      
#define MSGSIZE        1024         //收发缓冲区的大小      
#pragma comment(lib, "ws2_32.lib") 
#define UR3X  0.37000     //设定的机械臂移动的初始位置
#define UR3Y  -0.01000
#define UR3Z   0.05000
SOCKET sClient; //连接所用套节字 

int Socket_Connect();
void Move_UR3( float x, float y, float z);
void Init();

/*
	返回值0：表示连接成功 非0表示连接失败
*/

int Socket_Connect() {
	WSADATA wsaData;
	
	//保存远程服务器的地址信息      
	SOCKADDR_IN server;
	//收发缓冲区      
	     
	int ret;
	struct in6_addr  s; // IPv4地址结构体
	// Initialize Windows socket library      
	WSAStartup(0x0202, &wsaData);

	// 创建客户端套节字      
	sClient = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);       							
	memset(&server, 0, sizeof(SOCKADDR_IN));  
	server.sin_family = PF_INET;  
	server.sin_port = htons(PORT); 
	//server.sin_addr.s_addr = inet_pton(AF_INET,SERVER_ADDRESS,&foo.sin_addr);
	server.sin_addr.s_addr = inet_addr(SERVER_ADDRESS);												
	int rt = connect(sClient, (struct sockaddr*) & server, sizeof(SOCKADDR_IN));    
	return rt;													
/*
	传入的是机械臂的三个坐标，来控制机械臂的移动
*/
void Move_UR3(float x, float y, float z) {
	if (Socket_Connect() != 0) {
		printf("连接机械臂失败了");
		return ;
	}
	char szMessage[MSGSIZE];
	//成功接收字节的个数 
	ostringstream osx,osy,osz;
	osx <<x;    //将float类型转换成string类型，方便能够将命令传送给机械臂
	osy << y;
	osz << z;
	string s = osx.str();

	string str1 = "movel(p[";
	string str2 = osx.str();
	str2.append(",");
	string str3 = osy.str();
	str3.append(",");
	string str4 = osz.str();
	str4.append(",");
	string str5 = "3.1044, -0.1246, -0.0128], a = 0.5, v = 1.2)\n";
	string str = str1 + str2 + str3 + str4 + str5;
	for (int i = 0;i < str.size();i++) {
		szMessage[i] = str[i];
	}
	for (int i = 0;i < str.size();i++) {
		printf("%c", szMessage[i]);
	}
	send(sClient, szMessage, strlen(szMessage), 0);

}
/*
	这个是机械臂的初始化到一个固定的位置
*/
void Init() {
	Move_UR3(UR3X, UR3Y, UR3Z);
}

下面是主函数 （运行效果是上 下 左 右 pgup pgdown六个键对应着机械臂xyz方向上的移动）。

int main()
{
	int keypress;
	float x=0.0, y=0.0, z=0.0;
	x = x + UR3X;
	y = y + UR3Y;
	z = z + UR3Z;
	Move_UR3(UR3X, UR3Y, UR3Z);
	while (true) {
		keypress = getch();
		if (keypress == 72) {
			cout << "向上移动" << endl;
			z = z + 0.01;
			Move_UR3(x, y, z);
		}
		else if (keypress == 80) {
			cout << "向下移动" << endl;
			z = z - 0.01;
			Move_UR3(x, y, z);
		}
		else if (keypress == 75) {
			cout << "向左移动" << endl;
			y = y + 0.01;
			Move_UR3(x, y, z);
		}
		else if (keypress == 77) {
			cout << "向右移动" << endl;
			y = y - 0.01;
			Move_UR3(x, y, z);
		}
		else if (keypress == 73) {
			cout << "向前移动" << endl;
			x = x + 0.01;
			Move_UR3(x, y, z);
		}
		else if (keypress == 81) {
			cout << "向后移动" << endl;
			x = x - 0.01;
			Move_UR3(x, y, z);
		}

	}
	
}