机械手自动化程序

1、主程序main

&ACCESS RVP1

&REL 1

&PARAM DISKPATH = KRC:\R1\Program

DEF Pro_main( )

INT l

;FOLD INI;%{PE}

;FOLD BASISTECH INI

GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )

INTERRUPT ON 3

BAS (#INITMOV,0 )

;ENDFOLD (BASISTECH INI)

;FOLD USER INI

;Make your modifications here

;ENDFOLD (USER INI)

GLOBAL INTERRUPT DECL 4 WHEN $IN[10]==FALSE DO Itrpt1 ()

GLOBAL INTERRUPT DECL 5 WHEN $IN[11]==FALSE DO Itrpt1 ()

INTERRUPT DECL 6 WHEN $IN[9]==TRUE DO Itrpt1 ()；地轨异常进入中断

INTERRUPT ON 6；开地轨监视中断6

IF NOT Gripper_ChkStatus(1,"C") THEN

Gripper_SetStatus(1,"O")

Part0InLeft = FALSE

Part2InLeft = FALSE

ENDIF

IF NOT Gripper_ChkStatus(2,"C") THEN

Gripper_SetStatus(2,"O")

Part1InRight = FALSE

ENDIF

FOR l = 1 TO 14

$OUT[l] = FALSE

ENDFOR；此循环语句在初始化时把所有端口清零复位。

IF $IN[5] THEN

Machine1_OnlyDrop = TRUE

Machine2_OnlyDrop = TRUE；机台1有首件信号时，机台1与机台2第一次响应只放标志位置1 ELSE

Machine1_OnlyDrop = FALSE

Machine2_OnlyDrop = FALSE

ENDIF

$FLAG[8]=FALSE；FLAG[8]是干什么用的？

;ENDFOLD (INI)；初始化完成

$OV_PRO=70；限定系统运行速度

;FOLD PTP HOME Vel

$BWDSTART=FALSE

PDAT_ACT=PDEFAULT

FDAT_ACT=FHOME

BAS(#PTP_PARAMS,100)

$H_POS=XHOME

PTP XHOME

;ENDFOLD

IF $IN_HOME THEN

$OUT[5]=FALSE

$OUT[8]=FALSE

$OUT[11]=FALSE

ENDIF

LOOP

WAIT SEC 0.05

IF $IN[1] AND NOT Part0InLeft AND Gripper_ChkStatus(3,"O") THEN ToPick_Search()

ENDIF

IF Part0InLeft AND ($IN[3] OR Machine1_OnlyDrop) AND Gripper_ChkStatus(1,"C") AND Gripper_ChkStatus(2,"O") THEN

Machine1()

ENDIF

IF Part1InRight AND ($IN[6] OR Machine2_OnlyDrop) AND Gripper_ChkStatus(2,"C") AND Gripper_ChkStatus(1,"O") THEN

Machine2()

ENDIF

IF Part2InLeft AND ($IN[8] OR Machine3_OnlyDrop) AND Gripper_ChkStatus(1,"C") THEN

Machine3()

ENDIF

ENDLOOP

END

DEF Itrpt1 ()

HALT

BRAKE

END

2、取料程序（采用中断的方法自动探料，完成取料作业）

&ACCESS RVO2

&REL 1

&PARAM DISKPATH = KRC:\R1\Program

DEF ToPick_Search( )

;FOLD INI;%{PE}

;FOLD BASISTECH INI

GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )；单独用了一个系统中断？

INTERRUPT ON 3

BAS (#INITMOV,0 )

;ENDFOLD (BASISTECH INI)

;FOLD USER INI

;Make your modifications here

;ENDFOLD (USER INI)

;ENDFOLD (INI)

INTERRUPT DECL 1 WHEN $IN[10]==TRUE DO FOUND()

SevenAxis_Run(1)

GOTO MARK

;FOLD PTP P1 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT2

FDAT_ACT=FP1

BAS(#PTP_PARAMS,100)

PTP XP1 C_DIS

;ENDFOLD

;FOLD PTP P3 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT8

FDAT_ACT=FP3

BAS(#PTP_PARAMS,100)

PTP XP3 C_DIS

;ENDFOLD

;FOLD LIN P2 CONT Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT2

FDAT_ACT=FP2

BAS(#CP_PARAMS,2)

LIN XP2 C_DIS C_DIS

;ENDFOLD

;FOLD LIN P1 Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT1

FDAT_ACT=FP1

BAS(#CP_PARAMS,0.05)

LIN XP1

;ENDFOLD

;FOLD PTP HOME Vel

$BWDSTART=FALSE

PDAT_ACT=PDEFAULT

FDAT_ACT=FHOME

BAS(#PTP_PARAMS,100)

$H_POS=XHOME

PTP XHOME

;ENDFOLD

mark:

Search()

INTERRUPT OFF 1

WAIT SEC 0

XP4=$POS_ACT

;FOLD LIN P4 CONT Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT10

FDAT_ACT=FP4

BAS(#CP_PARAMS,2)

LIN XP4 C_DIS C_DIS

;ENDFOLD

Gripper_SetStatus(1,"O")

LIN_REL{Z 15}

LIN_REL{Z -17}

Gripper_SetStatus(1,"C")

Part0InLeft = TRUE

WAIT SEC 0

XP0=$POS_ACT

XP0.Z=XP0.Z+25 ；看一下寻料程序到底是怎么运转的！

;FOLD LIN P0 CONT Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT8

FDAT_ACT=FP0

BAS(#CP_PARAMS,0.025)

LIN XP0 C_DIS C_DIS

;ENDFOLD

WAIT SEC 0

IF NOT $IN[10] THEN

GOTO mark

ENDIF

;FOLD LIN P1 CONT Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT7

FDAT_ACT=FP1

BAS(#CP_PARAMS,2)

LIN XP1 C_DIS C_DIS

;ENDFOLD

END

DEF Search( )

;FOLD INI;%{PE}

;FOLD BASISTECH INI

GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )

INTERRUPT ON 3

BAS (#INITMOV,0 )

;ENDFOLD (BASISTECH INI)

;FOLD USER INI

;Make your modifications here

;ENDFOLD (USER INI)

;ENDFOLD (INI)

INTERRUPT ON 1

;FOLD PTP P1 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT6

FDAT_ACT=FP1

BAS(#PTP_PARAMS,100)

PTP XP1 C_DIS

;ENDFOLD

$OUT[16]=TRUE

;FOLD LIN P3 CONT Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT11

FDAT_ACT=FP3

BAS(#CP_PARAMS,2)

LIN XP3 C_DIS C_DIS

;ENDFOLD

;FOLD LIN P2 CONT Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT5

FDAT_ACT=FP2

BAS(#CP_PARAMS,0.05)

LIN XP2 C_DIS C_DIS

;ENDFOLD

;FOLD LIN P1 Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT6

FDAT_ACT=FP1

BAS(#CP_PARAMS,0.02)

LIN XP1

;ENDFOLD

WAIT SEC 0

END

DEF FOUND()

BRAKE

LIN $POS_INT ;rise a little

RESUME

END

3、机台1取放料程序（完成机台1取放料动作与信号交互）

&ACCESS RVO2

&REL 1

&PARAM DISKPATH = KRC:\R1\Program

DEF Machine1( )

;FOLD INI;%{PE}

;FOLD BASISTECH INI

GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( ) INTERRUPT ON 3

BAS (#INITMOV,0 )

;ENDFOLD (BASISTECH INI)

;FOLD USER INI

;Make your modifications here

;ENDFOLD (USER INI)

;ENDFOLD (INI)

SevenAxis_Run(1)

$OUT[5]=TRUE

;ENDFOLD

IF NOT Machine1_OnlyDrop THEN

;FOLD PTP P1 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT4

FDAT_ACT=FP1

BAS(#PTP_PARAMS,100)

PTP XP1 C_DIS

;ENDFOLD

mark:

;FOLD LIN P2 CONT Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT4

FDAT_ACT=FP2

BAS(#CP_PARAMS,2)

LIN XP2 C_DIS C_DIS

;ENDFOLD

;FOLD LIN P3 CONT Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT5

FDAT_ACT=FP3

BAS(#CP_PARAMS,2)

LIN XP3 C_DIS C_DIS

;ENDFOLD

Gripper_SetStatus(2,"C") LIN_REL{Z 7.5}

Gripper_SetStatus(2,"O")

;FOLD LIN P3 Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT1

FDAT_ACT=FP3

BAS(#CP_PARAMS,2)

LIN XP3

;ENDFOLD

Gripper_SetStatus(2,"C") Part1InRight = TRUE

;FOLD LIN P2 Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT6

FDAT_ACT=FP2

BAS(#CP_PARAMS,0.05)

LIN XP2

;ENDFOLD

IF NOT $IN[11] THEN

GOTO mark

ENDIF

;FOLD PTP P4 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT6

FDAT_ACT=FP4

BAS(#PTP_PARAMS,100)

PTP XP4 C_DIS

;ENDFOLD

;FOLD PTP P5 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT10

FDAT_ACT=FP5

BAS(#PTP_PARAMS,100)

PTP XP5 C_DIS

;ENDFOLD

;FOLD PTP P6 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT8

FDAT_ACT=FP6

BAS(#PTP_PARAMS,100)

PTP XP6 C_DIS

;ENDFOLD

;FOLD LIN P7 Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT7

FDAT_ACT=FP7

BAS(#CP_PARAMS,0.25)

LIN XP7

;ENDFOLD

Gripper_SetStatus(1,"O")

;FOLD PTP P6 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT14

FDAT_ACT=FP6

BAS(#PTP_PARAMS,100)

PTP XP6 C_DIS

;ENDFOLD

;FOLD PTP P8 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT9

FDAT_ACT=FP8

BAS(#PTP_PARAMS,100)

PTP XP8 C_DIS

;ENDFOLD

ELSE

;FOLD PTP P5 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT2

FDAT_ACT=FP5

BAS(#PTP_PARAMS,100)

PTP XP5 C_DIS

;ENDFOLD

;FOLD PTP P6 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT13

FDAT_ACT=FP6

BAS(#PTP_PARAMS,100)

PTP XP6 C_DIS

;ENDFOLD

;FOLD LIN P7 Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT9

FDAT_ACT=FP7

BAS(#CP_PARAMS,0.25)

LIN XP7

Gripper_SetStatus(1,"O")

;FOLD PTP P6 Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT12

FDAT_ACT=FP6

BAS(#PTP_PARAMS,100)

PTP XP6

;ENDFOLD

;FOLD PTP P8 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT7

FDAT_ACT=FP8

BAS(#PTP_PARAMS,100)

PTP XP8 C_DIS

;ENDFOLD

Machine_Start(1)

;FOLD PTP HOME Vel

$BWDSTART=FALSE

PDAT_ACT=PDEFAULT

FDAT_ACT=FHOME

BAS(#PTP_PARAMS,100)

$H_POS=XHOME

PTP XHOME

;ENDFOLD

ENDIF

IF Machine1_OnlyDrop THEN

Machine1_OnlyDrop = FALSE

ENDIF

Machine_Start(1)

END

4、机台2取放料程序（完成机台2取放料动作与信号交互）

&ACCESS RV2

&REL 6

&PARAM DISKPATH = KRC:\R1\Program

DEF Machine2()

;FOLD INI;%{PE}

;FOLD BASISTECH INI

GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( ) INTERRUPT ON 3

BAS (#INITMOV,0 )

;ENDFOLD (BASISTECH INI)

;FOLD USER INI

;Make your modifications here

;ENDFOLD (USER INI)

;ENDFOLD (INI)

PTP $POS_ACT

SevenAxis_Run(2)

$OUT[8]=TRUE

IF NOT Machine2_OnlyDrop THEN ;FOLD PTP P1 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT3

FDAT_ACT=FP1

BAS(#PTP_PARAMS,100)

PTP XP1 C_DIS

;ENDFOLD

mark:

;FOLD PTP P2 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT4

FDAT_ACT=FP2

BAS(#PTP_PARAMS,100)

PTP XP2 C_DIS

;ENDFOLD

;FOLD LIN P3 Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT4

FDAT_ACT=FP3

BAS(#CP_PARAMS,2)

LIN XP3

;ENDFOLD

Gripper_SetStatus(1,"C")

LIN_REL{Z 7.5}

Gripper_SetStatus(1,"O")

;FOLD LIN P3 Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT0

FDAT_ACT=FP3

BAS(#CP_PARAMS,2)

LIN XP3

;ENDFOLD

Gripper_SetStatus(1,"C")

Part2InLeft = TRUE

;FOLD LIN P2 CONT Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT3

FDAT_ACT=FP2

BAS(#CP_PARAMS,0.05)

LIN XP2 C_DIS C_DIS

;ENDFOLD

IF NOT $IN[10] THEN

GOTO mark

ENDIF

;FOLD PTP P4 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT7

FDAT_ACT=FP4

BAS(#PTP_PARAMS,100)

PTP XP4 C_DIS

;ENDFOLD

;FOLD PTP P5 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT14

FDAT_ACT=FP5

BAS(#PTP_PARAMS,100)

PTP XP5 C_DIS

;ENDFOLD

;FOLD PTP P6 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT9

FDAT_ACT=FP6

BAS(#PTP_PARAMS,100)

PTP XP6 C_DIS

;ENDFOLD

;FOLD LIN P7 Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT7

FDAT_ACT=FP7

BAS(#CP_PARAMS,0.25)

LIN XP7

;ENDFOLD

Gripper_SetStatus(2,"O")

;FOLD PTP P6 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT11

FDAT_ACT=FP6

BAS(#PTP_PARAMS,100)

PTP XP6 C_DIS

;ENDFOLD

;FOLD PTP P8 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT12

FDAT_ACT=FP8

BAS(#PTP_PARAMS,100)

PTP XP8 C_DIS

;ENDFOLD

ELSE

;FOLD PTP P5 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT13

FDAT_ACT=FP5

BAS(#PTP_PARAMS,100)

PTP XP5 C_DIS

;ENDFOLD

;FOLD PTP P6 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT1

FDAT_ACT=FP6

BAS(#PTP_PARAMS,100)

PTP XP6 C_DIS

;ENDFOLD

;FOLD LIN P7 Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT1

FDAT_ACT=FP7

BAS(#CP_PARAMS,0.25)

LIN XP7

;ENDFOLD

Gripper_SetStatus(2,"O")

;FOLD LIN P6 CONT Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT2

FDAT_ACT=FP6

BAS(#CP_PARAMS,2)

LIN XP6 C_DIS C_DIS

;ENDFOLD

;FOLD PTP P8 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT6

FDAT_ACT=FP8

BAS(#PTP_PARAMS,100)

PTP XP8 C_DIS

;ENDFOLD

Machine_Start(2)

;FOLD PTP HOME Vel5:100, 7:DEFAULT

$BWDSTART=FALSE

PDAT_ACT=PDEFAULT

FDAT_ACT=FHOME

BAS(#PTP_PARAMS,100)

$H_POS=XHOME

PTP XHOME

;ENDFOLD

ENDIF

IF Machine2_OnlyDrop THEN

Machine2_OnlyDrop = FALSE

ENDIF

Machine_Start(2)

END

5、机台3取放料程序（完成机台3取放料动作与信号交互）&ACCESS RVP1

&REL 6

DEF Machine3( )

;FOLD INI;%{PE}

;FOLD BASISTECH INI

GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( ) INTERRUPT ON 3

BAS (#INITMOV,0 )

;ENDFOLD (BASISTECH INI)

;FOLD USER INI

;Make your modifications here

;ENDFOLD (USER INI)

WAIT FOR $FLAG[1]

$FLAG[8]=TRUE

;ENDFOLD (INI)

PTP $POS_ACT

SevenAxis_Run(3)

$OUT[11]=TRUE

;ENDFOLD

;FOLD PTP P1 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT6

FDAT_ACT=FP1

BAS(#PTP_PARAMS,100)

PTP XP1 C_DIS

;ENDFOLD

;FOLD PTP P2 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT2

FDAT_ACT=FP2

BAS(#PTP_PARAMS,100)

PTP XP2 C_DIS

;ENDFOLD

;FOLD LIN P3 Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT1

FDAT_ACT=FP3

BAS(#CP_PARAMS,0.25)

LIN XP3

;ENDFOLD

Gripper_SetStatus(1,"O")

;FOLD LIN P2 CONT Vel

$BWDSTART=FALSE

LDAT_ACT=LCPDAT2

FDAT_ACT=FP2

BAS(#CP_PARAMS,2)

LIN XP2 C_DIS C_DIS

;ENDFOLD

;FOLD PTP P4 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT3

FDAT_ACT=FP4

BAS(#PTP_PARAMS,100)

PTP XP4 C_DIS

;FOLD PTP P5 CONT Vel

$BWDSTART=FALSE

PDAT_ACT=PPDAT5

FDAT_ACT=FP5

BAS(#PTP_PARAMS,100)

PTP XP5 C_DIS

;ENDFOLD

Machine_Start(3)

;FOLD PTP HOME Vel

$BWDSTART=FALSE

PDAT_ACT=PDEFAULT

FDAT_ACT=FHOME

BAS(#PTP_PARAMS,100)

$H_POS=XHOME

PTP XHOME

;ENDFOLD

$TIMER_STOP[1]=FALSE

IF $TIMER[1]>0 THEN

$TIMER_STOP[1]=TRUE

ENDIF

IF Machine3_OnlyDrop THEN

Machine3_OnlyDrop = FALSE

ENDIF

Count_Part=Count_Part+1

IF Count_Part>=40 THEN

Count_Part=0

SevenAxis_Run(4)

WAIT SEC 0.1

ENDIF

END

6、地轨运动程序（完成与地轨信号交互及运动交互）&ACCESS RV2

&REL 8

&PARAM EDITMASK = *

&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe DEF SevenAxis_Run(Num:IN)

INT Num

SWITCH Num

CASE 1

PULSE($OUT[1], TRUE,1)

;ENDFOLD

PULSE($OUT[14], TRUE,1)

;ENDFOLD

WAIT FOR $IN[2]

$OUT[1] = FALSE

$OUT[14] = FALSE

CASE 2

PULSE($OUT[2], TRUE,1)

PULSE($OUT[14], TRUE,1)

;ENDFOLD

WAIT SEC 0.5

WAIT FOR $IN[2]

$OUT[2] = FALSE

$OUT[14] = FALSE

CASE 3

6:, 8:1

PULSE($OUT[3], TRUE,1)

;ENDFOLD

PULSE($OUT[14], TRUE,1)

;ENDFOLD

WAIT SEC 0.5

WAIT FOR $IN[2]

$OUT[3] = FALSE

$OUT[14] = FALSE

CASE 4

PULSE($OUT[4], TRUE,1)

;ENDFOLD

PULSE($OUT[14], TRUE,1)

;ENDFOLD

WAIT SEC 0.5

WAIT FOR $IN[2]

$OUT[4] = FALSE

$OUT[14] = FALSE

DEFAULT

MsgQuit("Error")

ENDSWITCH

END

7、机台启动控制程序（完成机台启动，取消工作中信号给开始加工信号）&ACCESS RV2

&REL 3

&PARAM EDITMASK = *

&PARAM TEMPLATE = C:\KRC\Roboter\Template\ExpertVorgabe

DEF Machine_Start(Num:IN)

INT Num

SWITCH Num

CASE 1

$OUT[5]=FALSE

;ENDFOLD

PULSE($OUT[6], TRUE,0.5)

;ENDFOLD

CASE 2

$OUT[8]=FALSE

;ENDFOLD

PULSE($OUT[9], TRUE,0.5)

;ENDFOLD

CASE 3

$OUT[11]=FALSE

;ENDFOLD

PULSE($OUT[12], TRUE,0.5)

;ENDFOLD

DEFAULT

MsgQuit("Error")

ENDSWITCH

END

8、手爪控制程序（完成对手爪的控制）

手爪1控制点位为OUT[16]，对应检测为IN[10]，对应中断4手爪2控制点位为OUT[15]，对应检测为IN[11]，对应中断5 &ACCESS RV2

&REL 50

&PARAM EDITMASK = *

&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe

DEF Gripper_SetStatus(Num:IN,Status:IN)

INT Num

CHAR Status

SWITCH Num

CASE 1

SWITCH Status

CASE "O"

IF NOT $T1 THEN

INTERRUPT OFF 4

ENDIF

$OUT[16] = FALSE

WAIT SEC 0.3

WAIT FOR NOT $IN[10]

Part0InLeft = FALSE

Part2InLeft = FALSE

CASE "C"

IF NOT $T1 THEN

INTERRUPT ON 4

ENDIF

$OUT[16] = TRUE

WAIT SEC 0.5

WAIT FOR $IN[10]

DEFAULT

MsgQuit("Element Error")

ENDSWITCH

CASE 2

SWITCH Status

CASE "O"

IF NOT $T1 THEN

INTERRUPT OFF 5

ENDIF

INTERRUPT OFF 89

$OUT[15] = FALSE

WAIT SEC 0.3

WAIT FOR NOT $IN[11]

Part1InRight = FALSE

CASE "C"

IF NOT $T1 THEN

INTERRUPT ON 5

ENDIF

$OUT[15] = TRUE

WAIT SEC 0.5

WAIT FOR $IN[11]

Part1InRight = TRUE

DEFAULT

MsgQuit("Element Error")

ENDSWITCH

DEFAULT

MsgQuit("Element Error")

ENDSWITCH

END

9、手爪真空检测（完成对掉料状况的检测）

&ACCESS RV2

&REL 10

&PARAM EDITMASK = *

&PARAM TEMPLATE = C:\KRC\Roboter\Template\FunctionVorgabe DEFFCT BOOL Gripper_ChkStatus(Num:IN,Status:IN)

INT Num

CHAR Status

SWITCH Num

CASE 1

SWITCH Status

CASE "O"

IF NOT $OUT[16] AND NOT $IN[10] THEN

RETURN TRUE

ELSE

RETURN FALSE

ENDIF

CASE "C"

IF $OUT[16] AND $IN[10] THEN

RETURN TRUE

ELSE

RETURN FALSE

ENDIF

DEFAULT

MsgQuit("Element Error")

RETURN FALSE

ENDSWITCH

CASE 2

SWITCH Status

CASE "O"

IF NOT $OUT[15] AND NOT $IN[11] THEN

RETURN TRUE

ELSE

RETURN FALSE

ENDIF

CASE "C"

IF $OUT[15] AND $IN[11] THEN

RETURN TRUE

ELSE

RETURN FALSE

ENDIF

DEFAULT

MsgQuit("Element Error")

RETURN FALSE

ENDSWITCH

CASE 3

SWITCH Status

CASE "O"

IF NOT $OUT[15] AND NOT $OUT[16] AND NOT $IN[10] AND NOT $IN[11] THEN RETURN TRUE

ELSE

RETURN FALSE

ENDIF

CASE "C"

IF $OUT[15] AND $OUT[16] AND $IN[10] AND $IN[11] THEN

RETURN TRUE

ELSE

RETURN FALSE

ENDIF

DEFAULT

MsgQuit("Element Error")

RETURN FALSE

ENDSWITCH

DEFAULT

MsgQuit("Element Error")

RETURN FALSE

ENDSWITCH

ENDFCT

10、装夹检测子程序（完成对每台机的装夹检测与装夹异常信号交互）

11、后台监视程序（对安全姿态信号自动触发等后台程序进行配置）

&ACCESS R

&COMMENT HandlerOnRobotFault

DEF IR_STOPM ( )

;-----------------------------------

; Error Handling Robot Controller

; Switch OFF and Switch ON processes

; KRC Version >= V5.5

;-----------------------------------

;FOLD DECLARATIONS

;FOLD USER DECL

; Please insert user defined declarations

;ENDFOLD (USER DECL)

;FOLD BASISTECH DECL

BOOL ApplicationRunFlag

DECL CHAR ID[3]

;ENDFOLD (BASISTECH DECL)

;ENDFOLD (DECLARATIONS)

;FOLD BASISTECH INIT

INTERRUPT OFF 3

STOPM_FLAG=TRUE ;Reflects state of interrupt 3 to activate/deactivate $Stopmess interrupt ID[]="CTL"

If ($STOPMESS==TRUE) THEN

BRAKE

;ENDFOLD (BASISTECH INIT)

;FOLD USER STOP

;Make your modifications here

;ENDFOLD (USER STOP)

;FOLD BASISTECH STOP

P00 (#EXT_ERR,#PGNO_GET,ID[],128 )

ApplicationRunFlag=FALSE

IF (Appl_Run>0) THEN

IF $OUT[Appl_Run] THEN

ApplicationRunFlag=TRUE

$OUT[Appl_Run]=FALSE

ENDIF

ENDIF

REPEAT

POWER=SYNC()

HALT

UNTIL (($STOPMESS==FALSE) AND ($POWER_FAIL==FALSE))

;ENDFOLD (BASISTECH STOP)

;FOLD BASISTECH RESTART

P00 (#EXT_ERR,#PGNO_GET,ID[],0 )

IF (ApplicationRunFlag==TRUE) THEN

IF (Appl_Run>0) THEN

$OUT[Appl_Run]=TRUE

ENDIF

ENDIF

;ENDFOLD (BASISTECH RESTART)

;FOLD USER RESTART

;Make your modifications here

;ENDFOLD (USER RESTART)

;FOLD BASISTECH REACTIVATE

Endif

INTERRUPT ON 3

STOPM_FLAG=FALSE ;Reflects state of interrupt 3 to activate/deactivate $Stopmess interrupt ;ENDFOLD (BASISTECH REACTIVATE)

END