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

最新推荐文章于 2024-07-05 21:39:11 发布

-今晚打老虎-

最新推荐文章于 2024-07-05 21:39:11 发布

阅读量9.9k

点赞数 7

分类专栏： ABB机器人文章标签： tcp/ip 网络协议 wireshark qt

本文链接：https://blog.csdn.net/weixin_39383155/article/details/119240818

版权

ABB机器人专栏收录该内容

7 篇文章

订阅专栏

本文介绍了如何利用ABB机器人的CRobT位置函数实现精确的机器人位置控制，通过示例代码展示了如何判断机械臂是否到达预设位置，并提供了一个用于检测是否到达目标位置的CurrentPos函数。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

文章目录

前言

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

RAPID位置函数

ABB提供的位置函数
这里选取CRobT这个函数来实现需要的功能。

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