新松机器人：【示教编程】+【PC离线编程】+【PLC远程IO】相结合

项目背景

先说一下项目背景，项目要求潮湿的工业环境，在保证机器人末端跟流水线保持相对静止的情况下，控制末端执行器沿 $XZ$ 平面内的点进行移动。

功能要求：

机器人末端执行器实现对工厂流水线的随动，其中流水线沿下图 $Y$ 轴方向运动；
传感器实时获取目标点在 $XZ$ 平面内的信息；
在随动的同时，使用传感器获取的目标点信息指导末端执行器运动。

已知条件：

传感器获取的点只有 $X$ 、 $Z$ 二维坐标，即 $P(x,z)$ ；
运动起点的坐标可以设定，如 $P_0(x_0,y_0,z_0)$ ；
沿Z轴的运动速度可以给定，如 $v_z$ ；

思路

面对这种工况，一般有两种思路：完全采用离线编程来实现、采用示教编程+离线编程结合实现，接下来分别进行介绍。

思路1：完全采用离线编程

完全采用离线编程来实现上述功能，具体实现流程如下：

编码器获取流水线的速度 $v_y$ ；
传感器获取运动点的坐标 $P(x,z$ )；
实现随动：计算从点 $P_0$ 运动到点 $P(x,z)$ 的时间差： $t = (z-z_0)/v_z$ ，由此计算出该点的 $Y$ 坐标： $y=y_0+t*v_y$ ；
工控机通过离线编程，控制机器人末端执行器按照补全后的点 $P(x,y,z)$ 进行运动，完成功能；

思路2：示教编程+离线编程

该思路使用了机器人的示教点偏移功能(SHIFTON)和补偿随动功能(AMENDON)，考虑到待加工的对象结构、尺寸等都差不多，所以示教一套具有普适性的轨迹点作为标准点，机器人沿这些标准点进行运动；如果传感器获取的工件尺寸与标准尺寸的有偏差，则使用示教点偏移功能在原来的轨迹点上进行一定的偏移，补全偏差即可。而补偿随动功能只需要给定随动的速度，则机器人在运动时，会自动将随动速度补偿到相应的轨迹点上，实现随动功能。

示教点偏移功能

具体流程如下：

依次示教 $N$ 个点的坐标，作为标准点，写进示教作业里；
编码器获取流水线速度 $v_y$ ；
传感器获取运动点的坐标 $P(x,z$ )；
计算传感器获取的点与示教的标准点之间的坐标差值；
工控机通过离线编程将编码器获取的速度 $v_y$ 以及 $N$ 个点的 $X$ 、 $Z$ 坐标差写入到示教盒内(通过修改“位置变量”的方式)；
PLC通过操作系统IO的方式，实现执行主作业，完成功能；

对比

思路1：完全依赖软件进行运动规划、控制，工控机作为控制核心、PLC作为功能模块，自己“造轮子”，不可控因素较多；
思路2：运动控制写在示教作业里，提高了运行的稳定性；同时PLC作为控制核心，工控机则作为了一个功能模块，不参与系统的运动控制；采用了工业上现有的、成熟的机器人补偿和随动功能，简化了系统流程、降低了风险。

实现

示教作业：

子作业1：BACK

/*
功能：回原位
*/
NOP
MOVL P100 VL=200 CP=1 ACC 100 // P100:原位
RET
END

子作业2：PROCESS

/*
功能：设置轨迹点的位置变量Px为相应的用户变量Rx；并按位置变量指定的点进行运动
*/
NOP
// 设置轨迹点的x/y/z坐标(其实只需要x、z就够了，y可赋值为R2)
AMEND SET P99  				//  将位置变量赋值给amend规划
AMENDON F#1 M=1				// 实时补偿，#F1：机器人基坐标系M1：绝对补偿
MOVL P1 VL=200 CP=1 ACC 100	// 1代表连续，ACC：最大加速度百分比
MOVL P2 VL=200 CP=1 ACC 100
MOVL P3 VL=200 CP=1 ACC 100
MOVL P4 VL=200 CP=1 ACC 100
AMENDOFF
RET
END

作业名：MAIN

/*
功能：主作业，根据条件选择执行作业
*/
NOP
CALL IN#1=ON	BACK  	 // 回原位
CALL IN#3=ON	PROCESS  // 运动	
DELAY T=0.2		// 延时0.2s（0.1-999.0s）
END

离线编程:

#include <iostream>    
#include "stdafx.h"   
#include "RobotSDK.h"  

#define VARIABLESIZE 6

using namespace std;

int main()
{
	// 接口初始化操作：操作成功返回0，操作失败返回-1.
	int ErrorCode = RobotInit();
	if (ErrorCode == 0)
	{
		std::cout << "Robot Initilize success" << std::endl;
	}
	else
	{
		std::cout << "Robot Initilize failed, return code is " << ErrorCode << std::endl;
	}

	//机器人UDP连接，IP为192.168.3.150，端口号为9999.
	char* cIP = "192.168.3.150";
	short port = 9999;
	int connectID = ConnectUDP(cIP, port);
	if (connectID>0)
	{
		std::cout << "Robot connect success" << std::endl;
	}
	else
	{
		std::cout << "Robot connect failed, return code is " << connectID << std::endl;
	}
/*
	// 关闭安全门（必需操作）。
	ErrorCode = Send_SafeDoorOFF(connectID);
	if (ErrorCode == 0)
	{
		std::cout << "close safe door success" << std::endl;
	}
	else
	{
		std::cout << "failed" << connectID << std::endl;
	}	
	// 打开作业
	char *jobName = "VARIABLE";
	ErrorCode = OpenJob(jobName, connectID);
	if (ErrorCode == 0)
	{
		cout << "成功打开作业" << endl;
	}
	else
	{
		cout << "打开作业失败" << endl;
	}
	
	// 报警复位
	ErrorCode = Send_Reset(connectID);
	if (!ErrorCode)
	{
		cout << "报警复位成功" << endl;
	}
	else {
		cout << "报警复位失败" << endl;
	}

	//清除缓存
	ErrorCode = ClearBuffer(connectID);  //ClearBuffer()函数用于清空机器人控制器中的缓存。
	if (ErrorCode == 0)
	{
		std::cout << "Clear buffer successful!" << std::endl;
	}
	else
	{
		std::cout << "Clear buffer failed, return code is " << ErrorCode << std::endl;
	}

	//伺服上电
	ErrorCode = Send_PowerOn(connectID);
	Sleep(2000);	// 延迟2s，等待上电完成

	if (ErrorCode == 0)
	{
		std::cout << "Servo on power" << std::endl;
	}
	else
	{
		std::cout << "Servo power failed, return code is " << ErrorCode << std::endl;
	}
*/
	// 设置位置变量
	int poseID[VARIABLESIZE] = { 100,99,1,2,3,4 };	// 待修改位置变量的ID
	RPY poseRPY[VARIABLESIZE];	// 位置变量数组，存储相应点的位姿

	float exAxis[3] = { 0,0,0 }; //外部轴关节值
	int coordinate = 1; //坐标系选择，1：世界坐标系；2：用户坐标系；3：工具坐标系
	int TOOLNUM = 1; //工件坐标系选择
	int USERNUM = 1; //用户坐标系选择

	/*
		此处对poseRPY[i]进行操作，修改各点的位姿
	*/
	for (int i = 0; i < VARIABLESIZE; i++)
	{
		ErrorCode  = WriteUserVarRPYS(poseID[i], poseRPY[i], exAxis, coordinate, TOOLNUM,USERNUM, connectID);
	}

	if (ErrorCode == 0)
	{
		std::cout << "成功设置浮点型用户变量" << std::endl;
	}
	else
	{
		std::cout << "设置浮点型用户变量失败" << std::endl;
	}
/*
	// 外部启动
	ErrorCode = Send_Start(connectID);
	if (ErrorCode == 0)
	{
		std::cout << "Main operation is opened" << std::endl;
	}
	else
	{
		std::cout << "Main operation failed, return code is " << ErrorCode << std::endl;
	}
*/
	// 断开与机器人的连接
	ErrorCode = Disconnect(connectID);
	if (ErrorCode == 0)
	{
		std::cout << "Disconnect success" << std::endl;
	}
	else
	{
		std::cout << "Disconnect failed, return code is " << ErrorCode << std::endl;
	}

	system("pause");
	return 0;
}

备注

上述代码实现，并没有使用SHIFTON进行示教点偏移，因为SHIFTON一般只能把所有点作为一个整体进行偏移，对于每个点的每个坐标偏移量都不相同的场景不太适用。这里是直接把目标点的坐标直接赋值到位置变量里，并没有进行偏移。
离线编程代码部分不用打开作业也可修改用户变量，而且当机器人在作业MAIN中循环等待时，也无法对作业进行操作，否则会报错：“执行中无法操作作业”；
AMENDON功能默认没有被添加进机器人RC系统中，需要联系新松工作人员添加。
注意时序问题，需要在离线编程修改完位置变量以后，才能执行PROCESS子作业；否则PROCESS子作业中的P点是上一次修改后的P点，而不是本次的P点。