安川机器人之motoPlus的编程与导入

xx_233

于 2024-09-02 12:48:03 发布

阅读量570

点赞数 8

文章标签：机器人

本文链接：https://blog.csdn.net/xx_233/article/details/141815532

版权

由于网上关于安川机器人的motoPlus二次开发实在太少，本人最近由于项目原因接触并学习到了一点，仅此做个分享，有问题可以交流。

一、motoPlus编译文件的导入与使用

一般用的比较多的控制柜是DX200系列跟YRC1000系列，motoPlus软件与加密狗需要购买或者找公司相关人员借用。安川示教器本身不具备用户界面功能，程序调试十分麻烦，在使用motoPlus编译的.out文件导入时，需要先长按示教器上的“MainMenu”按钮，再开启控制柜进入维护模式，在维护模式下，需要在系统中将安全模式转换到管理模式。

管理模式下，在系统设置中，进入选项功能，将motoPlus功能开启，如果后续需要TCP网络服务功能的，可以在这也开启一下，不过开启网络服务需要加密狗权限，并且进入别的模式，本人不是安川员工，这一块可以去咨询相关人员。

开启motoPlus功能后，可以在主菜单的motoPlus应用中，将装置选择为示教器USB或者控制柜USB，将含有编译文件的U盘进行读取，点击motoPlus应用的安装可以进行安装。注意，控制柜中的motoPlus文件只能存在一个，同名的可以在安装时提示是否覆盖，不同名的只能先删除前一个再进行安装。

重点的来了，在安装完编译文件后，需要在motoPlus应用里进行额外的设置，后台电源自动运行以及资源权限的设置。安川机器人控制柜的motoPlus编译文件是重启控制柜后，后台自动运行的，在运行时，如果程序存在错误，示教器会进行报错，但是不存在用户界面看到你打印的信息，只能通过对里面的变量或者IO进行读取或写入，十分难绷。所以在写主程序逻辑的时候需要注意如何去看到自己的调试过程，千万别以为motoPlus编译通过了就能完全运行。报错信息可以去查看相关的文档，一般来说都是变量、函数、符号等存在问题，一定要仔细检查。

二、motoPlus的编程

其实大多项目二次开发都是用于Socket通讯，与上位机软件交互机器人的当前信息，motoPlus是基于C语言重新开发的，提供了比较多的API接口，大家可以去安川电机的官网搜索motoPlus查看相关的教程与文档。这里我简单的提供几个自己写的函数，以供参考。需要注意的是，机器人关节角度与偏移位置的获取需要将反馈脉冲转换到演算脉冲，再转换到角度，角度再转换到偏移位置，并且单位都是0.0001deg。Socket通信与C语言本身的通信是基本一致的，大家可以在文档中进行查看，这里我也把自己使用的Socket代码贴出来。本人代码能力有限，大伙想优化的自己优化。

//传入数组，获取当前的角度与偏移值
void GetCurPos(float* CurJpos, float* CurPpos) { 
	//Jpos
	long pulse[MP_GRP_AXES_NUM];
	long angle[MP_GRP_AXES_NUM];
	int i, rc;
	MP_CTRL_GRP_SEND_DATA CurJposGrp;
	MP_FB_PULSE_POS_RSP_DATA CurJposData;
	CurJposGrp.sCtrlGrp = 0;
	memset(&CurJposData, 0, sizeof(CurJposData));
	mpGetFBPulsePos(&CurJposGrp, &CurJposData);
	int grp_no = mpCtrlGrpId2GrpNo(MP_R1_GID);
	memset(pulse, 0, sizeof(pulse));
	memset(angle, 0, sizeof(angle));
	rc = mpConvFBPulseToPulse(grp_no, CurJposData.lPos, pulse);
	rc = mpConvPulseToAngle(grp_no, pulse, angle);
	for (i = 0; i <= 5; ++i) {
		CurJpos[i] = angle[i]/10000;
	}
	//Ppos
	MP_COORD bas_coord;
	MP_PULSE_POS_RSP_DATA PlsData;
	memset(&bas_coord, 0, sizeof(bas_coord));
	rc = mpConvAxesToCartPos(grp_no, angle, 0, NULL, &bas_coord);
	CurPpos[0] = bas_coord.x / 1000;
	CurPpos[1] = bas_coord.y / 1000;
	CurPpos[2] = bas_coord.z / 1000;
	CurPpos[3] = bas_coord.rx / 10000;
	CurPpos[4] = bas_coord.ry / 10000;
	CurPpos[5] = bas_coord.rz / 10000;
}

//获取指定的IO状态
bool GetIO(int ioAddr)
{   
    int* signal;
	MP_IO_INFO nfo;
	nfo.ulAddr = ioAddr;
	int a = mpReadIO(&nfo, signal, 1);
	return (bool)(*signal);
}

//获取从指定的Int变量开始，num个int值，存入给定的数组中
void GetVarInt(int index, int num, int* result) {
	int i;
	MP_VAR_INFO sData[20];
	int rData[20];
	for (i = 0; i < num; ++i) {
		sData[i].usType = MP_RESTYPE_VAR_I;
		sData[i].usIndex = index + i;
	}
	int status = mpGetVarData(&sData, rData, num);
	for (i = 0; i < num; ++i) {
		result[i] = rData[i];
	}
}

