更新KUKA程序

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

@@ -0,0 +1,19 @@
+&ACCESS RVO
+DEFFCT BOOL CheckInPos (R_offset:IN,EndPos:IN )
+   DECL REAL R_offset
+   DECL BOOL b_Pok
+   DECL E6POS ACT_Point
+   DECL E6POS EndPos
+   ACT_Point = $POS_ACT   
+   IF (ABS(ACT_Point.X - EndPos.X) > R_offset) OR (ABS(ACT_Point.Y - EndPos.Y) > R_offset) OR (ABS(ACT_Point.Z - EndPos.Z) > R_offset)  OR (ABS(ACT_Point.A - EndPos.A) > 0.5)  OR (ABS(ACT_Point.B - EndPos.B) > 0.5)  OR (ABS(ACT_Point.C - EndPos.C) > 0.5)  OR (ABS(ACT_Point.E1 - EndPos.E1) > R_offset) THEN
+      b_Pok=FALSE
+   ELSE
+      b_Pok=TRUE 
+   ENDIF      
+   IF NOT b_Pok THEN      
+      MsgNotify("CheckInPosError", "OutArea", , , 1) 
+      ;HALT
+   ENDIF
+   
+   RETURN b_Pok 
+ENDFCT

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

@@ -0,0 +1,12 @@
+&ACCESS RVO
+DEFFCT CHAR[32] INT_TO_STR(iVal:IN)
+   DECL INT iVal,I,offset
+   DECL CHAR Ret[32]
+   DECL STATE_T state
+   offset=0
+   FOR I=1 TO 32
+      Ret[I]=0
+   ENDFOR
+   SWRITE(Ret[],state,offset,"%d",iVal)
+   Return (Ret[])
+ENDFCT

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

@@ -0,0 +1,12 @@
+&ACCESS RV
+DEF Initmove ( )
+CONTINUE
+IF ($T1 OR $T2) AND NOT $NEAR_POSRET THEN
+    CONTINUE
+    GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
+    INTERRUPT ON 3 
+    BAS (#INITMOV,0 )
+    CONTINUE
+    PTP $POS_ACT C_DIS
+  ENDIF
+END

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

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

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

@@ -0,0 +1,19 @@
+&ACCESS RV
+DEFFCT INT REAL_TO_Output (rVal:IN )
+   DECL REAL rVal
+   DECL INT Offset,Ret
+   DECL INT INTS[4]
+   DECL CHAR Bytes[4]
+   Offset=0
+   Ret=0
+   CAST_TO(Bytes[],Offset,rVal)    
+   INTS[1] = Bytes[1]   
+   INTS[2] = Bytes[2]
+   INTS[2] = INTS[2]*256
+   INTS[3] = Bytes[3]
+   INTS[3] = INTS[3]*65536
+   INTS[4] = Bytes[4]
+   INTS[4] = INTS[4]*16777216   
+   Ret=INTS[1]+INTS[2]+INTS[3]+INTS[4]
+   RETURN Ret
+ENDFCT

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

@@ -0,0 +1,22 @@
+&ACCESS RVO
+DEFFCT CHAR[32] REAL_TO_STR(iVal:IN)
+   DECL REAL iVal
+   DECL INT I,offset
+   DECL CHAR Ret[32]
+   DECL STATE_T state
+   offset=0
+   FOR I=1 TO 32
+      Ret[I]=0
+   ENDFOR
+   SWRITE(Ret[],state,offset,"%.3f",iVal)
+   Return (Ret[])
+ENDFCT
+
+
+
+   
+   
+   
+   
+   
+