库卡KUKA-posCheck()-判断位置-私有库

简介：

• 用于判断机器人当前位是否在参考位
• 本程序仅考虑XYZ坐标和ABC姿态的比对，不考虑ST轴状态和外轴状态
• 用编号输入参考点工具和基座标
• 如果在参考位，返还true

源码：

DEFFCT BOOL func_posCheck(p_targetPos:IN,t_targetTool:IN,b_targetBase:IN,r_maxTrans:IN,r_maxRot:IN)

;******************************************* ;Name       :Pos Check(E6POS : XYZ+ABC) ;Author     :Wen ;Version    :1.1 ;Created    :05.17.2021 ;To check if robot is at the desired position ;*******************************************

DECL E6POS p_targetPos DECL int b_targetBase DECL int t_targetTool DECL REAL r_maxTrans DECL REAL r_maxRot DECL E6POS p_actPos DECL FRAME b_actBase DECL FRAME t_actTool DECL BOOL b_result

b_actBase = $BASE t_actTool = $TOOL $BASE = Base_Data[b_targetBase] $TOOL = Tool_Data[t_targetTool] p_actPos = $POS_ACT b_result = True

IF ABS(p_actPos.X - p_targetPos.X) > r_maxTrans THEN b_result = False ENDIF IF ABS(p_actPos.Y - p_targetPos.Y) > r_maxTrans THEN b_result = False ENDIF IF ABS(p_actPos.Z - p_targetPos.Z) > r_maxTrans THEN b_result = False ENDIF

IF ABS(p_actPos.A - p_targetPos.A) > r_maxRot THEN b_result = False ENDIF IF ABS(p_actPos.B - p_targetPos.B) > r_maxRot THEN b_result = False ENDIF IF ABS(p_actPos.C - p_targetPos.C) > r_maxRot THEN b_result = False ENDIF

$BASE = b_actBase $TOOL = t_actTool RETURN(b_result)

ENDFCT

用法范例：

IF NOT func_posCheck(xhome,1,1,5,0.5) THEN
    gohome()
ENDIF