机械手自动化程序

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=100 % DEFAULT;%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT

$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=100 % PDAT2 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P1, 3:C_DIS, 5:100, 7:PDAT2

$BWDSTART=FALSE

PDAT_ACT=PPDAT2

FDAT_ACT=FP1

BAS(#PTP_PARAMS,100)

PTP XP1 C_DIS

;ENDFOLD

;FOLD PTP P3 CONT Vel=100 % PDAT8 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P3, 3:C_DIS, 5:100, 7:PDAT8

$BWDSTART=FALSE

PDAT_ACT=PPDAT8

FDAT_ACT=FP3

BAS(#PTP_PARAMS,100)

PTP XP3 C_DIS

;ENDFOLD

;FOLD LIN P2 CONT Vel=2 m/s CPDAT2 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P2, 3:C_DIS C_DIS, 5:2, 7:CPDAT2

$BWDSTART=FALSE

LDAT_ACT=LCPDAT2

FDAT_ACT=FP2

BAS(#CP_PARAMS,2)

LIN XP2 C_DIS C_DIS

;ENDFOLD

;FOLD LIN P1 Vel=0.05 m/s CPDAT1 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P1, 3:, 5:0.05, 7:CPDAT1

$BWDSTART=FALSE

LDAT_ACT=LCPDAT1

FDAT_ACT=FP1

BAS(#CP_PARAMS,0.05)

LIN XP1

;ENDFOLD

;FOLD PTP HOME Vel=100 % DEFAULT;%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT

$BWDSTART=FALSE

PDAT_ACT=PDEFAULT

FDAT_ACT=FHOME

BAS(#PTP_PARAMS,100)

$H_POS=XHOME

PTP XHOME

;ENDFOLD

mark: