机械手自动化程序.docx

1、机械手自动化程序 集团文件版本号：（M928-T898-M248-WU2669-I2896-DQ586-M1988）机械手自动化程序1、 主程序 main&ACCESS RVP1&REL 1&PARAM DISKPATH = KRC:R1ProgramDEF 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) ;F

2、OLD USER INI ;Make your modifications here ;ENDFOLD (USER INI)GLOBAL INTERRUPT DECL 4 WHEN $IN10=FALSE DO Itrpt1 ()GLOBAL INTERRUPT DECL 5 WHEN $IN11=FALSE DO Itrpt1 ()INTERRUPT DECL 6 WHEN $IN9=TRUE DO Itrpt1 ()；地轨异常进入中断INTERRUPT ON 6；开地轨监视中断6IF NOT Gripper_ChkStatus(1,C) THEN Gripper_SetStatus(1,O

3、) Part0InLeft = FALSE Part2InLeft = FALSEENDIFIF NOT Gripper_ChkStatus(2,C) THEN Gripper_SetStatus(2,O) Part1InRight = FALSEENDIFFOR l = 1 TO 14 $OUTl = FALSEENDFOR；此循环语句在初始化时把所有端口清零复位。IF $IN5 THEN Machine1_OnlyDrop = TRUE Machine2_OnlyDrop = TRUE；机台1有首件信号时，机台1与机台2第一次响应只放标志位置1ELSE Machine1_OnlyDrop

