PLC/6040-003
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
cee4549e7980dbac905519e14c9cc3cbda461f33
6040-003/KUKA/KRC/R1/System/bas.src
zhusenlin cee4549e79 更新优化程序
2026-02-11 17:34:41 +08:00

2084 lines
53 KiB
Plaintext
Raw Blame History

&ACCESS R3
&REL 1
&COMMENT BASIS package
DEF BAS (COMMAND :IN,REAL_PAR :IN )
;FOLD BAS ()
DECL BAS_COMMAND COMMAND
REAL REAL_PAR
SWITCH COMMAND
CASE #INITMOV
INITMOV ( )
RESET_CD_PARAMS ()
CASE #ACC_CP
ACC_CP (REAL_PAR )
CASE #ACC_GLUE
ACC_GLUE (REAL_PAR )
CASE #ACC_PTP
ACC_PTP (REAL_PAR )
CASE #VEL_CP
VEL_CP (REAL_PAR )
CASE #VEL_PTP
VEL_PTP (REAL_PAR )
CASE #TOOL
TOOL (REAL_PAR )
CASE #BASE
BASE (REAL_PAR )
CASE #EX_BASE
EX_BASE (REAL_PAR, 0 )
CASE #FRAMES
FRAMES ( )
CASE #CP_DAT
VEL_CP (LDAT_ACT.VEL )
CP_DAT ( )
CASE #PTP_DAT
VEL_PTP (PDAT_ACT.VEL )
PTP_DAT ( )
CASE #OUT_SYNC
OUT_SYNC ( )
CASE #OUT_ASYNC
OUT_ASYNC ( )
CASE #GROUP
GROUP (REAL_PAR )
CASE #PTP_PARAMS
PTP_DAT ( )
FRAMES ( )
VEL_PTP (REAL_PAR )
CASE #CP_PARAMS
CP_DAT ( )
FRAMES ( )
VEL_CP (REAL_PAR )
ENDSWITCH
END
;ENDFOLD BAS ()
;FOLD INITMOV ()
DEFFCT CHAR[128] IntToStrWithPrefix(tVar:IN, bPrefixComma:IN)
DECL INT tVar
DECL CHAR tStr[128]
DECL STATE_T STATE
DECL INT OFFSET
DECL BOOL bPrefixComma
OFFSET = 0
IF bPrefixComma THEN
SWRITE(tStr[],STATE,OFFSET,",%d",tVar)
ELSE
SWRITE(tStr[],STATE,OFFSET,"%d",tVar)
ENDIF
RETURN tStr[]
ENDFCT
DEF UpdateLoadModes ( )
INT I
FOR I=1 TO MAX_TOOL STEP 1
IF (LOAD_MODE[I]==#NONE) AND (LOAD_DATA[I].M <> -1) THEN
LOAD_MODE[I]=#MANUAL
ENDIF
ENDFOR
END
DEF CheckLoadModes ( )
INT I,iRet
CHAR MsgPar[256]
BOOL first, bResult,foundT1Modes
DECL KrlMsgOpt_T MsgOpt
IF ($MODE_OP <> #T1) THEN
first = TRUE
foundT1Modes=FALSE
bResult = STRCLEAR(MsgPar[])
FOR I=1 TO MAX_TOOL STEP 1
IF (LOAD_MODE[I]==#T1) THEN
foundT1Modes=TRUE
IF first THEN
first = FALSE
iRet = STRADD(MsgPar[], IntToStrWithPrefix(I, FALSE))
ELSE
iRet = STRADD(MsgPar[], IntToStrWithPrefix(I, TRUE))
ENDIF
ENDIF
ENDFOR
IF (foundT1Modes) THEN
MsgOpt={VL_Stop FALSE,Clear_P_Reset TRUE,Log_To_DB TRUE}
IF ($MODE_OP==#EX) THEN
MsgNotify("LddPerformedInT1", "BasisTech", ,MsgPar[],35,MsgOpt)
ELSE
MsgQuit ("LddPerformedInT1", "BasisTech", ,MsgPar[],35,MsgOpt)
ENDIF
ENDIF
ENDIF
END
DEF CheckLoadModeForTool(tool_no:IN)
INT iRet,tool_no
CHAR MsgPar[256]
DECL KrlMsgOpt_T MsgOpt
CONTINUE
IF (($ON_PATH==FALSE) AND ($MODE_OP <> #T1) AND (LOAD_MODE[tool_no]==#T1)) THEN
MsgOpt={VL_Stop FALSE,Clear_P_Reset TRUE,Log_To_DB TRUE}
iRet = STRADD(MsgPar[], IntToStrWithPrefix(tool_no, FALSE))
IF ($MODE_OP==#EX) THEN
MsgNotify("LddPerformedInT1", "BasisTech", ,MsgPar[],35,MsgOpt)
ELSE
MsgQuit ("LddPerformedInT1", "BasisTech", ,MsgPar[],35,MsgOpt)
ENDIF
ENDIF
END
DEF INITMOV ( )
INT I
; PTP-Parameters:
ACC_PTP (100.0 ) ; ptp acceleration 100% = DEF_ACC_PTP
VEL_PTP (100.0 ) ; ptp velocity 100% = DEF_VEL_PTP
; CP-Parameters:
ACC_CP (100.0 ) ; path acceleration 100% = DEF_ACC_CP
$ACC.ORI1=DEF_ACC_ORI1 ; swivel acceleration
$ACC.ORI2=DEF_ACC_ORI2 ; rotation acceleration
VEL_CP (DEF_VEL_CP ) ; path velocity
$VEL.ORI1=DEF_VEL_ORI1 ; swivel velocity
$VEL.ORI2=DEF_VEL_ORI2 ; rotation velocity
; Parameters for Approximation:
$APO.CPTP=DEF_APO_CPTP ; PTP-Approximation
$APO.CDIS=DEF_APO_CDIS ; Distance-Approximation
$APO.CVEL=DEF_APO_CVEL ; Speed-Approximation
$APO.CORI=DEF_APO_CORI ; Orientation-Approximation
$FILTER=$DEF_FLT_PTP ; filter
$ADVANCE=DEF_ADVANCE ; advance
$ORI_TYPE=#VAR ; variable orientation
$CIRC_TYPE=DEF_CIRC_TYP ; CIRC-orientation defined in $config.dat
$JERK=SJERK(LDEFAULT)
TOOL_CORR=$NULLFRAME
TOOL_CORR_ON=FALSE
M_BASE_CORR=$NULLFRAME
M_BAS_COR_ON=FALSE
TOOL_CORR_W=$NULLFRAME
TOOL_CORR_W_ON=FALSE
BASE_CORR=$NULLFRAME
BASE_CORR_ON=FALSE
bJointOffsetHasBeenSet=FALSE
IF iLastAxisNo > 0 THEN
$JOINT_OFFSET[iLastAxisNo] = 0
ENDIF
iLastAxisNo=0
IF $KINCLASS<>#NONE THEN
BASE (0 ) ; Activating Base $WORLD
$TOOL=TOOL_DATA[1]
$ACT_TOOL=1
ENDIF
IF $TCP_IPO==TRUE THEN
$IPO_MODE=#BASE
ENDIF
IF $ADAP_ACC<>#NONE THEN
IF LOAD_A1_DATA.M<0 THEN
$LOAD_A1.M=0
$LOAD_A1.CM={X 0.0, Y 0.0, Z 0.0, A 0.0, B 0.0, C 0.0}
$LOAD_A1.J={X 0.0, Y 0.0, Z 0.0}
ELSE
$LOAD_A1.M=LOAD_A1_DATA.M
$LOAD_A1.CM=LOAD_A1_DATA.CM
$LOAD_A1.J=LOAD_A1_DATA.J
ENDIF
IF LOAD_A2_DATA.M<0 THEN
$LOAD_A2.M=0
$LOAD_A2.CM={X 0.0, Y 0.0, Z 0.0, A 0.0, B 0.0, C 0.0}
$LOAD_A2.J={X 0.0, Y 0.0, Z 0.0}
ELSE
$LOAD_A2.M=LOAD_A2_DATA.M
$LOAD_A2.CM=LOAD_A2_DATA.CM
$LOAD_A2.J=LOAD_A2_DATA.J
ENDIF
IF LOAD_A3_DATA.M<0 THEN
$LOAD_A3.M=$DEF_LA3_M
$LOAD_A3.CM=$DEF_LA3_CM
$LOAD_A3.J=$DEF_LA3_J
ELSE
$LOAD_A3.M=LOAD_A3_DATA.M
$LOAD_A3.CM=LOAD_A3_DATA.CM
IF ((LOAD_A3_DATA.M<>0) AND (LOAD_A3_DATA.J.X==0) AND (LOAD_A3_DATA.J.Y==0) AND (LOAD_A3_DATA.J.Z==0)) THEN
$LOAD_A3.J=$DEF_LA3_J
ELSE
$LOAD_A3.J=LOAD_A3_DATA.J
ENDIF
ENDIF
ENDIF
; standard group definition
COMPL_GROUP=1
FOR I=1 TO $EX_AX_NUM
COMPL_GROUP=COMPL_GROUP*2+1
ENDFOR
$ACT_GROUP=COMPL_GROUP
STOPM_FLAG=FALSE ;reflects state of interrupt 3
; Set $Gear_Jerk[] with defaults
FOR I = 1 TO 12
$GEAR_JERK[I] = DEF_GEAR_JERK
ENDFOR
END
;ENDFOLD INITMOV ()
;FOLD BASE ()
DEF BASE (BASE_NO :IN )
INT BASE_NO
IF (BASE_NO<=0) THEN
IF BASE_CORR_ON AND M_BAS_COR_ON THEN
$BASE=BASE_CORR:$WORLD:M_BASE_CORR
ELSE
IF M_BAS_COR_ON THEN
$BASE=$WORLD:M_BASE_CORR
ELSE
IF BASE_CORR_ON THEN
$BASE=BASE_CORR:$WORLD
ELSE
$BASE=$WORLD
ENDIF
ENDIF
ENDIF
IF bJointOffsetHasBeenSet THEN
bJointOffsetHasBeenSet = FALSE
IF iLastAxisNo > 0 THEN
$JOINT_OFFSET[iLastAxisNo] = 0
ENDIF
ENDIF
ELSE
IF BASE_CORR_ON AND M_BAS_COR_ON THEN
$BASE=BASE_CORR:BASE_DATA[BASE_NO]:M_BASE_CORR
ELSE
IF M_BAS_COR_ON THEN
$BASE=BASE_DATA[BASE_NO]:M_BASE_CORR
ELSE
IF BASE_CORR_ON THEN
$BASE=BASE_CORR:BASE_DATA[BASE_NO]
ELSE
$BASE=BASE_DATA[BASE_NO]
ENDIF
ENDIF
ENDIF
ENDIF
CONTINUE
$ACT_BASE=BASE_NO ; Storing actual Base-Number in global variable
CONTINUE
$ACT_EX_AX=-1
END
;ENDFOLD BASE ()
;FOLD EX_BASE ()
DEF EX_BASE (BASE_NO :IN , MACH_IDX : IN)
INT BASE_NO, MACH_IDX
IF BASE_NO > 0 THEN
IF BASE_CORR_ON AND M_BAS_COR_ON THEN
$BASE=EK(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].MECH_TYPE,BASE_CORR:BASE_DATA[BASE_NO]:M_BASE_CORR)
ELSE
IF M_BAS_COR_ON THEN
$BASE=EK(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].MECH_TYPE,BASE_DATA[BASE_NO]:M_BASE_CORR)
ELSE
IF BASE_CORR_ON THEN
$BASE=EK(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].MECH_TYPE,BASE_CORR:BASE_DATA[BASE_NO])
ELSE
$BASE=EK(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].MECH_TYPE,BASE_DATA[BASE_NO])
ENDIF
ENDIF
ENDIF
ENDIF
CONTINUE
$ACT_BASE=BASE_NO
END
;ENDFOLD EX_BASE ()
;FOLD LK_BASE ()
DEF LK_BASE (MACH_IDX :IN, FRAME_IDX : IN, TOOL_IDX : IN )
INT MACH_IDX, FRAME_IDX, TOOL_IDX
IF FRAME_IDX>-1 THEN
; Linking to a frame - end-of-arm tool
IF BASE_CORR_ON AND M_BAS_COR_ON THEN
$BASE=LK(MACHINE_DEF[MACH_IDX].ROOT, $COOP_KRC[MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX].IP_ADDR[], BASE_CORR:BASE_DATA[FRAME_IDX]:M_BASE_CORR, MACHINE_DEF[MACH_IDX].MECH_TYPE)
ELSE
IF M_BAS_COR_ON THEN
$BASE=LK(MACHINE_DEF[MACH_IDX].ROOT, $COOP_KRC[MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX].IP_ADDR[], BASE_DATA[FRAME_IDX]:M_BASE_CORR, MACHINE_DEF[MACH_IDX].MECH_TYPE)
ELSE
IF BASE_CORR_ON THEN
$BASE=LK(MACHINE_DEF[MACH_IDX].ROOT, $COOP_KRC[MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX].IP_ADDR[], BASE_CORR:BASE_DATA[FRAME_IDX], MACHINE_DEF[MACH_IDX].MECH_TYPE)
ELSE
$BASE=LK(MACHINE_DEF[MACH_IDX].ROOT, $COOP_KRC[MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX].IP_ADDR[], BASE_DATA[FRAME_IDX], MACHINE_DEF[MACH_IDX].MECH_TYPE)
ENDIF
ENDIF
ENDIF
ELSE
; Linking to a tool - remote tool
$BASE=LK(MACHINE_DEF[MACH_IDX].ROOT, $COOP_KRC[MACHINE_DEF[MACH_IDX].COOP_KRC_INDEX].IP_ADDR[], TOOL_DATA[TOOL_IDX], MACHINE_DEF[MACH_IDX].MECH_TYPE)
ENDIF
CONTINUE
$ACT_EX_AX=-1
CONTINUE
$ACT_BASE=FRAME_IDX
END
;ENDFOLD LK_BASE ()
;FOLD EB_BASE ()
DEF EB_BASE (MACH_IDX :IN, BASE_NO :IN)
INT MACH_IDX, BASE_NO
IF BASE_CORR_ON AND M_BAS_COR_ON THEN
$BASE=EB(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_CORR:BASE_DATA[BASE_NO]:M_BASE_CORR,CONV_PART_NBR[MACH_IDX])
ELSE
IF M_BAS_COR_ON THEN
$BASE=EB(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_DATA[BASE_NO]:M_BASE_CORR,CONV_PART_NBR[MACH_IDX])
ELSE
IF BASE_CORR_ON THEN
$BASE=EB(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_CORR:BASE_DATA[BASE_NO],CONV_PART_NBR[MACH_IDX])
ELSE
$BASE=EB(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_DATA[BASE_NO],CONV_PART_NBR[MACH_IDX])
ENDIF
ENDIF
ENDIF
CONTINUE
$ACT_BASE=BASE_NO
END
;ENDFOLD EB_BASE ()
;FOLD EB_ABS_BASE ()
DEF EB_ABS_BASE (MACH_IDX :IN, BASE_NO :IN)
INT MACH_IDX, BASE_NO
IF BASE_CORR_ON AND M_BAS_COR_ON THEN
$BASE=EB_ABS(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_CORR:BASE_DATA[BASE_NO]:M_BASE_CORR,CONV_PART_NBR[MACH_IDX])
ELSE
IF M_BAS_COR_ON THEN
$BASE=EB_ABS(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_DATA[BASE_NO]:M_BASE_CORR,CONV_PART_NBR[MACH_IDX])
ELSE
IF BASE_CORR_ON THEN
$BASE=EB_ABS(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_CORR:BASE_DATA[BASE_NO],CONV_PART_NBR[MACH_IDX])
ELSE
$BASE=EB_ABS(MACHINE_DEF[MACH_IDX].ROOT,MACHINE_DEF[MACH_IDX].NAME[],BASE_DATA[BASE_NO],CONV_PART_NBR[MACH_IDX])
ENDIF
ENDIF
ENDIF
CONTINUE
$ACT_BASE=BASE_NO
END
;ENDFOLD EB_ABS_BASE ()
;FOLD ACC_CP ()
DEF ACC_CP (ACC_FACTOR :IN )
REAL ACC_FACTOR ; [%]
IF ACC_FACTOR>100.0 THEN
$ACC.CP=DEF_ACC_CP
$ACC.ORI1=DEF_ACC_ORI1
$ACC.ORI2=DEF_ACC_ORI2
ELSE
$ACC.CP=DEF_ACC_CP*ACC_FACTOR/100.0
$ACC.ORI1=DEF_ACC_ORI1*ACC_FACTOR/100.0
$ACC.ORI2=DEF_ACC_ORI2*ACC_FACTOR/100.0
ENDIF
END
;ENDFOLD ACC_CP ()
;FOLD ACC_GLUE ()
DEF ACC_GLUE (ACC_FACTOR :IN )
REAL ACC_FACTOR ; [%]
IF ACC_FACTOR>100.0 THEN
$ACC.CP=$ACC_MA.CP
$ACC.ORI1=$ACC_MA.ORI1
$ACC.ORI2=$ACC_MA.ORI2
ELSE
$ACC.CP=$ACC_MA.CP*ACC_FACTOR/100.0
$ACC.ORI1=$ACC_MA.ORI1*ACC_FACTOR/100.0
$ACC.ORI2=$ACC_MA.ORI2*ACC_FACTOR/100.0
ENDIF
END
;ENDFOLD ACC_GLUE ()
;FOLD ACC_PTP ()
DEF ACC_PTP (ACC_FACTOR :IN )
REAL ACC_FACTOR ; [%]
REAL ACC
INT I
IF $ADAP_ACC==#NONE THEN
IF ACC_FACTOR>100.0 THEN
ACC=DEF_ACC_PTP
ELSE
ACC=DEF_ACC_PTP*ACC_FACTOR/100.0
ENDIF
ELSE
ACC=ACC_FACTOR
ENDIF
FOR I=1 TO $NUM_AX
$ACC_AXIS[I]=ACC
ENDFOR
FOR I=1 TO $EX_AX_NUM
CHECK_REDEXAX_ACC(I)
$ACC_EXTAX[I]=ACC*RedAccExAx[I]/100.0
ENDFOR
END
;ENDFOLD ACC_PTP ()
;FOLD VEL_CP ()
DEF VEL_CP (VEL_VALUE :IN )
REAL VEL_VALUE, VEL_CHECK ; [m/s]
INT I
IF VEL_VALUE>$VEL_MA.CP THEN
$VEL.CP=$VEL_MA.CP
ELSE
$VEL.CP=VEL_VALUE*DEF_VEL_FACT
ENDIF
ON_ERROR_PROCEED
VEL_CHECK = $VEL.ORI1
IF $ERR.NUMBER > 0 THEN
$VEL.ORI1=DEF_VEL_ORI1 ; swivel velocity
ERR_CLEAR($ERR)
ENDIF
ON_ERROR_PROCEED
VEL_CHECK = $VEL.ORI2
IF $ERR.NUMBER > 0 THEN
$VEL.ORI2=DEF_VEL_ORI2 ; rotation velocity
ERR_CLEAR($ERR)
ENDIF
FOR I=1 TO $EX_AX_NUM
CHECK_REDEXAX_VEL(I)
$VEL_EXTAX[I]=DEF_VEL_PTP*RedVelExAx[I]/100.0
ENDFOR
END
;ENDFOLD VEL_CP ()
;FOLD VEL_PTP ()
DEF VEL_PTP (VEL_FACTOR :IN )
REAL VEL_FACTOR ; [%]
REAL VEL
INT I
IF VEL_FACTOR>100.0 THEN
VEL=DEF_VEL_PTP
ELSE
VEL=DEF_VEL_PTP*VEL_FACTOR/100.0
ENDIF
FOR I=1 TO $NUM_AX
$VEL_AXIS[I]=VEL
ENDFOR
FOR I=1 TO $EX_AX_NUM
CHECK_REDEXAX_VEL(I)
$VEL_EXTAX[I]=VEL*RedVelExAx[I]/100.0
ENDFOR
END
;ENDFOLD VEL_PTP ()
;FOLD {H}
DEF CHECK_REDEXAX_ACC(I :IN )
INT I
IF RedAccExAx[I]>100.0 THEN
RedAccExAx[I]=100.0
ENDIF
IF RedAccExAx[I]<1.0 THEN
RedAccExAx[I]=1.0
ENDIF
END
DEF CHECK_REDEXAX_VEL(I :IN )
INT I
IF RedVelExAx[I]>100.0 THEN
RedVelExAx[I]=100.0
ENDIF
IF RedVelExAx[I]<1.0 THEN
RedVelExAx[I]=1.0
ENDIF
END
;ENDFOLD
;FOLD TOOL ()
DEF TOOL (TOOL_NO :IN )
INT TOOL_NO
IF (TOOL_NO<=0) THEN
IF TOOL_CORR_W_ON AND TOOL_CORR_ON THEN
$TOOL=TOOL_CORR_W:TOOL_CORR
ELSE
IF TOOL_CORR_ON THEN
$TOOL=TOOL_CORR
ELSE
IF TOOL_CORR_W_ON THEN
$TOOL=TOOL_CORR_W
ELSE
$TOOL=$NULLFRAME
ENDIF
ENDIF
ENDIF
IF $ADAP_ACC<>#NONE THEN
$LOAD.M=$DEF_L_M
$LOAD.CM=$DEF_L_CM
$LOAD.J=$DEF_L_J
ENDIF
ELSE
IF TOOL_CORR_W_ON AND TOOL_CORR_ON THEN
$TOOL=TOOL_CORR_W:TOOL_DATA[TOOL_NO]:TOOL_CORR
ELSE
IF TOOL_CORR_ON THEN
$TOOL=TOOL_DATA[TOOL_NO]:TOOL_CORR
ELSE
IF TOOL_CORR_W_ON THEN
$TOOL=TOOL_CORR_W:TOOL_DATA[TOOL_NO]
ELSE
$TOOL=TOOL_DATA[TOOL_NO]
ENDIF
ENDIF
ENDIF
IF $ADAP_ACC<>#NONE THEN
IF LOAD_DATA[TOOL_NO].M<0 THEN
$LOAD.M=$DEF_L_M
$LOAD.CM=$DEF_L_CM
$LOAD.J=$DEF_L_J
ELSE
$LOAD.M=LOAD_DATA[TOOL_NO].M
$LOAD.CM=LOAD_DATA[TOOL_NO].CM
IF ((LOAD_DATA[TOOL_NO].M<>0) AND (LOAD_DATA[TOOL_NO].J.X==0) AND (LOAD_DATA[TOOL_NO].J.Y==0) AND (LOAD_DATA[TOOL_NO].J.Z==0)) THEN
$LOAD.J=$DEF_L_J
ELSE
$LOAD.J=LOAD_DATA[TOOL_NO].J
ENDIF
ENDIF
ENDIF
ENDIF
CONTINUE
$ACT_TOOL=TOOL_NO
END
;ENDFOLD TOOL ()
;FOLD FRAMES ()
DEF FRAMES ( )
INT MACH_INDEX, COOP_KRC_INDEX, JointDeviceIndex
DECL ESYS MECH_TYPE
BOOL Result
IF STOPM_FLAG==TRUE THEN ;no correct finish of IR_STOPMESS
STOPM_FLAG=FALSE ;reflects state of interrupt 3
ON_ERROR_PROCEED
INTERRUPT ON 3
SWITCH($ERR.NUMBER)
CASE 0
;ok
CASE 1430
GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ()
INTERRUPT ON 3
ERR_CLEAR($ERR)
DEFAULT
ERR_RAISE($ERR)
ENDSWITCH
ENDIF
; Special case: Base=0
IF (FDAT_ACT.BASE_NO == 0) THEN
TOOL (FDAT_ACT.TOOL_NO)
BASE (0)
SET_IPO_MODE (FDAT_ACT.IPO_FRAME )
RETURN
ENDIF
IF (FDAT_ACT.TOOL_NO <> 0) THEN
IF (MACHINE_TOOL_DAT[FDAT_ACT.TOOL_NO].MACH_DEF_INDEX <> 0) THEN
IF (MACHINE_DEF[MACHINE_TOOL_DAT[FDAT_ACT.TOOL_NO].MACH_DEF_INDEX].COOP_KRC_INDEX <> 1) THEN
MsgQuit ("IllegalUseOfRemoteTool", "BAS")
RETURN
ENDIF
ENDIF
ENDIF
;Set IPO_MODE, $TOOL and $LOAD
SET_IPO_MODE (FDAT_ACT.IPO_FRAME )
TOOL (FDAT_ACT.TOOL_NO )
;Set $JOINT_OFFSET
IF JOINT_DEVICE_INDEX[FDAT_ACT.BASE_NO] > 0 THEN
JointDeviceIndex = JOINT_DEVICE_INDEX[FDAT_ACT.BASE_NO]
IF JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX > 0 THEN
IF IsDeviceDefined(JOINT_DEVICE[JointDeviceIndex].NAME[]) THEN
$JOINT_OFFSET[JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX] = EO(JOINT_DEVICE[JointDeviceIndex].NAME[], JOINT_DEVICE[JointDeviceIndex].OFFSET )
ELSE
$JOINT_OFFSET[JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX] = JOINT_DEVICE[JointDeviceIndex].OFFSET
ENDIF
bJointOffsetHasBeenSet = TRUE
iLastAxisNo = JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX
ENDIF
ELSE
IF bJointOffsetHasBeenSet THEN
bJointOffsetHasBeenSet = FALSE
IF iLastAxisNo > 0 THEN
$JOINT_OFFSET[iLastAxisNo] = 0
ENDIF
ENDIF
ENDIF
;Get the frame's machine index, COOP_KRC index and mechanism type
MACH_INDEX = MACHINE_FRAME_DAT[FDAT_ACT.BASE_NO].MACH_DEF_INDEX
IF (MACH_INDEX > 0) Then
MECH_TYPE = MACHINE_DEF[MACH_INDEX].MECH_TYPE
COOP_KRC_INDEX = MACHINE_DEF[MACH_INDEX].COOP_KRC_INDEX
ELSE
MECH_TYPE = #NONE
COOP_KRC_INDEX = 0
ENDIF
IF (MECH_TYPE == #ROBOT) THEN
IF (COOP_KRC_INDEX > 1) THEN
LK_BASE (MACH_INDEX, FDAT_ACT.BASE_NO, -1 )
ELSE
IF (COOP_KRC_INDEX == 1) THEN
BASE (FDAT_ACT.BASE_NO )
ENDIF
ENDIF
ELSE
IF (MECH_TYPE == #NONE) THEN
IF (FDAT_ACT.BASE_NO <= MAX_BASE) THEN
BASE (FDAT_ACT.BASE_NO )
ENDIF
ELSE
; The base is relative to an external axis - need to determine if it is a local
; external axis or a remote one and call EK/LK appropriately. A COOP_KRC_INDEX
; of 1 signifies it is a local external axis.
IF (MECH_TYPE == #CONVEYOR) THEN
IF (ActualConveyorMode == #Absolute) THEN
EB_ABS_BASE(MACH_INDEX, FDAT_ACT.BASE_NO)
ELSE
EB_BASE(MACH_INDEX, FDAT_ACT.BASE_NO)
CONV_PART_NBR[MACH_INDEX] = 0
ENDIF
ELSE
IF (COOP_KRC_INDEX == 1) THEN
EX_BASE (FDAT_ACT.BASE_NO, MACH_INDEX )
ELSE
LK_BASE (MACH_INDEX, FDAT_ACT.BASE_NO, -1 )
ENDIF
ENDIF
ENDIF
ENDIF
END
;ENDFOLD FRAMES ()
;FOLD PTP_DAT ()
DEF PTP_DAT ( )
DECL APO_MODE_T MODE_CHECK
ACC_PTP (PDAT_ACT.ACC )
ON_ERROR_PROCEED
MODE_CHECK = PDAT_ACT.APO_MODE
IF $ERR.NUMBER == 0 THEN
IF PDAT_ACT.APO_MODE==#CDIS THEN
$APO.CDIS=PDAT_ACT.APO_DIST
ELSE
IF PDAT_ACT.APO_DIST > 100 THEN
$APO.CPTP=100
ELSE
$APO.CPTP=PDAT_ACT.APO_DIST
ENDIF
ENDIF
ELSE
IF PDAT_ACT.APO_DIST > 100 THEN
$APO.CPTP=100
ELSE
$APO.CPTP=PDAT_ACT.APO_DIST
ENDIF
ERR_CLEAR($ERR)
ENDIF
END
;ENDFOLD PTP_DAT ()
;FOLD CP_DAT ()
DEF CP_DAT ( )
DECL ORI_TYPE ORI_CHECK
DECL CIRC_TYPE CIRC_CHECK
ACC_CP (LDAT_ACT.ACC )
$APO.CDIS=LDAT_ACT.APO_DIST
ON_ERROR_PROCEED
ORI_CHECK = LDAT_ACT.ORI_TYP
IF $ERR.NUMBER == 0 THEN
$ORI_TYPE=LDAT_ACT.ORI_TYP
ELSE
$ORI_TYPE=#VAR
ERR_CLEAR($ERR)
ENDIF
ON_ERROR_PROCEED
CIRC_CHECK = LDAT_ACT.CIRC_TYP
IF $ERR.NUMBER == 0 THEN
$CIRC_TYPE=LDAT_ACT.CIRC_TYP
ELSE
$CIRC_TYPE=DEF_CIRC_TYP
ERR_CLEAR($ERR)
ENDIF
END
;ENDFOLD CP_DAT ()
;FOLD OUT_ASYNC ()
DEF OUT_ASYNC ( )
SWITCH ODAT_ACT.OUT_MODE
CASE #CONT_
IF ODAT_ACT.PULSE_TIME>0 THEN
CONTINUE
PULSE ($OUT[ODAT_ACT.OUT_NO],ODAT_ACT.STATE,ODAT_ACT.PULSE_TIME )
ELSE
CONTINUE
$OUT[ODAT_ACT.OUT_NO]=ODAT_ACT.STATE
ENDIF
CASE #STOP_
IF ODAT_ACT.PULSE_TIME>0 THEN
PULSE ($OUT[ODAT_ACT.OUT_NO],ODAT_ACT.STATE,ODAT_ACT.PULSE_TIME )
ELSE
$OUT[ODAT_ACT.OUT_NO]=ODAT_ACT.STATE
ENDIF
ENDSWITCH
END
;ENDFOLD OUT_ASYNC ()
;FOLD OUT_SYNC ()
DEF OUT_SYNC ( )
SWITCH ODAT_ACT.OUT_MODE
CASE #TRIGGER_
IF ODAT_ACT.PULSE_TIME>0 THEN
CONTINUE
PULSE ($OUT[ODAT_ACT.OUT_NO],ODAT_ACT.STATE,ODAT_ACT.PULSE_TIME )
ELSE
CONTINUE
$OUT[ODAT_ACT.OUT_NO]=ODAT_ACT.STATE
ENDIF
ENDSWITCH
END
;ENDFOLD OUT_SYNC ()
;FOLD GROUP ()
DEF GROUP (GROUP_NO :IN )
INT GROUP_NO
$ACT_GROUP=DEF_GROUP[GROUP_NO]
END
;ENDFOLD GROUP ()
;FOLD SET_IPO_MODE ()
DEF SET_IPO_MODE (IPO_FRAME :IN )
DECL IPO_MODE IPO_FRAME
IF $IPO_MODE==IPO_FRAME THEN
RETURN
ELSE
$IPO_MODE=IPO_FRAME
ENDIF
END
;ENDFOLD SET_IPO_MODE ()
;FOLD {H}
GLOBAL DEFFCT CP SVEL_CP (CP_VALUE :IN, ORI_VALUE :IN, PARAMS :IN )
REAL AX_VEL
REAL CP_VALUE
REAL ORI_VALUE
DECL CP RET_VEL
DECL LDAT PARAMS
IF CP_VALUE>$VEL_MA.CP THEN
RET_VEL.CP=$VEL_MA.CP
ELSE
RET_VEL.CP=CP_VALUE
ENDIF
RET_VEL.ORI1=DEF_VEL_ORIS
;for Spline-HFP
ON_ERROR_PROCEED
AX_VEL=PARAMS.AXIS_VEL
IF $ERR.NUMBER>0 THEN
ERR_CLEAR($ERR)
ELSE
IF PARAMS.AXIS_VEL>100.0 THEN
AX_VEL=DEF_VEL_PTP
ELSE
AX_VEL=DEF_VEL_PTP*PARAMS.AXIS_VEL/100.0
ENDIF
;APP// control structure not allowed here
$VEL_AXIS[1]=AX_VEL
$VEL_AXIS[2]=AX_VEL
$VEL_AXIS[3]=AX_VEL
$VEL_AXIS[4]=AX_VEL
$VEL_AXIS[5]=AX_VEL
$VEL_AXIS[6]=AX_VEL
IF RedVelExAx[1]>100.0 THEN
RedVelExAx[1]=100.0
ENDIF
IF RedVelExAx[1]<1.0 THEN
RedVelExAx[1]=1.0
ENDIF
IF RedVelExAx[2]>100.0 THEN
RedVelExAx[2]=100.0
ENDIF
IF RedVelExAx[2]<1.0 THEN
RedVelExAx[2]=1.0
ENDIF
IF RedVelExAx[3]>100.0 THEN
RedVelExAx[3]=100.0
ENDIF
IF RedVelExAx[3]<1.0 THEN
RedVelExAx[3]=1.0
ENDIF
IF RedVelExAx[4]>100.0 THEN
RedVelExAx[4]=100.0
ENDIF
IF RedVelExAx[4]<1.0 THEN
RedVelExAx[4]=1.0
ENDIF
IF RedVelExAx[5]>100.0 THEN
RedVelExAx[5]=100.0
ENDIF
IF RedVelExAx[5]<1.0 THEN
RedVelExAx[5]=1.0
ENDIF
IF RedVelExAx[6]>100.0 THEN
RedVelExAx[6]=100.0
ENDIF
IF RedVelExAx[6]<1.0 THEN
RedVelExAx[6]=1.0
ENDIF
$VEL_EXTAX[1]=AX_VEL*RedVelExAx[1]/100.0
$VEL_EXTAX[2]=AX_VEL*RedVelExAx[2]/100.0
$VEL_EXTAX[3]=AX_VEL*RedVelExAx[3]/100.0
$VEL_EXTAX[4]=AX_VEL*RedVelExAx[4]/100.0
$VEL_EXTAX[5]=AX_VEL*RedVelExAx[5]/100.0
$VEL_EXTAX[6]=AX_VEL*RedVelExAx[6]/100.0
ENDIF
;return CP-structure
RETURN RET_VEL
ENDFCT
GLOBAL DEFFCT REAL SVEL_JOINT (VEL_FACTOR :IN)
REAL VEL_FACTOR ; [%]
REAL VEL
IF VEL_FACTOR>100.0 THEN
VEL=DEF_VEL_PTP
ELSE
VEL=DEF_VEL_PTP*VEL_FACTOR/100.0
ENDIF
;$VEL_AXIS[1]=VEL
$VEL_AXIS[2]=VEL
$VEL_AXIS[3]=VEL
$VEL_AXIS[4]=VEL
$VEL_AXIS[5]=VEL
$VEL_AXIS[6]=VEL
IF RedVelExAx[1]>100.0 THEN
RedVelExAx[1]=100.0
ENDIF
IF RedVelExAx[1]<1.0 THEN
RedVelExAx[1]=1.0
ENDIF
IF RedVelExAx[2]>100.0 THEN
RedVelExAx[2]=100.0
ENDIF
IF RedVelExAx[2]<1.0 THEN
RedVelExAx[2]=1.0
ENDIF
IF RedVelExAx[3]>100.0 THEN
RedVelExAx[3]=100.0
ENDIF
IF RedVelExAx[3]<1.0 THEN
RedVelExAx[3]=1.0
ENDIF
IF RedVelExAx[4]>100.0 THEN
RedVelExAx[4]=100.0
ENDIF
IF RedVelExAx[4]<1.0 THEN
RedVelExAx[4]=1.0
ENDIF
IF RedVelExAx[5]>100.0 THEN
RedVelExAx[5]=100.0
ENDIF
IF RedVelExAx[5]<1.0 THEN
RedVelExAx[5]=1.0
ENDIF
IF RedVelExAx[6]>100.0 THEN
RedVelExAx[6]=100.0
ENDIF
IF RedVelExAx[6]<1.0 THEN
RedVelExAx[6]=1.0
ENDIF
$VEL_EXTAX[1]=VEL*RedVelExAx[1]/100.0
$VEL_EXTAX[2]=VEL*RedVelExAx[2]/100.0
$VEL_EXTAX[3]=VEL*RedVelExAx[3]/100.0
$VEL_EXTAX[4]=VEL*RedVelExAx[4]/100.0
$VEL_EXTAX[5]=VEL*RedVelExAx[5]/100.0
$VEL_EXTAX[6]=VEL*RedVelExAx[6]/100.0
RETURN VEL
ENDFCT
GLOBAL DEFFCT CP SACC_CP (PARAMS :IN)
REAL AX_ACC
DECL LDAT PARAMS
DECL CP RET_ACC
PARAMS.ACC=PARAMS.AXIS_ACC
IF PARAMS.ACC>100.0 THEN
RET_ACC.CP=DEF_ACC_CP
RET_ACC.ORI1=DEF_ACC_ORIS
ELSE
RET_ACC.CP=DEF_ACC_CP*PARAMS.ACC/100.0
RET_ACC.ORI1=DEF_ACC_ORIS*PARAMS.ACC/100.0
ENDIF
;for Spline-HFP
IF $ADAP_ACC==#NONE THEN;
IF PARAMS.AXIS_ACC>100.0 THEN
AX_ACC=DEF_ACC_SPTP
ELSE
AX_ACC=DEF_ACC_SPTP*PARAMS.AXIS_ACC/100.0
ENDIF
ELSE
AX_ACC=DEF_ACC_SPTP*PARAMS.AXIS_ACC/100.0
ENDIF
;APP// control structure not allowed here
$ACC_AXIS[1]=AX_ACC
$ACC_AXIS[2]=AX_ACC
$ACC_AXIS[3]=AX_ACC
$ACC_AXIS[4]=AX_ACC
$ACC_AXIS[5]=AX_ACC
$ACC_AXIS[6]=AX_ACC
IF RedAccExAx[1]>100.0 THEN
RedAccExAx[1]=100.0
ENDIF
IF RedAccExAx[1]<1.0 THEN
RedAccExAx[1]=1.0
ENDIF
IF RedAccExAx[2]>100.0 THEN
RedAccExAx[2]=100.0
ENDIF
IF RedAccExAx[2]<1.0 THEN
RedAccExAx[2]=1.0
ENDIF
IF RedAccExAx[3]>100.0 THEN
RedAccExAx[3]=100.0
ENDIF
IF RedAccExAx[3]<1.0 THEN
RedAccExAx[3]=1.0
ENDIF
IF RedAccExAx[4]>100.0 THEN
RedAccExAx[4]=100.0
ENDIF
IF RedAccExAx[4]<1.0 THEN
RedAccExAx[4]=1.0
ENDIF
IF RedAccExAx[5]>100.0 THEN
RedAccExAx[5]=100.0
ENDIF
IF RedAccExAx[5]<1.0 THEN
RedAccExAx[5]=1.0
ENDIF
IF RedAccExAx[6]>100.0 THEN
RedAccExAx[6]=100.0
ENDIF
IF RedAccExAx[6]<1.0 THEN
RedAccExAx[6]=1.0
ENDIF
$ACC_EXTAX[1]=AX_ACC*RedAccExAx[1]/100.0
$ACC_EXTAX[2]=AX_ACC*RedAccExAx[2]/100.0
$ACC_EXTAX[3]=AX_ACC*RedAccExAx[3]/100.0
$ACC_EXTAX[4]=AX_ACC*RedAccExAx[4]/100.0
$ACC_EXTAX[5]=AX_ACC*RedAccExAx[5]/100.0
$ACC_EXTAX[6]=AX_ACC*RedAccExAx[6]/100.0
;return CP-structure
RETURN RET_ACC
ENDFCT
GLOBAL DEFFCT REAL SACC_JOINT (PARAMS :IN)
DECL PDAT PARAMS
REAL AX_ACC
IF PARAMS.ACC>100.0 THEN
AX_ACC=DEF_ACC_SPTP
ELSE
AX_ACC=DEF_ACC_SPTP*PARAMS.ACC/100.0
ENDIF
;$ACC_AXIS[1]=AX_ACC
$ACC_AXIS[2]=AX_ACC
$ACC_AXIS[3]=AX_ACC
$ACC_AXIS[4]=AX_ACC
$ACC_AXIS[5]=AX_ACC
$ACC_AXIS[6]=AX_ACC
IF RedAccExAx[1]>100.0 THEN
RedAccExAx[1]=100.0
ENDIF
IF RedAccExAx[1]<1.0 THEN
RedAccExAx[1]=1.0
ENDIF
IF RedAccExAx[2]>100.0 THEN
RedAccExAx[2]=100.0
ENDIF
IF RedAccExAx[2]<1.0 THEN
RedAccExAx[2]=1.0
ENDIF
IF RedAccExAx[3]>100.0 THEN
RedAccExAx[3]=100.0
ENDIF
IF RedAccExAx[3]<1.0 THEN
RedAccExAx[3]=1.0
ENDIF
IF RedAccExAx[4]>100.0 THEN
RedAccExAx[4]=100.0
ENDIF
IF RedAccExAx[4]<1.0 THEN
RedAccExAx[4]=1.0
ENDIF
IF RedAccExAx[5]>100.0 THEN
RedAccExAx[5]=100.0
ENDIF
IF RedAccExAx[5]<1.0 THEN
RedAccExAx[5]=1.0
ENDIF
IF RedAccExAx[6]>100.0 THEN
RedAccExAx[6]=100.0
ENDIF
IF RedAccExAx[6]<1.0 THEN
RedAccExAx[6]=1.0
ENDIF
$ACC_EXTAX[1]=AX_ACC*RedAccExAx[1]/100.0
$ACC_EXTAX[2]=AX_ACC*RedAccExAx[2]/100.0
$ACC_EXTAX[3]=AX_ACC*RedAccExAx[3]/100.0
$ACC_EXTAX[4]=AX_ACC*RedAccExAx[4]/100.0
$ACC_EXTAX[5]=AX_ACC*RedAccExAx[5]/100.0
$ACC_EXTAX[6]=AX_ACC*RedAccExAx[6]/100.0
RETURN AX_ACC
ENDFCT
GLOBAL DEFFCT CONT SAPO (PARAMS :IN)
DECL LDAT PARAMS
DECL CONT RET_APO
RET_APO.CDIS=PARAMS.APO_DIST
RET_APO.CPTP=DEF_APO_CPTP
RET_APO.CORI=DEF_APO_CORIS
RETURN RET_APO
ENDFCT
GLOBAL DEFFCT CONT SAPO_PTP (PARAMS :IN)
DECL PDAT PARAMS
DECL CONT RET_APO
DECL APO_MODE_T MODE_CHECK
ON_ERROR_PROCEED
MODE_CHECK = PARAMS.APO_MODE
IF $ERR.NUMBER == 0 THEN
IF PARAMS.APO_MODE == #CPTP THEN
RET_APO.CDIS=1E6
RET_APO.CPTP=PARAMS.APO_DIST
ELSE
RET_APO.CDIS=PARAMS.APO_DIST
RET_APO.CPTP=100
ENDIF
ELSE
RET_APO.CDIS=PARAMS.APO_DIST
RET_APO.CPTP=DEF_APO_CPTP
ERR_CLEAR($ERR)
ENDIF
RET_APO.CORI=DEF_APO_CORIS
RETURN RET_APO
ENDFCT
GLOBAL DEFFCT FRAME STOOL (TOOL_NO :IN)
INT TOOL_NO
DECL FRAME RET_TOOL
IF (TOOL_NO<=0) THEN
IF TOOL_CORR_W_ON AND TOOL_CORR_ON THEN
RET_TOOL=TOOL_CORR_W:TOOL_CORR
ELSE
IF TOOL_CORR_ON THEN
RET_TOOL=TOOL_CORR
ELSE
IF TOOL_CORR_W_ON THEN
RET_TOOL=TOOL_CORR_W
ELSE
RET_TOOL=$NULLFRAME
ENDIF
ENDIF
ENDIF
ELSE
IF TOOL_CORR_W_ON AND TOOL_CORR_ON THEN
RET_TOOL=TOOL_CORR_W:TOOL_DATA[TOOL_NO]:TOOL_CORR
ELSE
IF TOOL_CORR_ON THEN
RET_TOOL=TOOL_DATA[TOOL_NO]:TOOL_CORR
ELSE
IF TOOL_CORR_W_ON THEN
RET_TOOL=TOOL_CORR_W:TOOL_DATA[TOOL_NO]
ELSE
RET_TOOL=TOOL_DATA[TOOL_NO]
ENDIF
ENDIF
ENDIF
ENDIF
RETURN RET_TOOL
ENDFCT
GLOBAL DEFFCT FRAME STOOL2 (FSP :IN)
DECL FDAT FSP
DECL FRAME RET_TOOL
$ACT_TOOL = FSP.TOOL_NO
$ACT_BASE = FSP.BASE_NO
RET_TOOL = STOOL(FSP.TOOL_NO)
RETURN RET_TOOL
ENDFCT
GLOBAL DEFFCT LOAD SLOAD (TOOL_NO :IN)
INT TOOL_NO
DECL LOAD RET_LOAD
IF (TOOL_NO<=0) THEN
IF $ADAP_ACC<>#NONE THEN
RET_LOAD.M=$DEF_L_M
RET_LOAD.CM=$DEF_L_CM
RET_LOAD.J=$DEF_L_J
RETURN RET_LOAD
ENDIF
ELSE
IF $ADAP_ACC<>#NONE THEN
IF LOAD_DATA[TOOL_NO].M<0 THEN
RET_LOAD.M=$DEF_L_M
RET_LOAD.CM=$DEF_L_CM
RET_LOAD.J=$DEF_L_J
RETURN RET_LOAD
ELSE
RET_LOAD.M=LOAD_DATA[TOOL_NO].M
RET_LOAD.CM=LOAD_DATA[TOOL_NO].CM
RET_LOAD.J=LOAD_DATA[TOOL_NO].J
IF ((RET_LOAD.M<>0) AND (RET_LOAD.J.X==0) AND (RET_LOAD.J.Y==0) AND (RET_LOAD.J.Z==0)) THEN
RET_LOAD.J=$DEF_L_J
ENDIF
RETURN RET_LOAD
ENDIF
ENDIF
ENDIF
RETURN $LOAD
ENDFCT
GLOBAL DEFFCT FRAME SBASE (BASE_NO :IN)
INT BASE_NO, JointDeviceIndex
DECL FRAME RET_BASE
IF (BASE_NO<=0) THEN
IF BASE_CORR_ON AND M_BAS_COR_ON THEN
RET_BASE=BASE_CORR:$WORLD:M_BASE_CORR
ELSE
IF M_BAS_COR_ON THEN
RET_BASE=$WORLD:M_BASE_CORR
ELSE
IF BASE_CORR_ON THEN
RET_BASE=BASE_CORR:$WORLD
ELSE
RET_BASE=$WORLD
ENDIF
ENDIF
ENDIF
IF bJointOffsetHasBeenSet THEN
bJointOffsetHasBeenSet = FALSE
IF iLastAxisNo > 0 THEN
$JOINT_OFFSET[iLastAxisNo] = 0
ENDIF
ENDIF
ELSE
IF BASE_CORR_ON AND M_BAS_COR_ON THEN
RET_BASE=BASE_CORR:BASE_DATA[BASE_NO]:M_BASE_CORR
ELSE
IF M_BAS_COR_ON THEN
RET_BASE=BASE_DATA[BASE_NO]:M_BASE_CORR
ELSE
IF BASE_CORR_ON THEN
RET_BASE=BASE_CORR:BASE_DATA[BASE_NO]
ELSE
RET_BASE=BASE_DATA[BASE_NO]
ENDIF
ENDIF
ENDIF
;Set $JOINT_OFFSET
IF JOINT_DEVICE_INDEX[BASE_NO] > 0 THEN
JointDeviceIndex = JOINT_DEVICE_INDEX[BASE_NO]
IF JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX > 0 THEN
IF IsDeviceDefinedSPL(JOINT_DEVICE[JointDeviceIndex].NAME[]) THEN
$JOINT_OFFSET[JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX] = EO(JOINT_DEVICE[JointDeviceIndex].NAME[], JOINT_DEVICE[JointDeviceIndex].OFFSET )
ELSE
$JOINT_OFFSET[JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX] = JOINT_DEVICE[JointDeviceIndex].OFFSET
ENDIF
bJointOffsetHasBeenSet = TRUE
iLastAxisNo = JOINT_DEVICE[JointDeviceIndex].AXIS_INDEX
ENDIF
ELSE
IF bJointOffsetHasBeenSet THEN
bJointOffsetHasBeenSet = FALSE
IF iLastAxisNo > 0 THEN
$JOINT_OFFSET[iLastAxisNo] = 0
ENDIF
ENDIF
ENDIF
ENDIF
RETURN RET_BASE
ENDFCT
GLOBAL DEFFCT IPO_MODE SIPO_MODE (IPO_FRAME :IN)
DECL IPO_MODE IPO_FRAME
RETURN IPO_FRAME
ENDFCT
GLOBAL DEFFCT ORI_TYPE SORI_TYP (PARAMS :IN)
DECL LDAT PARAMS
RETURN PARAMS.ORI_TYP
ENDFCT
GLOBAL DEFFCT CIRC_TYPE SCIRC_TYP (PARAMS :IN)
DECL LDAT PARAMS
RETURN PARAMS.CIRC_TYP
ENDFCT
GLOBAL DEFFCT CIRC_MODE SCIRC_M (PARAMS :IN)
DECL LDAT PARAMS
RETURN PARAMS.CB
ENDFCT
GLOBAL DEFFCT REAL SGEAR_JERK (PARAMS :IN )
DECL PDAT PARAMS
REAL GEAR_JERK
IF PARAMS.GEAR_JERK>100.0 THEN
GEAR_JERK=DEF_GEAR_JERK
ELSE
GEAR_JERK=DEF_GEAR_JERK*PARAMS.GEAR_JERK/100.0
ENDIF
;$GEAR_JERK[1]=GEAR_JERK
$GEAR_JERK[2]=GEAR_JERK
$GEAR_JERK[3]=GEAR_JERK
$GEAR_JERK[4]=GEAR_JERK
$GEAR_JERK[5]=GEAR_JERK
$GEAR_JERK[6]=GEAR_JERK
IF $EX_AX_NUM>0 THEN
$GEAR_JERK[7]=GEAR_JERK
$GEAR_JERK[8]=GEAR_JERK
$GEAR_JERK[9]=GEAR_JERK
$GEAR_JERK[10]=GEAR_JERK
$GEAR_JERK[11]=GEAR_JERK
$GEAR_JERK[12]=GEAR_JERK
ENDIF
RETURN GEAR_JERK
ENDFCT
GLOBAL DEFFCT JERK_STRUC SJERK (PARAMS :IN )
DECL JERK_STRUC RET_JERK
DECL LDAT PARAMS
REAL GEAR_JERK, J_FACTOR, J_C0, J_F0, J_D3
PARAMS.JERK_FAC=PARAMS.GEAR_JERK
;GEAR_JERK[1..6]for Spline-HFP
IF PARAMS.GEAR_JERK>100.0 THEN
GEAR_JERK=DEF_GEAR_JERK
ELSE
GEAR_JERK=DEF_GEAR_JERK*PARAMS.GEAR_JERK/100.0
ENDIF
;APP// control structure not allowed here
$GEAR_JERK[1]=GEAR_JERK
$GEAR_JERK[2]=GEAR_JERK
$GEAR_JERK[3]=GEAR_JERK
$GEAR_JERK[4]=GEAR_JERK
$GEAR_JERK[5]=GEAR_JERK
$GEAR_JERK[6]=GEAR_JERK
IF $EX_AX_NUM>0 THEN
$GEAR_JERK[7]=GEAR_JERK
$GEAR_JERK[8]=GEAR_JERK
$GEAR_JERK[9]=GEAR_JERK
$GEAR_JERK[10]=GEAR_JERK
$GEAR_JERK[11]=GEAR_JERK
$GEAR_JERK[12]=GEAR_JERK
ENDIF
;JERK
IF PARAMS.JERK_FAC>=100.0 THEN
RET_JERK.CP=$JERK_MA.CP
RET_JERK.ORI=$JERK_MA.ORI
RET_JERK.AX=$JERK_MA.AX
RETURN RET_JERK
ELSE
;APPBlock/*
;f(J_Factor) = linear for 0 <= J_Factor <= J_C0
;f(J_Factor) = cubic for J_C0 < J_Factor <= 1
;f(J_C0) = J_F0 with J_C0[0,1] and J_F0[0,1]
;J_C0 >= J_F0 >= 0
;APPBlock*/
J_F0=0.05
J_C0=0.50
J_FACTOR=PARAMS.JERK_FAC/100.0
IF J_FACTOR<=J_C0 THEN
J_FACTOR=J_F0/J_C0*J_FACTOR
ELSE
J_D3=(J_FACTOR-J_C0)/(1-J_C0)
J_D3=J_D3*J_D3*J_D3
J_FACTOR=J_F0/J_C0*J_FACTOR + (J_C0-J_F0)*J_D3/J_C0
ENDIF
RET_JERK.CP=$JERK_MA.CP*J_FACTOR
RET_JERK.ORI=$JERK_MA.ORI*J_FACTOR
RET_JERK.AX.A1=$JERK_MA.AX.A1*J_FACTOR
RET_JERK.AX.A2=$JERK_MA.AX.A2*J_FACTOR
RET_JERK.AX.A3=$JERK_MA.AX.A3*J_FACTOR
RET_JERK.AX.A4=$JERK_MA.AX.A4*J_FACTOR
RET_JERK.AX.A5=$JERK_MA.AX.A5*J_FACTOR
RET_JERK.AX.A6=$JERK_MA.AX.A6*J_FACTOR
IF $EX_AX_NUM>0 THEN
RET_JERK.AX.E1=$JERK_MA.AX.E1*J_FACTOR
RET_JERK.AX.E2=$JERK_MA.AX.E2*J_FACTOR
RET_JERK.AX.E3=$JERK_MA.AX.E3*J_FACTOR
RET_JERK.AX.E4=$JERK_MA.AX.E4*J_FACTOR
RET_JERK.AX.E5=$JERK_MA.AX.E5*J_FACTOR
RET_JERK.AX.E6=$JERK_MA.AX.E6*J_FACTOR
ENDIF
;return JERK-structure
RETURN RET_JERK
ENDIF
ENDFCT
GLOBAL DEFFCT INT S_EXAX_IGN(PARAMS :IN)
DECL LDAT PARAMS
RETURN PARAMS.EXAX_IGN
ENDFCT
GLOBAL DEFFCT INT S_EXAX_IGN2(PARAMS :IN)
DECL PDAT PARAMS
RETURN PARAMS.EXAX_IGN
ENDFCT
GLOBAL DEFFCT FRAME K_ROOT (BASE_NO :IN)
INT BASE_NO, IDX
IF BASE_NO > 0 THEN
IDX=MACHINE_FRAME_DAT[BASE_NO].MACH_DEF_INDEX
IF IDX > 0 THEN
RETURN MACHINE_DEF[IDX].ROOT
ENDIF
ENDIF
RETURN $NULLFRAME
ENDFCT
GLOBAL DEFFCT ESYS K_TYPE (BASE_NO :IN)
INT BASE_NO, IDX
IF BASE_NO > 0 THEN
IDX=MACHINE_FRAME_DAT[BASE_NO].MACH_DEF_INDEX
IF IDX > 0 THEN
RETURN MACHINE_DEF[IDX].MECH_TYPE
ENDIF
ENDIF
RETURN #NONE
ENDFCT
GLOBAL DEFFCT FRAME K_OFFS(BASE_NO :IN)
INT BASE_NO
DECL FRAME RET_BASE
IF (BASE_NO > 0) THEN
RET_BASE=BASE_DATA[BASE_NO]
ELSE
RET_BASE=$WORLD
ENDIF
IF BASE_CORR_ON THEN
RET_BASE=BASE_CORR:RET_BASE
ENDIF
IF M_BAS_COR_ON THEN
RET_BASE=RET_BASE:M_BASE_CORR
ENDIF
RETURN RET_BASE
ENDFCT
GLOBAL DEFFCT FRAME LK_OFFS(BASE_NO :IN, TOOL_NO :IN)
INT BASE_NO, TOOL_NO
IF (BASE_NO > -1) THEN
RETURN K_OFFS(BASE_NO)
ELSE
;linking to remote tool
IF (TOOL_NO > 0) THEN
RETURN TOOL_DATA[TOOL_NO]
ELSE
RETURN $NULLFRAME
ENDIF
ENDIF
ENDFCT
GLOBAL DEFFCT CHAR[15] K_ADDR(BASE_NO :IN)
INT BASE_NO, IDX
IF BASE_NO > 0 THEN
IDX=MACHINE_FRAME_DAT[BASE_NO].MACH_DEF_INDEX
IF IDX > 0 THEN
RETURN $COOP_KRC[MACHINE_DEF[IDX].COOP_KRC_INDEX].IP_ADDR[]
ENDIF
RETURN " "
ENDIF
ENDFCT
GLOBAL DEFFCT CHAR[40] K_NAME(BASE_NO :IN)
INT BASE_NO, IDX
IF BASE_NO > 0 THEN
IDX=MACHINE_FRAME_DAT[BASE_NO].MACH_DEF_INDEX
IF IDX > 0 THEN
RETURN MACHINE_DEF[IDX].NAME[]
ENDIF
ENDIF
RETURN " "
ENDFCT
GLOBAL DEFFCT INT K_SHIFT(BASE_NO :IN)
INT BASE_NO, IDX
IF BASE_NO > 0 THEN
IDX=MACHINE_FRAME_DAT[BASE_NO].MACH_DEF_INDEX
IF IDX > 0 THEN
RETURN MACHINE_DEF[IDX].COOP_KRC_INDEX
ENDIF
ENDIF
RETURN 0
ENDFCT
DEFFCT BOOL IsDeviceDefined(StringVar :IN)
DECL CHAR StringVar[]
INT StringLen, StringPos, CharValue
StringLen=STRLEN(StringVar[])
IF StringLen>0 THEN
FOR StringPos=1 TO StringLen
CharValue=StringVar[StringPos]
IF CharValue > 32 THEN
RETURN (TRUE)
ENDIF
ENDFOR
RETURN(FALSE)
ELSE
RETURN(FALSE)
ENDIF
ENDFCT
DEFFCT BOOL IsDeviceDefinedSPL(StringVar :IN)
DECL CHAR StringVar[]
INT CharValue
IF StringVar[1] > 32 THEN
IF StringVar[2] > 32 THEN
IF StringVar[3] > 32 THEN
RETURN (TRUE)
ENDIF
ENDIF
ELSE
RETURN(FALSE)
ENDIF
ENDFCT
GLOBAL DEF setFixedBaseIfLinked()
IF (FDAT_ACT.BASE_NO == $ACT_BASE) THEN
IF (STRCOMP($LK_MASTER[], " ", #CASE_SENS) == FALSE) THEN
$BASE=$NULLFRAME
$ACT_BASE=0
LIN_REL {X 0}
ENDIF
ENDIF
END
GLOBAL DEF TraceStart(StringDef :IN, StringName :IN, UseRingBuffer :IN)
DECL CHAR StringDef[], StringName[], tempName[64]
DECL INT counter, index, OFFSET, act_iter, max_iter
DECL BOOL compErg, UseRingBuffer, DummyBool
DECL STATE_T STATE
ON_ERROR_PROCEED
DummyBool = UseRingBuffer
IF $ERR.NUMBER > 0 THEN
UseRingBuffer = FALSE
ERR_CLEAR($ERR)
ENDIF
index = -1
FOR counter = 1 TO 10 STEP 1
compErg = STRCOMP(StringName[], TRACE_CACHE[counter].NAME[], #NOT_CASE_SENS)
IF compErg THEN
index = counter
continue
ENDIF
ENDFOR
IF index > 0 THEN
act_iter = TRACE_CACHE[index].ACT_ITER
max_iter = TRACE_CACHE[index].MAX_ITER
act_iter = act_iter + 1
IF act_iter > max_iter THEN
IF UseRingBuffer THEN
act_iter = 1
ELSE
RETURN
ENDIF
ENDIF
tempName[] = StringName[]
OFFSET = STRLEN(tempName[])
SWRITE(tempName[],STATE,OFFSET,"%d",act_iter)
TRACE_CACHE[index].ACT_ITER = act_iter
ELSE
tempName[] = StringName[]
ENDIF
$TRACE.MODE = #T_STOP
WAIT FOR $TRACE.STATE == #T_END
$TRACE.CONFIG[] = StringDef[]
$TRACE.NAME[] = tempName[]
$TRACE.MODE = #T_START
WAIT FOR $TRACE.STATE <> #T_END
WAIT SEC 0.012
END
GLOBAL DEF TraceStop()
WAIT SEC 0.012
$TRACE.MODE = #T_STOP
WAIT FOR $TRACE.STATE == #T_END
END
GLOBAL DEF TimerAction (Action :IN,TimerIndex :IN,TimerValue :IN)
INT TimerIndex, TimerValue
DECL TIMER_ACTIONTYPE Action
SWITCH Action
CASE #TimerStart
IF ($TIMER_STOP[TimerIndex]==FALSE) THEN
$TIMER_STOP[TimerIndex]=TRUE
MsgNotify("RunningTimerHasBeenStopped","BasisTech",TimerIndex,,26)
ENDIF
$TIMER[TimerIndex]=0
$TIMER_STOP[TimerIndex]=FALSE
CASE #TimerStop
$TIMER_STOP[TimerIndex]=TRUE
CASE #TimeOut
IF ($TIMER_STOP[TimerIndex]==FALSE) THEN
$TIMER_STOP[TimerIndex]=TRUE
MsgNotify("RunningTimerHasBeenStopped","BasisTech",TimerIndex,,26)
ENDIF
$TIMER[TimerIndex]=TimerValue
$TIMER_STOP[TimerIndex]=FALSE
ENDSWITCH
END
GLOBAL DEF BasisTech_PLC_INIT()
UpdateLoadModes ( )
InitHomePositions ( )
END
GLOBAL DEF BasisTech_PLC_LOOP()
DECL KRLMSG_T MSG
DECL KRLMSGPAR_T MsgPar[4]
DECL KRLMSGOPT_T Opt
DECL BOOL bDummy
MSG.Nr = 6789
MSG.MODUL[] = "TechHandler"
MSG.MSG_TXT[] = "EasyTeachKeepOriWarning"
Opt = {VL_STOP FALSE, CLEAR_P_RESET FALSE, LOG_TO_DB TRUE}
IF EasyTeachKeepOriWarning THEN
IF EasyTeachKeepOriHandle == 0 THEN
EasyTeachKeepOriHandle = SET_KRLMSG(#State,MSG,MsgPar[],Opt)
ENDIF
ELSE
IF EasyTeachKeepOriHandle > 0 THEN
bDummy=Clear_KrlMsg(EasyTeachKeepOriHandle)
IF bDummy THEN
EasyTeachKeepOriHandle = 0
ENDIF
ENDIF
ENDIF
END
GLOBAL DEFFCT BOOL IsMovingBase(BaseNumber:IN )
INT MACH_INDEX, COOP_KRC_INDEX, BaseNumber
DECL ESYS MECH_TYPE
IF (NOT (VARSTATE("BaseNumber") == #INITIALIZED)) THEN
BaseNumber = $ACT_BASE
ENDIF
IF (BaseNumber == 0) THEN
RETURN (FALSE)
ENDIF
MACH_INDEX = MACHINE_FRAME_DAT[BaseNumber].MACH_DEF_INDEX
IF (MACH_INDEX > 0) Then
MECH_TYPE = MACHINE_DEF[MACH_INDEX].MECH_TYPE
COOP_KRC_INDEX = MACHINE_DEF[MACH_INDEX].COOP_KRC_INDEX
ELSE
RETURN (FALSE)
ENDIF
IF (MECH_TYPE == #ROBOT) THEN
IF (COOP_KRC_INDEX > 1) THEN
;LK Base
RETURN (TRUE)
ELSE
RETURN (FALSE)
ENDIF
ELSE
IF (MECH_TYPE == #NONE) THEN
RETURN (FALSE)
ELSE
IF (MECH_TYPE == #CONVEYOR) THEN
;EB Base
RETURN (TRUE)
ELSE
;EK Base
RETURN (TRUE)
ENDIF
ENDIF
ENDIF
ENDFCT
GLOBAL DEF HopMove (StartPos :IN, TargetPos :IN, VelTransfer :IN,HopData :IN, MoveNotDirect :IN, TransferedType :OUT, StartBase :IN, TargetFdat:IN, isWithTrigger:IN)
BOOL isSCARA, isDELTA, isPtpPossible, MoveNotDirect, isPtpDesiredByCustomer, isMovingStart, isMovingTarget, isWithTrigger
BOOL MoveInBase, DummyBool
E6POS StartPos, TargetPos, MidPos, PosOverSP, PosOverTP
E6AXIS AxisOverSP, AxisOverTP, AxisMidPt
REAL StartVel, TargetVel, StartAcc, TargetAcc, TransferVel, VelTransfer
DECL HDAT HopData
INT TransferedType, DummyInt
DECL KrlMsgOpt_T MsgOpt
FRAME StartBase
DECL FDAT TargetFdat
MsgOpt={VL_Stop FALSE,Clear_P_Reset TRUE,Log_To_DB TRUE}
MoveInBase = FALSE
ON_ERROR_PROCEED
DummyInt = TargetFdat.Base_No
IF $ERR.NUMBER > 0 THEN
MoveInBase = TRUE
TargetFdat.BASE_NO = $ACT_BASE
ERR_CLEAR($ERR)
ENDIF
ON_ERROR_PROCEED
DummyInt = StartBase.X
IF $ERR.NUMBER > 0 THEN
StartBase = $BASE
MoveInBase = TRUE
ERR_CLEAR($ERR)
ENDIF
ON_ERROR_PROCEED
DummyBool = isWithTrigger
IF $ERR.NUMBER > 0 THEN
isWithTrigger = FALSE
ERR_CLEAR($ERR)
ENDIF
$CP_VEL_MODE = 5
isSCARA = (($MAIN_AXIS == #CC) AND ($NUM_AX == 4))
isDELTA = (($KINCLASS == #DELTA_KIN) AND ($NUM_AX == 4))
isPtpPossible = isSCARA AND NOT isWithTrigger
isPtpDesiredByCustomer = FALSE
isMovingStart = IsMovingBase($ACT_BASE)
isMovingTarget = IsMovingBase(TargetFdat.BASE_NO)
IF MoveNotDirect AND (isMovingStart OR isMovingTarget) THEN
MsgQuit ("HopIndirectAndMovingBaseNotPossible", "BasisTech", , ,39 ,MsgOpt)
ENDIF
IF HopData.VEL_START > $VEL_MA.CP * $CP_VEL_MODE THEN
StartVel = $VEL_MA.CP * $CP_VEL_MODE
ELSE
StartVel = HopData.VEL_START
ENDIF
IF HopData.VEL_DEST > $VEL_MA.CP * $CP_VEL_MODE THEN
TargetVel = $VEL_MA.CP * $CP_VEL_MODE
ELSE
TargetVel = HopData.VEL_DEST
ENDIF
IF HopData.ACC_START/100.0 > $CP_VEL_MODE THEN
StartAcc = $ACC_MA.CP * $CP_VEL_MODE
ELSE
StartAcc = $ACC_MA.CP * HopData.ACC_START/100.0
ENDIF
IF HopData.ACC_DEST/100.0 > $CP_VEL_MODE THEN
TargetAcc = $ACC_MA.CP * $CP_VEL_MODE
ELSE
TargetAcc = $ACC_MA.CP * HopData.ACC_DEST/100.0
ENDIF
IF VelTransfer > $VEL_MA.CP * $CP_VEL_MODE THEN
TransferVel = $VEL_MA.CP * $CP_VEL_MODE
ELSE
TransferVel = VelTransfer
ENDIF
IF $ON_PATH THEN
IF MoveInBase THEN
PosOverSP = StartPos
PosOverSP.Z = PosOverSP.Z + HopData.HT_Start
PosOverTP = TargetPos
PosOverTP.Z = PosOverTP.Z + HopData.HT_Dest
ELSE
PosOverSP = PositionToWorld(StartPos, $ACT_BASE)
PosOverSP.Z = PosOverSP.Z + HopData.HT_Start
PosOverTP = PositionToWorld(TargetPos, TargetFdat.BASE_NO)
PosOverTP.Z = PosOverTP.Z + HopData.HT_Dest
ENDIF
IF NOT ((isMovingStart OR isMovingTarget) OR (isPtpPossible OR isPtpDesiredByCustomer)) THEN
;change current Base to World..
IF NOT MoveInBase THEN
$BASE = $NULLFRAME
ENDIF
ERR_STATUS = 0
AxisOverSP = INVERSE (PosOverSP,$AXIS_ACT,ERR_STATUS)
CheckERRStatus()
AxisOverTP = INVERSE (PosOverTP,$AXIS_ACT,ERR_STATUS)
CheckERRStatus()
AxisMidPt.A1 = (AxisOverSP.A1 + AxisOverTP.A1) / 2
AxisMidPt.A2 = (AxisOverSP.A2 + AxisOverTP.A2) / 2
AxisMidPt.A3 = (AxisOverSP.A3 + AxisOverTP.A3) / 2
AxisMidPt.A4 = (AxisOverSP.A4 + AxisOverTP.A4) / 2
AxisMidPt.A5 = (AxisOverSP.A5 + AxisOverTP.A5) / 2
AxisMidPt.A6 = (AxisOverSP.A6 + AxisOverTP.A6) / 2
AxisMidPt.E1 = 0
AxisMidPt.E2 = 0
AxisMidPt.E3 = 0
AxisMidPt.E4 = 0
AxisMidPt.E5 = 0
AxisMidPt.E6 = 0
MidPos = FORWARD (AxisMidPt,ERR_STATUS)
CheckERRStatus()
IF MoveInBase THEN
MidPos.A = StartPos.A
MidPos.B = StartPos.B
MidPos.C = StartPos.C
ELSE
MidPos = PositionFromWorld(MidPos, TargetFdat.BASE_NO)
; Set Base to Current Base
$BASE = StartBase
ENDIF
ENDIF
IF NOT MoveInBase THEN
PosOverSP = PositionFromWorld(PosOverSP, $ACT_BASE)
PosOverTP = PositionFromWorld(PosOverTP, TargetFdat.BASE_NO)
ENDIF
ENDIF
IF ((NOT isMovingTarget) AND (NOT isMovingStart) AND (isPtpPossible)) THEN
IF $ON_PATH THEN
$VEL.CP = StartVel
$VEL.ORI1 = $CP_VEL_MODE * $VEL_MA.ORI1
$VEL.ORI2 = $CP_VEL_MODE * $VEL_MA.ORI2
$ACC.CP = StartAcc
$ACC.ORI1 = $CP_VEL_MODE * $ACC_MA.ORI1
$ACC.ORI2 = $CP_VEL_MODE * $ACC_MA.ORI2
IF (HopData.HT_Start - HopData.APO_START > 0) THEN
$APO.CDIS = HopData.HT_Start - HopData.APO_START
PTP PosOverSP C_DIS
ELSE
PTP PosOverSP
ENDIF
IF NOT MoveInBase THEN
;-----------------------------
; Set TOOL,BASE to Target Point
FDAT_ACT = TargetFdat
BAS(#FRAMES)
;-----------------------------
ENDIF
IF (HopData.HT_Dest - HopData.APO_DEST > 0) THEN
$APO.CDIS = HopData.HT_Dest - HopData.APO_DEST
ON_ERROR_PROCEED
PTP PosOverTP C_DIS
IF $ERR.NUMBER > 0 THEN
SWITCH $ERR.NUMBER
CASE 1447
;APP// software limit switch point cannot be reached
LIN PosOverTP
DEFAULT
MsgQuit("UnexpectedErrorDuringHopMovement","BasisTech",$ERR.NUMBER, ,38 ,MsgOpt)
ENDSWITCH
ERR_CLEAR($ERR)
ENDIF
ELSE
ON_ERROR_PROCEED
PTP PosOverTP
IF $ERR.NUMBER > 0 THEN
SWITCH $ERR.NUMBER
CASE 1447
;APP// software limit switch point cannot be reached
LIN PosOverTP
DEFAULT
MsgQuit("UnexpectedErrorDuringHopMovement","BasisTech",$ERR.NUMBER, ,38 ,MsgOpt)
ENDSWITCH
ERR_CLEAR($ERR)
ENDIF
ENDIF
$VEL.CP = TargetVel
$ACC.CP = TargetAcc
IF MoveInBase THEN
PTP TargetPos
ENDIF
ELSE
;APP// not on path (block selection)
IF MoveInBase THEN
LIN TargetPos
ENDIF
ENDIF
ELSE
;APP// no ptp movement
IF MoveNotDirect THEN
IF $ON_PATH THEN
TransferedType = 2 ;Forced Indirect
$VEL.CP = StartVel
$VEL.ORI1 = $CP_VEL_MODE * $VEL_MA.ORI1
$VEL.ORI2 = $CP_VEL_MODE * $VEL_MA.ORI2
$ACC.CP = StartAcc
$ACC.ORI1 = $CP_VEL_MODE * $ACC_MA.ORI1
$ACC.ORI2 = $CP_VEL_MODE * $ACC_MA.ORI2
IF (HopData.HT_Start - HopData.APO_START > 0) THEN
$APO.CDIS = HopData.HT_Start - HopData.APO_START
LIN PosOverSP C_DIS
ELSE
LIN PosOverSP
ENDIF
IF NOT MoveInBase THEN
;-----------------------------
; Set TOOL,BASE to Target Point
FDAT_ACT = TargetFdat
BAS(#FRAMES)
;-----------------------------
ENDIF
$VEL.CP = TransferVel
$APO.CDIS = 1000.0
LIN MidPos C_DIS
IF (HopData.HT_Dest - HopData.APO_DEST > 0) THEN
$APO.CDIS = HopData.HT_Dest - HopData.APO_DEST
LIN PosOverTP C_DIS
ELSE
LIN PosOverTP
ENDIF
$VEL.CP = TargetVel
$ACC.CP = TargetAcc
IF MoveInBase THEN
LIN TargetPos
ENDIF
ELSE
IF MoveInBase THEN
LIN TargetPos
ELSE
;-----------------------------
; Set TOOL,BASE to Target Point
FDAT_ACT = TargetFdat
BAS(#FRAMES)
;-----------------------------
ENDIF
ENDIF
;APP// no ptp movement, direct
ELSE
IF $ON_PATH THEN
$VEL.CP = StartVel
$VEL.ORI1 = $CP_VEL_MODE * $VEL_MA.ORI1
$VEL.ORI2 = $CP_VEL_MODE * $VEL_MA.ORI2
$ACC.CP = StartAcc
$ACC.ORI1 = $CP_VEL_MODE * $ACC_MA.ORI1
$ACC.ORI2 = $CP_VEL_MODE * $ACC_MA.ORI2
IF (HopData.HT_Start - HopData.APO_START > 0) THEN
$APO.CDIS = HopData.HT_Start - HopData.APO_START
LIN PosOverSP C_DIS
ELSE
LIN PosOverSP
ENDIF
IF NOT MoveInBase THEN
;-----------------------------
; Set TOOL,BASE to Target Point
FDAT_ACT = TargetFdat
BAS(#FRAMES)
;-----------------------------
ENDIF
$VEL.CP = TransferVel
IF (HopData.HT_Dest - HopData.APO_DEST > 0) THEN
$APO.CDIS = HopData.HT_Dest - HopData.APO_DEST
ON_ERROR_PROCEED
LIN PosOverTP C_DIS
IF $ERR.NUMBER > 0 THEN
TransferedType = 1 ;Indirect
$APO.CDIS = 1000.0
IF NOT (isMovingStart OR isMovingTarget) THEN
LIN MidPos C_DIS
ENDIF
$APO.CDIS = HopData.HT_Dest - HopData.APO_DEST
LIN PosOverTP C_DIS
ERR_CLEAR($ERR)
ELSE
TransferedType = 0 ;Direct
ENDIF
ELSE
ON_ERROR_PROCEED
LIN PosOverTP
IF $ERR.NUMBER > 0 THEN
TransferedType = 1 ; Indirect
$APO.CDIS = 1000.0
IF NOT (isMovingStart OR isMovingTarget) THEN
LIN MidPos C_DIS
ENDIF
LIN PosOverTP
ERR_CLEAR($ERR)
ELSE
TransferedType = 0 ;Direct
ENDIF
ENDIF
$VEL.CP = TargetVel
$ACC.CP = TargetAcc
IF MoveInBase THEN
LIN TargetPos
ENDIF
ELSE
;APP// not on path (block selection)
IF MoveInBase THEN
LIN TargetPos
ELSE
;-----------------------------
; Set TOOL,BASE to Target Point
FDAT_ACT = TargetFdat
BAS(#FRAMES)
;-----------------------------
ENDIF
ENDIF
ENDIF
ENDIF
IF MoveInBase THEN
$ACC.CP = DEF_ACC_CP
$ACC.ORI1 = DEF_ACC_ORI1
$ACC.ORI2 = DEF_ACC_ORI2
$VEL.CP = DEF_VEL_CP
$VEL.ORI1 = DEF_VEL_ORI1
$VEL.ORI2 = DEF_VEL_ORI2
$CP_VEL_MODE = 0
ENDIF
END
DEF CheckERRStatus()
DECL INT ERR_STAT
DECL KrlMsgOpt_T MsgOpt
MsgOpt={VL_Stop FALSE,Clear_P_Reset TRUE,Log_To_DB TRUE}
ERR_STAT = ERR_STATUS
ERR_STATUS = 0
REPEAT
SWITCH ERR_STAT
CASE 0
RETURN
CASE 1
MsgQuit ("HopPositionWrongCalculation1", "BasisTech", , ,36 ,MsgOpt)
CASE 2,3
MsgQuit ("HopPositionWrongCalculation2", "BasisTech", , ,37 ,MsgOpt)
ENDSWITCH
UNTIL FALSE
END
DEF InitHomePositions()
DECL KrlMsgOpt_T MsgOpt
INT nHandle
BOOL bDummy
MsgOpt={VL_Stop FALSE,Clear_P_Reset TRUE,Log_To_DB TRUE}
IF ($KINCLASS == #DELTA_KIN) OR ($MAIN_AXIS == #CC) THEN
IF (XHOME.A2 == -90) AND (XHOME.A3 == 90) THEN
XHOME = $H_POS
MsgState (nHandle,"XHomeInitialized", "BasisTech", , ,38 ,MsgOpt)
bDummy=CLEAR_KRLMSG (nHandle)
ENDIF
IF (XHOME1.A2 == -90) AND (XHOME1.A3 == 90) THEN
XHOME1 = $AXIS_HOME[1]
MsgState (nHandle,"XHome1Initialized", "BasisTech", , ,39 ,MsgOpt)
bDummy=CLEAR_KRLMSG (nHandle)
ENDIF
IF (XHOME2.A2 == -90) AND (XHOME2.A3 == 90) THEN
XHOME2 = $AXIS_HOME[2]
MsgState (nHandle,"XHome2Initialized", "BasisTech", , ,40 ,MsgOpt)
bDummy=CLEAR_KRLMSG (nHandle)
ENDIF
IF (XHOME3.A2 == -90) AND (XHOME3.A3 == 90) THEN
XHOME3 = $AXIS_HOME[3]
MsgState (nHandle,"XHome3Initialized", "BasisTech", , ,41 ,MsgOpt)
bDummy=CLEAR_KRLMSG (nHandle)
ENDIF
IF (XHOME4.A2 == -90) AND (XHOME4.A3 == 90) THEN
XHOME4 = $AXIS_HOME[4]
MsgState (nHandle,"XHome4Initialized", "BasisTech", , ,42 ,MsgOpt)
bDummy=CLEAR_KRLMSG (nHandle)
ENDIF
IF (XHOME5.A2 == -90) AND (XHOME5.A3 == 90) THEN
XHOME5 = $AXIS_HOME[5]
MsgState (nHandle,"XHome5Initialized", "BasisTech", , ,43 ,MsgOpt)
bDummy=CLEAR_KRLMSG (nHandle)
ENDIF
ENDIF
END
GLOBAL DEF SetDefaultValues()
$ACC.CP = DEF_ACC_CP
$ACC.ORI1 = DEF_ACC_ORI1
$ACC.ORI2 = DEF_ACC_ORI2
$VEL.CP = DEF_VEL_CP
$VEL.ORI1 = DEF_VEL_ORI1
$VEL.ORI2 = DEF_VEL_ORI2
$CP_VEL_MODE = 0
END
DEFFCT E6POS PositionToWorld(Position:IN, PositionBaseNumber:IN)
DECL E6POS Position, Result
DECL INT PositionBaseNumber, MACH_INDEX, PartNumber
DECL FRAME MachineFrame, PositionBase
DECL BOOL Part_Present
DECL CHAR ConveyorName[40]
MachineFrame = $NULLFRAME
IF(PositionBaseNumber > 0) THEN
PositionBase = BASE_DATA[PositionBaseNumber]
MACH_INDEX = MACHINE_FRAME_DAT[PositionBaseNumber].MACH_DEF_INDEX
IF MACH_INDEX > 0 THEN
MachineFrame = MACHINE_DEF[MACH_INDEX].ROOT
IF MACHINE_DEF[MACH_INDEX].MECH_TYPE == #CONVEYOR THEN
ConveyorName[] = MACHINE_DEF[MACH_INDEX].Name[]
PartNumber = CONV_PART_NBR[MACH_INDEX]
Result = EB_TEST(MachineFrame, ConveyorName[], PositionBase, PartNumber, Part_Present)
Result = Result : Position
RETURN RESULT
ENDIF
ENDIF
ELSE
PositionBase = $NULLFRAME
ENDIF
;calculate the position in world
Result = MachineFrame:PositionBase:Position
RETURN Result
ENDFCT
DEFFCT E6POS PositionFromWorld(Position:IN, PositionBaseNumber:IN)
DECL FRAME PositionBase, MachineFrame
DECL E6POS Position, Result
DECL INT PositionBaseNumber , MACH_INDEX, PartNumber
DECL BOOL Part_Present
DECL CHAR ConveyorName[40]
MachineFrame = $NULLFRAME
IF(PositionBaseNumber > 0) THEN
PositionBase = BASE_DATA[PositionBaseNumber]
MACH_INDEX = MACHINE_FRAME_DAT[PositionBaseNumber].MACH_DEF_INDEX
IF MACH_INDEX > 0 THEN
MachineFrame = MACHINE_DEF[MACH_INDEX].ROOT
IF MACHINE_DEF[MACH_INDEX].MECH_TYPE == #CONVEYOR THEN
ConveyorName[] = MACHINE_DEF[MACH_INDEX].Name[]
PartNumber = CONV_PART_NBR[MACH_INDEX]
Result = EB_TEST(MachineFrame, ConveyorName[], PositionBase, PartNumber, Part_Present)
Result = INV_POS(Result) : Position
RETURN RESULT
ENDIF
ENDIF
ELSE
PositionBase = $NULLFRAME
ENDIF
;Position in World => Position in Base..
Result = INV_POS(PositionBase): INV_POS(MachineFrame) :Position
RETURN Result
ENDFCT
;ENDFOLD
Reference in New Issue View Git Blame Copy Permalink