机械手自动化程序20170915

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)