4、= FALSE Machine2_OnlyDrop = FALSEENDIF$FLAG8=FALSE；FLAG8是干什么用的？;ENDFOLD (INI)；初始化完成$OV_PRO=70；限定系统运行速度;FOLD PTP HOME Vel$BWDSTART=FALSEPDAT_ACT=PDEFAULTFDAT_ACT=FHOMEBAS(#PTP_PARAMS,100)$H_POS=XHOMEPTP XHOME ;ENDFOLDIF $IN_HOME THEN $OUT5=FALSE $OUT8=FALSE $OUT11=FALSEENDIFLOOP WAIT SEC 0.05 IF $IN1

5、 AND NOT Part0InLeft AND Gripper_ChkStatus(3,O) THEN ToPick_Search() ENDIF IF Part0InLeft AND ($IN3 OR Machine1_OnlyDrop) AND Gripper_ChkStatus(1,C) AND Gripper_ChkStatus(2,O) THEN Machine1() ENDIF IF Part1InRight AND ($IN6 OR Machine2_OnlyDrop) AND Gripper_ChkStatus(2,C) AND Gripper_ChkStatus(1,O)

6、THEN Machine2() ENDIF IF Part2InLeft AND ($IN8 OR Machine3_OnlyDrop) AND Gripper_ChkStatus(1,C) THEN Machine3() ENDIFENDLOOPENDDEF Itrpt1 ()HALTBRAKEEND2、 取料程序（采用中断的方法自动探料，完成取料作业）&ACCESS RVO2&REL 1&PARAM DISKPATH = KRC:R1ProgramDEF ToPick_Search( );FOLD INI;%PE ;FOLD BASISTECH INI GLOBAL INTERRUPT D

7、ECL 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 $IN10=TRUE DO FOUND()SevenAxis_Run(1)GOTO MARK;FOLD PTP P1 CONT Vel$BWDSTART=FALSEPDAT_A

8、CT=PPDAT2FDAT_ACT=FP1BAS(#PTP_PARAMS,100)PTP XP1 C_DIS;ENDFOLD;FOLD PTP P3 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT8FDAT_ACT=FP3BAS(#PTP_PARAMS,100)PTP XP3 C_DIS;ENDFOLD;FOLD LIN P2 CONT Vel$BWDSTART=FALSELDAT_ACT=LCPDAT2FDAT_ACT=FP2BAS(#CP_PARAMS,2)LIN XP2 C_DIS C_DIS;ENDFOLD;FOLD LIN P1 Vel$BWDSTART=

9、FALSELDAT_ACT=LCPDAT1FDAT_ACT=FP1BAS(#CP_PARAMS,0.05)LIN XP1 ;ENDFOLD;FOLD PTP HOME Vel$BWDSTART=FALSEPDAT_ACT=PDEFAULTFDAT_ACT=FHOMEBAS(#PTP_PARAMS,100)$H_POS=XHOMEPTP XHOME ;ENDFOLDmark:Search()INTERRUPT OFF 1WAIT SEC 0XP4=$POS_ACT;FOLD LIN P4 CONT Vel$BWDSTART=FALSELDAT_ACT=LCPDAT10FDAT_ACT=FP4BA

10、S(#CP_PARAMS,2)LIN XP4 C_DIS C_DIS;ENDFOLDGripper_SetStatus(1,O)LIN_RELZ 15LIN_RELZ -17Gripper_SetStatus(1,C)Part0InLeft = TRUEWAIT SEC 0XP0=$POS_ACTXP0.Z=XP0.Z+25 ；看一下寻料程序到底是怎么运转的！;FOLD LIN P0 CONT Vel$BWDSTART=FALSELDAT_ACT=LCPDAT8FDAT_ACT=FP0BAS(#CP_PARAMS,0.025)LIN XP0 C_DIS C_DIS;ENDFOLDWAIT SE

11、C 0IF NOT $IN10 THEN GOTO markENDIF;FOLD LIN P1 CONT Vel$BWDSTART=FALSELDAT_ACT=LCPDAT7FDAT_ACT=FP1BAS(#CP_PARAMS,2)LIN XP1 C_DIS C_DIS;ENDFOLDENDDEF Search( );FOLD INI;%PE ;FOLD BASISTECH INI GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS=TRUE DO IR_STOPM ( ) INTERRUPT ON 3 BAS (#INITMOV,0 ) ;ENDFOLD (BASI

12、STECH INI);FOLD USER INI;Make your modifications here;ENDFOLD (USER INI);ENDFOLD (INI)INTERRUPT ON 1;FOLD PTP P1 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT6FDAT_ACT=FP1BAS(#PTP_PARAMS,100)PTP XP1 C_DIS;ENDFOLD$OUT16=TRUE;FOLD LIN P3 CONT Vel$BWDSTART=FALSELDAT_ACT=LCPDAT11FDAT_ACT=FP3BAS(#CP_PARAMS,2)LIN

13、 XP3 C_DIS C_DIS;ENDFOLD;FOLD LIN P2 CONT Vel$BWDSTART=FALSELDAT_ACT=LCPDAT5FDAT_ACT=FP2BAS(#CP_PARAMS,0.05)LIN XP2 C_DIS C_DIS;ENDFOLD;FOLD LIN P1 Vel$BWDSTART=FALSELDAT_ACT=LCPDAT6FDAT_ACT=FP1BAS(#CP_PARAMS,0.02)LIN XP1 ;ENDFOLDWAIT SEC 0ENDDEF FOUND()BRAKELIN $POS_INT ;rise a littleRESUMEEND3、 机台

14、1取放料程序（完成机台1取放料动作与信号交互）&ACCESS RVO2&REL 1&PARAM DISKPATH = KRC:R1ProgramDEF 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 I

15、NI);ENDFOLD (INI)SevenAxis_Run(1)$OUT5=TRUE;ENDFOLDIF NOT Machine1_OnlyDrop THEN;FOLD PTP P1 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT4FDAT_ACT=FP1BAS(#PTP_PARAMS,100)PTP XP1 C_DIS;ENDFOLDmark:;FOLD LIN P2 CONT Vel$BWDSTART=FALSELDAT_ACT=LCPDAT4FDAT_ACT=FP2BAS(#CP_PARAMS,2)LIN XP2 C_DIS C_DIS;ENDFOLD;FO

16、LD LIN P3 CONT Vel$BWDSTART=FALSELDAT_ACT=LCPDAT5FDAT_ACT=FP3BAS(#CP_PARAMS,2)LIN XP3 C_DIS C_DIS;ENDFOLDGripper_SetStatus(2,C)LIN_RELZ 7.5Gripper_SetStatus(2,O);FOLD LIN P3 Vel$BWDSTART=FALSELDAT_ACT=LCPDAT1FDAT_ACT=FP3BAS(#CP_PARAMS,2)LIN XP3 ;ENDFOLDGripper_SetStatus(2,C)Part1InRight = TRUE;FOLD

17、LIN P2 Vel$BWDSTART=FALSELDAT_ACT=LCPDAT6FDAT_ACT=FP2BAS(#CP_PARAMS,0.05)LIN XP2 ;ENDFOLDIF NOT $IN11 THEN GOTO markENDIF;FOLD PTP P4 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT6FDAT_ACT=FP4BAS(#PTP_PARAMS,100)PTP XP4 C_DIS;ENDFOLD;FOLD PTP P5 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT10FDAT_ACT=FP5BAS(#PTP_PA

18、RAMS,100)PTP XP5 C_DIS;ENDFOLD;FOLD PTP P6 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT8FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 C_DIS;ENDFOLD;FOLD LIN P7 Vel$BWDSTART=FALSELDAT_ACT=LCPDAT7FDAT_ACT=FP7BAS(#CP_PARAMS,0.25)LIN XP7 ;ENDFOLDGripper_SetStatus(1,O);FOLD PTP P6 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDA

19、T14FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 C_DIS;ENDFOLD;FOLD PTP P8 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT9FDAT_ACT=FP8BAS(#PTP_PARAMS,100)PTP XP8 C_DIS;ENDFOLDELSE;FOLD PTP P5 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT2FDAT_ACT=FP5BAS(#PTP_PARAMS,100)PTP XP5 C_DIS;ENDFOLD;FOLD PTP P6 CONT Vel$BWDSTART=F

20、ALSEPDAT_ACT=PPDAT13FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 C_DIS;ENDFOLD;FOLD LIN P7 Vel$BWDSTART=FALSELDAT_ACT=LCPDAT9FDAT_ACT=FP7BAS(#CP_PARAMS,0.25)LIN XP7 ;ENDFOLDGripper_SetStatus(1,O);FOLD PTP P6 Vel$BWDSTART=FALSEPDAT_ACT=PPDAT12FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 ;ENDFOLD;FOLD PTP P8 CO

21、NT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT7FDAT_ACT=FP8BAS(#PTP_PARAMS,100)PTP XP8 C_DIS;ENDFOLDMachine_Start(1);FOLD PTP HOME Vel$BWDSTART=FALSEPDAT_ACT=PDEFAULTFDAT_ACT=FHOMEBAS(#PTP_PARAMS,100)$H_POS=XHOMEPTP XHOME ;ENDFOLDENDIFIF Machine1_OnlyDrop THEN Machine1_OnlyDrop = FALSEENDIFMachine_Start(1)END4

22、、 机台2取放料程序（完成机台2取放料动作与信号交互）&ACCESS RV2&REL 6&PARAM DISKPATH = KRC:R1ProgramDEF 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

23、 INI);ENDFOLD (INI)PTP $POS_ACTSevenAxis_Run(2)$OUT8=TRUE;ENDFOLDIF NOT Machine2_OnlyDrop THEN;FOLD PTP P1 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT3FDAT_ACT=FP1BAS(#PTP_PARAMS,100)PTP XP1 C_DIS;ENDFOLDmark:;FOLD PTP P2 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT4FDAT_ACT=FP2BAS(#PTP_PARAMS,100)PTP XP2 C_DIS;

24、ENDFOLD;FOLD LIN P3 Vel$BWDSTART=FALSELDAT_ACT=LCPDAT4FDAT_ACT=FP3BAS(#CP_PARAMS,2)LIN XP3 ;ENDFOLDGripper_SetStatus(1,C)LIN_RELZ 7.5Gripper_SetStatus(1,O);FOLD LIN P3 Vel$BWDSTART=FALSELDAT_ACT=LCPDAT0FDAT_ACT=FP3BAS(#CP_PARAMS,2)LIN XP3 ;ENDFOLDGripper_SetStatus(1,C)Part2InLeft = TRUE;FOLD LIN P2

25、CONT Vel$BWDSTART=FALSELDAT_ACT=LCPDAT3FDAT_ACT=FP2BAS(#CP_PARAMS,0.05)LIN XP2 C_DIS C_DIS;ENDFOLDIF NOT $IN10 THEN GOTO markENDIF;FOLD PTP P4 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT7FDAT_ACT=FP4BAS(#PTP_PARAMS,100)PTP XP4 C_DIS;ENDFOLD;FOLD PTP P5 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT14FDAT_ACT=FP5BA

26、S(#PTP_PARAMS,100)PTP XP5 C_DIS;ENDFOLD;FOLD PTP P6 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT9FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 C_DIS;ENDFOLD;FOLD LIN P7 Vel$BWDSTART=FALSELDAT_ACT=LCPDAT7FDAT_ACT=FP7BAS(#CP_PARAMS,0.25)LIN XP7 ;ENDFOLDGripper_SetStatus(2,O);FOLD PTP P6 CONT Vel$BWDSTART=FALSEPDAT

27、_ACT=PPDAT11FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 C_DIS;ENDFOLD;FOLD PTP P8 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT12FDAT_ACT=FP8BAS(#PTP_PARAMS,100)PTP XP8 C_DIS;ENDFOLDELSE;FOLD PTP P5 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT13FDAT_ACT=FP5BAS(#PTP_PARAMS,100)PTP XP5 C_DIS;ENDFOLD;FOLD PTP P6 CONT Vel

28、$BWDSTART=FALSEPDAT_ACT=PPDAT1FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 C_DIS;ENDFOLD;FOLD LIN P7 Vel$BWDSTART=FALSELDAT_ACT=LCPDAT1FDAT_ACT=FP7BAS(#CP_PARAMS,0.25)LIN XP7 ;ENDFOLDGripper_SetStatus(2,O);FOLD LIN P6 CONT Vel$BWDSTART=FALSELDAT_ACT=LCPDAT2FDAT_ACT=FP6BAS(#CP_PARAMS,2)LIN XP6 C_DIS C_DIS

29、;ENDFOLD;FOLD PTP P8 CONT Vel$BWDSTART=FALSEPDAT_ACT=PPDAT6FDAT_ACT=FP8BAS(#PTP_PARAMS,100)PTP XP8 C_DIS;ENDFOLDMachine_Start(2);FOLD PTP HOME Vel5:100, 7:DEFAULT$BWDSTART=FALSEPDAT_ACT=PDEFAULTFDAT_ACT=FHOMEBAS(#PTP_PARAMS,100)$H_POS=XHOMEPTP XHOME ;ENDFOLDENDIFIF Machine2_OnlyDrop THEN Machine2_OnlyDrop = FALSEENDIFMachine_Start(2)END5、 机台3取放料程序（完成机台3取放料动作与信号交互）&ACCESS RVP1&REL 6D