KUKA KRL 현재 로봇 위치가 특정 티칭위치 근처에 있는지 확인하는 함수

좌표공간에서 두 점 사이의 거리를 구하는 공식을 이용하여 로봇의 TCP가 특정 티칭위치 근처에 있는지 확인하는 함수를 소개합니다.

좌표공간 두 점 사이 거리 공식

 

아래 함수는 첫번째 인수로 거리값을 mm로 받고, 두번째 인수로 티칭 포지션을 받습니다.

GLOBAL DEFFCT BOOL CHECK_LOCATION1(rDistance:IN,pCheckPos:IN)

   

   BOOL bReturnVal

   REAL X1,X2,Y1,Y2,Z1,Z2,rCalResult,rDistance

   E6POS pCheckPos

   

   bReturnVal=FALSE

   

   X1=$POS_ACT_MES.X

   Y1=$POS_ACT_MES.Y

   Z1=$POS_ACT_MES.Z

   

   X2=pCheckPos.X

   Y2=pCheckPos.Y

   Z2=pCheckPos.Z

   

   rCalResult=SQRT((X1-X2)*(X1-X2)+(Y1-Y2)*(Y1-Y2)+(Z1-Z2)*(Z1-Z2))

   

   IF rDistance >= rCalResult THEN      

      bReturnVal=TRUE

   ENDIF  

   

   RETURN bReturnVal

   

ENDFCT

 

사용예는 다음과 같습니다.

 

아래와 같이 함수를 호출했을 때, 현재 로봇의 TCP가 Home 위치에서 3mm 내에 있으면 True를, 그렇지 않으면 False를 Result 값에 저장합니다.

 

Result = CHECK_LOCATION1(3, Home)

 

위의 함수를 응용하면 가장 가까운 티칭 포인트로 차례차례 이동하여 안전하게 Home 복귀 루틴을 구현할 수 있습니다.

 

감사합니다.