//获取从指定的Real变量开始，num个float值，存入给定的数组中
void GetVarFlt(int index, int num, float* result) {
	int i;
	MP_VAR_INFO sData[20];
	float rData[20];
	for (i = 0; i < num; ++i) {
		sData[i].usType = MP_RESTYPE_VAR_R;
		sData[i].usIndex = index + i;
	}
	int status = mpGetVarData(&sData, rData, num);
	for (i = 0; i < num; ++i) {
		result[i] = rData[i];
	}
}

//将input数组中的值设置到从index开始的num个Int变量中，其中sData的大小自己可以设置
void SetVarInt(int index, int* input, int num) {
	MP_VAR_DATA sData[20];
	int i;
	for (i = 0; i < num; ++i) {
		sData[i].usType = MP_RESTYPE_VAR_I;
		sData[i].usIndex = index + i;
		sData[i].ulValue = input[i];
	}
	long status = mpPutVarData(&sData, num);
}

//将input数组中的值设置到从index开始的num个R变量中，其中sData的大小自己可以设置
void SetVarFlt(int index, float* input, int num) {
	MP_VAR_DATA sData[20];
	int i;
	for (i = 0; i < num; ++i) {
		sData[i].usType = MP_RESTYPE_VAR_R;
		sData[i].usIndex = index + i;
		sData[i].ulValue = input[i];
	}
	long status = mpPutVarData(&sData, num);
}

//设置一个value值到index编号的Int变量中
void SetOnlyInt(int index, int value) {
	MP_VAR_DATA sData;
	sData.usType = MP_RESTYPE_VAR_I;
	sData.usIndex = index;
	sData.ulValue = value;
	long status = mpPutVarData(&sData, 1);
}

//获取index编号的Int变量值
int GetOnlyInt(int index) {
	MP_VAR_INFO sData;
	sData.usType = MP_RESTYPE_VAR_I;
	sData.usIndex = index;
	int rData = 0;
	long status = mpGetVarData(&sData, &rData, 1);
	return rData;
}

Socket的使用如下，这里我使用循环去控制通讯的交互，再断连后可以重新连接，机器人做服务端。

//将服务端Socket定义全局，后续可以拿来重新连接，初始化socket，返回连接的客户端socket
int SeverSocket;
int TcpInit() {
	const int SeverPort = 8800;
	struct sockaddr_in  serverSockAddr;
	struct sockaddr_in cilentSockAddr;
	int status;
	int AcceptHandle;
	SeverSocket = mpSocket(AF_INET, SOCK_STREAM, 0);
	if (SeverSocket < 0) {
		printf("create socket fail\n");
		return -1;
	}
	memset(&cilentSockAddr, 0, sizeof(cilentSockAddr));	
	memset(&serverSockAddr, 0, sizeof(serverSockAddr));
	serverSockAddr.sin_family = AF_INET;
	serverSockAddr.sin_addr.s_addr = INADDR_ANY;
	serverSockAddr.sin_port = mpHtons(SeverPort);
	status = mpBind(SeverSocket, (struct sockaddr*)&serverSockAddr, sizeof(serverSockAddr));
	if (status < 0) {
		printf("Bind socket fail\n");
		CloseSocket(SeverSocket);
	}
	status = mpListen(SeverSocket, SOMAXCONN);
	if (status < 0) {
		printf("listen port fail\n");
		CloseSocket(SeverSocket);
	}
	AcceptHandle = mpAccept(SeverSocket, (struct sockaddr*)&cilentSockAddr, sizeof(cilentSockAddr));
	if (AcceptHandle < 0) {
	AcceptHandle = Reconnect(AcceptHandle);
	}
	
	return AcceptHandle;
}

//接收数据
void Analyze(int AcceptHandle) {
	int byteRecv, Order;
	char Buffer[10] = { 0 }; //接收数据的长度自己定义
	int DelayTime = mpGetRtc();
	while (1) {
		mpTaskDelay(20*DelayTime);
		ResetVar();
		memset(Buffer, 0, sizeof(Buffer));
		byteRecv = mpRecv(AcceptHandle, Buffer, 10, 0); //这里也需要修改
		if ((byteRecv <= 0)) {
			printf("recieve data fail");
			AcceptHandle = Reconnect(AcceptHandle);
			continue; 
		}
        
    AnalyzeDataLogic();//处理数据的逻辑写在这
    
    char SendBuffer[10];//需要发送的数据
    int byteSend = mpSend(CilentSocket, SendBuffer, strlen(SendBuffer), 0);
    if (byteSend != 10) { //发送数据的长度
	    printf("send data error \n");
    }

    }
}

//客户端断开后，关闭客户端socket资源，不然服务端会拒绝连接，重新建立客户端进行连接，并返回客户端描述符
int Reconnect(int ClientSocket) {
	CloseSocket(ClientSocket);
	struct sockaddr_in cilentSockAddr;
	memset(&cilentSockAddr, 0, sizeof(cilentSockAddr));	
	int AcceptHandle = mpAccept(SeverSocket, (struct sockaddr*)&cilentSockAddr, sizeof(cilentSockAddr));
	return AcceptHandle;
}