联系我们
库卡机器人编程之如何判断机器人当前是否到目标位置
声明：本媒体部分图片、文章来源于网络
版权归原作者所有，如有侵权，请与我联系删除。
库卡系统里存在系统变量$IN_HOME来监测当前机器人是否在原点位置，如果我们想监测机器人当前是否到达自己想要的目标位置，可以通过如下代码实现，如果需要实时监测，可以使用$CYCFLAG[X]或SPS：
DEF E6PosCheck_Function( )
DECL BOOL bResult
DECL E6Pos Target
Target={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)
halt
END
GLOBAL 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
DECLFRAME nfBase ;; Base data of Target Position
DECLFRAME nfTool ;; Tool data of Target Position
DECL REAL nrMaxTrans ;; Max Translation Margin
DECL REAL nrMaxRot ;;Max rotory Margin
DECL E6POS npActPos ;;Current Position Data
DECLFRAME nfActBase ;;Current Base data
DECLFRAME nfActTool ;;Current Tool data
DECL BOOL nb_BInPos ;; Return value for function
CONTINUE
nfActBase=$BASE ;;store current Base
CONTINUE
nfActTool=$TOOL ;;Store current tool
CONTINUE
$BASE=nfBase ;;selec new base for transformation
CONTINUE
$TOOL=nfTool ;;selec new tool for transformation
CONTINUE
npActPos=$POS_ACT ;;Actual position with new tool and base
nb_BInPos=TRUE
IF ABS(npActPos.X-npTargetPos.X) > nrMaxTrans THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.Y-npTargetPos.Y) > nrMaxTrans THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.Z-npTargetPos.Z) > nrMaxTrans THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.A-npTargetPos.A) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.B-npTargetPos.B) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.C-npTargetPos.C) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF (npActPos.S <> npTargetPos.S)THEN
nb_BInPos=FALSE
ENDIF
IF (npActPos.T <> npTargetPos.T) THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.E1-npTargetPos.E1) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.E2-npTargetPos.E2) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.E3-npTargetPos.E3) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.E4-npTargetPos.E4) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.E5-npTargetPos.E5) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.E6-npTargetPos.E6) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
$BASE=nfActBase
$TOOL=nfActTool
RETURN(nb_BInPos)
ENDFCT
DEF E6PosCheck_Function( )
DECL BOOL bResult
DECL E6Pos Target
Target={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)
halt
END
GLOBAL 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
DECL
DECL REAL nrMaxTrans ;; Max Translation Margin
DECL REAL nrMaxRot ;;Max rotory Margin
DECL E6POS npActPos ;;Current Position Data
DECL
DECL
DECL BOOL nb_BInPos ;; Return value for function
CONTINUE
nfActBase=$BASE ;;store current Base
CONTINUE
nfActTool=$TOOL ;;Store current tool
CONTINUE
$BASE=nfBase ;;selec new base for transformation
CONTINUE
$TOOL=nfTool ;;selec new tool for transformation
CONTINUE
npActPos=$POS_ACT ;;Actual position with new tool and base
nb_BInPos=TRUE
IF ABS(npActPos.X-npTargetPos.X) > nrMaxTrans THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.Y-npTargetPos.Y) > nrMaxTrans THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.Z-npTargetPos.Z) > nrMaxTrans THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.A-npTargetPos.A) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.B-npTargetPos.B) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.C-npTargetPos.C) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF (npActPos.S <> npTargetPos.S)THEN
nb_BInPos=FALSE
ENDIF
IF (npActPos.T <> npTargetPos.T) THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.E1-npTargetPos.E1) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.E2-npTargetPos.E2) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.E3-npTargetPos.E3) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.E4-npTargetPos.E4) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.E5-npTargetPos.E5) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
IF ABS(npActPos.E6-npTargetPos.E6) > nrMaxRot THEN
nb_BInPos=FALSE
ENDIF
$BASE=nfActBase
$TOOL=nfActTool
RETURN(nb_BInPos)
ENDFCT
更多资讯：库卡机器人
- 下一篇：库卡机器人编程之运动编程
- 上一篇：库卡机器人编程之如何log数据