利用ABB机器人的CRobT写一个判断机器人是否移动至目标位置的函数

前言

在使用ABB机器人做二次开发时，可能会需要根据机器人目前的位置来判断是否需要进行下一步的指令执行，ABB机器人提供了获取机器人实时位置状态的函数，查阅ABB官方的手册可以找到。我再根据此函数写了判断机器人是否到达目标位置的功能，仅供参考。

RAPID位置函数

这里选取CRobT这个函数来实现需要的功能。

CRobT的说明

没有特殊情况的话，选取它的两个主元[\Tool][\WObj]初始化就行，即
CRobT(\Tool:=tool0\WObj:=wobj0);

判断是否达到目标位置的代码实现

先定义一个变量robtarget ActualPos，再与目标位置p10的x、y、z、q1、q2、q3、q4比较，在误差范围内则判断机械臂已移至目标位置，据此写一个Function CurrentPos，输入参数分别时目标位置，和使用的TCP工具，这里使用默认的tool0。

VAR bool timeout;
VAR robtarget p10:=[[-1609.73,-1330.24,32.35],[0.133529,-0.93591,0.289425,-0.149922],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
WaitUntil CurrentPos(p10, tool0)=True\MaxTime:=120\TimeFlag:=timeout;
IF timeout THEN
	TPWrite "Robot did not reach designated position within expected time";
	Stop;
ELSE
	TPWrite"Arrive Pisition-1";
ENDIF


!判断位置功能，用于检测机器人是否在某个位置上
FUNC bool CurrentPos(robtarget ComparePos, INOUT tooldata TCP)
	!局部变量
	VAR num Counter:=0;
	VAR robtarget ActualPos;
	!比较当前位置与给定位置是否相等
	ActualPos:=CRobT(\Tool:=tool0\WObj:=wobj0);
	IF ActualPos.trans x>ComparePos.trans.x-25 AND ActualPos.trans.x<ComparePos.trans.x+25 Counter:=Counter+1;
	IF ActualPos.trans.y>ComparePos.trans.y-25 AND ActualPos.trans.y<ComparePos.trans.y+25 Counter:=Counter+1;
	IF ActualPos.trans.z>ComparePos.trans.z-25 AND ActualPos.trans.z<ComparePos.trans.z+25 Counter:=Counter+1;	
	IF ActualPos.rot.q1>ComparePos.rot.q1-0.1 AND ActualPos.rot.q1<Compare.Pos.rot.q1+0.1 Counter:=Counter+1;
	IF ActualPos.rot.q2>ComparePos.rot.q2-0.1 AND ActualPos.rot.q2<Compare.Pos.rot.q2+0.1 Counter:=Counter+1;
	IF ActualPos.rot.q3>ComparePos.rot.q3-0.1 AND ActualPos.rot.q3<Compare.Pos.rot.q3+0.1 Counter:=Counter+1;
	IF ActualPos.rot.q4>ComparePos.rot.q4-0.1 AND ActualPos.rot.q4<Compare.Pos.rot.q4+0.1 Counter:=Counter+1;
	RETURN Counter=7;
ENDFUNC