库卡机器人编程之如何判断机器人当前是否到目标位置
2019-11-11 14:43
库卡系统里存在系统变量$IN_HOME来监测当前机器人是否在原点位置，如果我们想监测机器人当前是否到达自己想要的目标位置，可以通过如下代码实现，如果需要实时监测，可以使用$CYCFLAG[X]或SPS：
DEF E6PosCheck_Function( )DECL BOOL bResultDECL E6Pos TargetTarget={X 800.28,Y -73.29,Z 1288.51,A -26.58,B 38.19,C 39.67,S 2,T 35,E1 0,E2 0,E3 0,E4 0,E5 0,E6 0}BAS(#TOOL,0)BAS(#BASE,0)bResult=E6Pos_Check(Target,Base_DATA[1],Tool_Data[1],0.5,0.5)haltENDGLOBAL DEFFCT BOOL E6Pos_Check(npTargetPos:IN,nfBase:IN,nfTool:IN,nrMaxTrans:IN,nrMaxRot:IN );***************************************************;* Customer : XXXXX Company *;* Roboter : KR 16-2 *;* Version : KSS 8.3.32 *;* Roboter Nr. : 423121 *;* Controller Nr: 342132 *;* Author : Andrew Wang *;* Company : XXXX Company *;* Department : Eletric *;* Telephone : 86 156-8082-2827 *;* Version : 1.1 *;* Created : 12.12.2018 *;* Modified : *;* Project : xxxxx *;* Function Name : E6Pos_Check *;* To check if robot is at the desired position; *;***************************************************DECL E6POS npTargetPos ;;Target Position data DECL FRAME nfBase ;; Base data of Target Position DECL FRAME nfTool ;; Tool data of Target PositionDECL REAL nrMaxTrans ;; Max Translation MarginDECL REAL nrMaxRot ;;Max rotory Margin DECL E6POS npActPos ;;Current Position Data DECL FRAME nfActBase ;;Current Base dataDECL FRAME nfActTool ;;Current Tool dataDECL BOOL nb_BInPos ;; Return value for functionCONTINUE nfActBase=$BASE ;;store current BaseCONTINUE nfActTool=$TOOL ;;Store current tool CONTINUE $BASE=nfBase ;;Select new base for transformationCONTINUE $TOOL=nfTool ;;Select new tool for transformation CONTINUE npActPos=$POS_ACT ;;Actual position with new tool and basenb_BInPos=TRUEIF ABS(npActPos.X-npTargetPos.X) > nrMaxTrans THENnb_BInPos=FALSEENDIFIF ABS(npActPos.Y-npTargetPos.Y) > nrMaxTrans THENnb_BInPos=FALSEENDIFIF ABS(npActPos.Z-npTargetPos.Z) > nrMaxTrans THENnb_BInPos=FALSEENDIFIF ABS(npActPos.A-npTargetPos.A) > nrMaxRot THENnb_BInPos=FALSEENDIFIF ABS(npActPos.B-npTargetPos.B) > nrMaxRot THENnb_BInPos=FALSEENDIFIF ABS(npActPos.C-npTargetPos.C) > nrMaxRot THENnb_BInPos=FALSEENDIFIF (npActPos.S <> npTargetPos.S)THENnb_BInPos=FALSEENDIFIF (npActPos.T <> npTargetPos.T) THENnb_BInPos=FALSEENDIFIF ABS(npActPos.E1-npTargetPos.E1) > nrMaxRot THENnb_BInPos=FALSEENDIFIF ABS(npActPos.E2-npTargetPos.E2) > nrMaxRot THENnb_BInPos=FALSEENDIFIF ABS(npActPos.E3-npTargetPos.E3) > nrMaxRot THENnb_BInPos=FALSEENDIFIF ABS(npActPos.E4-npTargetPos.E4) > nrMaxRot THENnb_BInPos=FALSEENDIFIF ABS(npActPos.E5-npTargetPos.E5) > nrMaxRot THENnb_BInPos=FALSEENDIFIF ABS(npActPos.E6-npTargetPos.E6) > nrMaxRot THENnb_BInPos=FALSEENDIF$BASE=nfActBase$TOOL=nfActToolRETURN(nb_BInPos)ENDFCT--end--
声明：本文章由网友投稿作为教育分享用途，如有侵权原作者可通过邮件及时和我们联系删除：freemanzk@qq.com
