更新KUKA程序

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

@@ -0,0 +1,64 @@
+&ACCESS RV
+&REL 1
+DEFFCT E6POS OffsetByBase(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 FrameTrans, FrameRot, PosTrans, PosRot
+
+  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
+
+  rPosition = Position
+
+  FrameTrans = $NULLFRAME
+  FrameTrans.X = rRelX
+  FrameTrans.Y = rRelY
+  FrameTrans.Z = rRelZ
+
+  FrameRot = $NULLFRAME
+  FrameRot.A = rRelA
+  FrameRot.B = rRelB
+  FrameRot.C = rRelC
+
+  PosTrans = $NULLFRAME
+  PosTrans.X = rPosition.X
+  PosTrans.y = rPosition.Y
+  PosTrans.Z = rPosition.Z
+
+  PosRot = $NULLFRAME
+  PosRot.A = rPosition.A
+  PosRot.B = rPosition.B
+  PosRot.C = rPosition.C
+
+  ;Positionstransformation durchfuehren
+  rPosition = FrameTrans:PosTrans:FrameRot:PosRot
+
+  ;Status & Turn der Originalposition uebernehmen
+
+  ;Werte der externen Achse der Orignalposition 
+  ;uebernehmen
+  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