更新KUKA程序

KUKA/KRC/R1/Program/Mylib/OffsetByTool.src

@@ -0,0 +1,45 @@
+&ACCESS RV
+&REL 1
+DEFFCT E6POS OffsetByTool(Position:IN, rRelX:IN, rRelY:IN, rRelZ:IN, rRelA:IN, rRelB:IN, rRelC:IN)
+  DECL E6POS Position,rPosition
+  DECL REAL rRelX, rRelY, rRelZ, rRelA, rRelB, rRelC
+  DECL FRAME FrameTmp
+
+  IF VARSTATE("rRelX") <> #INITIALIZED THEN
+    rRelX = 0
+  ENDIF
+  IF VARSTATE("rRelY") <> #INITIALIZED THEN
+    rRelY = 0
+  ENDIF
+  IF VARSTATE("rRelZ") <> #INITIALIZED THEN
+    rRelZ = 0
+  ENDIF
+  IF VARSTATE("rRelA") <> #INITIALIZED THEN
+    rRelA = 0
+  ENDIF
+  IF VARSTATE("rRelB") <> #INITIALIZED THEN
+    rRelB = 0
+  ENDIF
+  IF VARSTATE("rRelC") <> #INITIALIZED THEN
+    rRelC = 0
+  ENDIF
+
+  FrameTmp = $NULLFRAME
+  FrameTmp.X = rRelX
+  FrameTmp.Y = rRelY
+  FrameTmp.Z = rRelZ
+  FrameTmp.A = rRelA
+  FrameTmp.B = rRelB
+  FrameTmp.C = rRelC
+  
+  rPosition = Position:FrameTmp
+  
+  rPosition.E1 = Position.E1
+  rPosition.E2 = Position.E2
+  rPosition.E3 = Position.E3
+  rPosition.E4 = Position.E4
+  rPosition.E5 = Position.E5
+  rPosition.E6 = Position.E6
+
+  RETURN(rPosition)
+ENDFCT