机械手自动化程序20170915

1、主程序main&ACCESS RVP1&REL 1&PARAM DISKPATH = KRC:\R1\ProgramDEF Pro_main( )INT l;FOLD INI;%{PE};FOLD BASISTECH INIGLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( ) INTERRUPT ON 3BAS (#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；开地轨监视中断6IF NOT Gripper_ChkStatus(1,"C") THENGripper_SetStatus(1,"O")Part2InLeft = FALSEENDIFIF NOT Gripper_ChkStatus(2,"C") THENGripper_SetStatus(2,"O")Part1InRight = FALSEENDIFFOR l = 1 TO 14$OUT[l] = FALSEENDFOR；此循环语句在初始化时把所有端口清零复位。

IF $IN[5] THENMachine1_OnlyDrop = TRUEMachine2_OnlyDrop = TRUE；机台1有首件信号时，机台1与机台2第一次响应只放标志位置1 ELSEMachine1_OnlyDrop = FALSEMachine2_OnlyDrop = FALSEENDIF$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,$BWDSTART=FALSEPDAT_ACT=PDEFAULTFDAT_ACT=FHOMEBAS(#PTP_PARAMS,100)$H_POS=XHOMEPTP XHOME;ENDFOLDIF $IN_HOME THEN$OUT[5]=FALSE$OUT[8]=FALSE$OUT[11]=FALSEENDIFLOOPWAIT SEC 0.05IF $IN[1] AND NOT Part0InLeft AND Gripper_ChkStatus(3,"O") THEN ToPick_Search()ENDIFIF Part0InLeft AND ($IN[3] OR Machine1_OnlyDrop) AND Gripper_ChkStatus(1,"C") AND Gripper_ChkStatus(2,"O") THENMachine1()ENDIFGripper_ChkStatus(1,"O") THENMachine2()ENDIFIF Part2InLeft AND ($IN[8] OR Machine3_OnlyDrop) AND Gripper_ChkStatus(1,"C") THENMachine3()ENDIFENDLOOPENDDEF Itrpt1 ()HALTBRAKEEND2、取料程序（采用中断的方法自动探料，完成取料作业）&ACCESS RVO2&REL 1&PARAM DISKPATH = KRC:\R1\ProgramDEF ToPick_Search( );FOLD INI;%{PE};FOLD BASISTECH INIGLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )；单独用了一个系统中INTERRUPT ON 3BAS (#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 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P1, 3:C_DIS, 5:100, 7:PDAT2$BWDSTART=FALSEPDAT_ACT=PPDAT2FDAT_ACT=FP1BAS(#PTP_PARAMS,100)PTP XP1 C_DIS;ENDFOLD;FOLD PTP P3 CONT Vel=100 % PDAT8 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P3, 3:C_DIS, 5:100, 7:PDAT8$BWDSTART=FALSEPDAT_ACT=PPDAT8FDAT_ACT=FP3PTP 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=FALSELDAT_ACT=LCPDAT2FDAT_ACT=FP2BAS(#CP_PARAMS,2)LIN XP2 C_DIS C_DIS;ENDFOLD;FOLD LIN P1 Vel=0.05 m/s CPDAT1 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P1, 3:, 5:0.05, 7:CPDAT1$BWDSTART=FALSELDAT_ACT=LCPDAT1FDAT_ACT=FP1BAS(#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=FALSEPDAT_ACT=PDEFAULTFDAT_ACT=FHOME$H_POS=XHOMEPTP XHOME;ENDFOLDmark:Search()INTERRUPT OFF 1WAIT SEC 0XP4=$POS_ACT;FOLD LIN P4 CONT Vel=2 m/s CPDAT10 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P4, 3:C_DIS C_DIS, 5:2, 7:CPDAT10$BWDSTART=FALSELDAT_ACT=LCPDAT10FDAT_ACT=FP4BAS(#CP_PARAMS,2)LIN XP4 C_DIS C_DIS;ENDFOLDGripper_SetStatus(1,"O")LIN_REL{Z 15}LIN_REL{Z -17}Gripper_SetStatus(1,"C")Part0InLeft = TRUEWAIT SEC 0XP0=$POS_ACT;FOLD LIN P0 CONT Vel=0.025 m/s CPDAT8 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P0, 3:C_DIS C_DIS, 5:0.025, 7:CPDAT8$BWDSTART=FALSELDAT_ACT=LCPDAT8FDAT_ACT=FP0BAS(#CP_PARAMS,0.025)LIN XP0 C_DIS C_DIS;ENDFOLDWAIT SEC 0IF NOT $IN[10] THENGOTO markENDIF;FOLD LIN P1 CONT Vel=2 m/s CPDAT7 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P1, 3:C_DIS C_DIS, 5:2, 7:CPDAT7$BWDSTART=FALSELDAT_ACT=LCPDAT7FDAT_ACT=FP1BAS(#CP_PARAMS,2)LIN XP1 C_DIS C_DIS;ENDFOLDENDDEF Search( );FOLD BASISTECH INIGLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )INTERRUPT ON 3BAS (#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=100 % PDAT6 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P1, 3:C_DIS, 5:100, 7:PDAT6$BWDSTART=FALSEPDAT_ACT=PPDAT6FDAT_ACT=FP1BAS(#PTP_PARAMS,100)PTP XP1 C_DIS;ENDFOLD$OUT[16]=TRUE;FOLD LIN P3 CONT Vel=2 m/s CPDAT11 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P3, 3:C_DIS C_DIS, 5:2, 7:CPDAT11$BWDSTART=FALSEFDAT_ACT=FP3BAS(#CP_PARAMS,2)LIN XP3 C_DIS C_DIS;ENDFOLD;FOLD LIN P2 CONT Vel=0.05 m/s CPDAT5 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P2, 3:C_DIS C_DIS, 5:0.05, 7:CPDAT5$BWDSTART=FALSELDAT_ACT=LCPDAT5FDAT_ACT=FP2BAS(#CP_PARAMS,0.05)LIN XP2 C_DIS C_DIS;ENDFOLD;FOLD LIN P1 Vel=0.02 m/s CPDAT6 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P1, 3:, 5:0.02, 7:CPDAT6$BWDSTART=FALSELDAT_ACT=LCPDAT6FDAT_ACT=FP1BAS(#CP_PARAMS,0.02)LIN XP1;ENDFOLDWAIT SEC 0ENDBRAKELIN $POS_INT ;rise a littleRESUMEEND3、机台1取放料程序（完成机台1取放料动作与信号交互）&ACCESS RVO2&REL 1&PARAM DISKPATH = KRC:\R1\ProgramDEF Machine1( );FOLD INI;%{PE};FOLD BASISTECH INIGLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )INTERRUPT ON 3BAS (#INITMOV,0 );ENDFOLD (BASISTECH INI);FOLD USER INI;Make your modifications here;ENDFOLD (USER INI);ENDFOLD (INI)SevenAxis_Run(1);FOLD OUT 5 '' State=TRUE ;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:5, 3:, 5:TRUE, 6: $OUT[5]=TRUE;ENDFOLDIF NOT Machine1_OnlyDrop THEN;FOLD PTP P1 CONT Vel=100 % PDAT4 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P1, 3:C_DIS, 5:100, 7:PDAT4$BWDSTART=FALSEPDAT_ACT=PPDAT4FDAT_ACT=FP1BAS(#PTP_PARAMS,100)PTP XP1 C_DIS;ENDFOLDmark:;FOLD LIN P2 CONT Vel=2 m/s CPDAT4 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:CPDAT4$BWDSTART=FALSELDAT_ACT=LCPDAT4FDAT_ACT=FP2BAS(#CP_PARAMS,2)LIN XP2 C_DIS C_DIS;ENDFOLD;FOLD LIN P3 CONT Vel=2 m/s CPDAT5 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P3, 3:C_DIS C_DIS, 5:2, 7:CPDAT5$BWDSTART=FALSELDAT_ACT=LCPDAT5FDAT_ACT=FP3BAS(#CP_PARAMS,2)LIN XP3 C_DIS C_DIS;ENDFOLDGripper_SetStatus(2,"C")LIN_REL{Z 7.5}Gripper_SetStatus(2,"O");FOLD LIN P3 Vel=2 m/s CPDAT1 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P3, 3:, 5:2, 7:CPDAT1$BWDSTART=FALSELDAT_ACT=LCPDAT1FDAT_ACT=FP3BAS(#CP_PARAMS,2)LIN XP3;ENDFOLDGripper_SetStatus(2,"C")Part1InRight = TRUE;FOLD LIN P2 Vel=0.05 m/s CPDAT6 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P2, 3:, 5:0.05, 7:CPDAT6$BWDSTART=FALSELDAT_ACT=LCPDAT6FDAT_ACT=FP2BAS(#CP_PARAMS,0.05)LIN XP2;ENDFOLDIF NOT $IN[11] THENGOTO markENDIF;FOLD PTP P4 CONT Vel=100 % PDAT6 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P4, 3:C_DIS, 5:100, 7:PDAT6$BWDSTART=FALSEPDAT_ACT=PPDAT6FDAT_ACT=FP4BAS(#PTP_PARAMS,100)PTP XP4 C_DIS;ENDFOLD;FOLD PTP P5 CONT Vel=100 % PDAT10 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P5, 3:C_DIS, 5:100, 7:PDAT10$BWDSTART=FALSEPDAT_ACT=PPDAT10FDAT_ACT=FP5BAS(#PTP_PARAMS,100)PTP XP5 C_DIS;ENDFOLD;FOLD PTP P6 CONT Vel=100 % PDAT8 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P6, 3:C_DIS, 5:100, 7:PDAT8$BWDSTART=FALSEPDAT_ACT=PPDAT8FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 C_DIS;ENDFOLD;FOLD LIN P7 Vel=0.25 m/s CPDAT7 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P7, 3:, 5:0.25, 7:CPDAT7$BWDSTART=FALSELDAT_ACT=LCPDAT7FDAT_ACT=FP7BAS(#CP_PARAMS,0.25)LIN XP7;ENDFOLDGripper_SetStatus(1,"O");FOLD PTP P6 CONT Vel=100 % PDAT14 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P6, 3:C_DIS, 5:100, 7:PDAT14$BWDSTART=FALSEPDAT_ACT=PPDAT14FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 C_DIS;ENDFOLD;FOLD PTP P8 CONT Vel=100 % PDAT9 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P8, 3:C_DIS, 5:100, 7:PDAT9$BWDSTART=FALSEPDAT_ACT=PPDAT9FDAT_ACT=FP8BAS(#PTP_PARAMS,100)PTP XP8 C_DIS;ENDFOLDELSE;FOLD PTP P5 CONT Vel=100 % PDAT2 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P5, 3:C_DIS, 5:100, 7:PDAT2$BWDSTART=FALSEPDAT_ACT=PPDAT2FDAT_ACT=FP5BAS(#PTP_PARAMS,100)PTP XP5 C_DIS;ENDFOLD;FOLD PTP P6 CONT Vel=100 % PDAT13 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P6, 3:C_DIS, 5:100, 7:PDAT13$BWDSTART=FALSEPDAT_ACT=PPDAT13FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 C_DIS;ENDFOLD;FOLD LIN P7 Vel=0.25 m/s CPDAT9 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P7, 3:, 5:0.25, 7:CPDAT9$BWDSTART=FALSELDAT_ACT=LCPDAT9FDAT_ACT=FP7BAS(#CP_PARAMS,0.25)LIN XP7;ENDFOLDGripper_SetStatus(1,"O");FOLD PTP P6 Vel=100 % PDAT12 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P6, 3:, 5:100, 7:PDAT12$BWDSTART=FALSEPDAT_ACT=PPDAT12FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6;ENDFOLD;FOLD PTP P8 CONT Vel=100 % PDAT7 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P8, 3:C_DIS, 5:100, 7:PDAT7$BWDSTART=FALSEPDAT_ACT=PPDAT7FDAT_ACT=FP8BAS(#PTP_PARAMS,100)PTP XP8 C_DIS;ENDFOLDMachine_Start(1);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=FALSEPDAT_ACT=PDEFAULTFDAT_ACT=FHOMEBAS(#PTP_PARAMS,100)$H_POS=XHOMEPTP XHOME;ENDFOLDENDIFIF Machine1_OnlyDrop THENMachine1_OnlyDrop = FALSEENDIFMachine_Start(1)END4、机台2取放料程序（完成机台2取放料动作与信号交互）&ACCESS RV2&REL 6&PARAM DISKPATH = KRC:\R1\ProgramDEF Machine2();FOLD INI;%{PE};FOLD BASISTECH INIGLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( ) INTERRUPT ON 3BAS (#INITMOV,0 );ENDFOLD (BASISTECH INI);FOLD USER INI;Make your modifications here;ENDFOLD (USER INI);ENDFOLD (INI)PTP $POS_ACTSevenAxis_Run(2);FOLD OUT 8 '' State=TRUE ;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:8, 3:, 5:TRUE, 6: $OUT[8]=TRUE;ENDFOLDIF NOT Machine2_OnlyDrop THEN;FOLD PTP P1 CONT Vel=100 % PDAT3 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P1, 3:C_DIS, 5:100, 7:PDAT3$BWDSTART=FALSEPDAT_ACT=PPDAT3FDAT_ACT=FP1BAS(#PTP_PARAMS,100)PTP XP1 C_DIS;ENDFOLDmark:;FOLD PTP P2 CONT Vel=100 % PDAT4 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P2, 3:C_DIS, 5:100, 7:PDAT4$BWDSTART=FALSEPDAT_ACT=PPDAT4FDAT_ACT=FP2BAS(#PTP_PARAMS,100)PTP XP2 C_DIS;ENDFOLD;FOLD LIN P3 Vel=2 m/s CPDAT4 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P3, 3:, 5:2, 7:CPDAT4$BWDSTART=FALSELDAT_ACT=LCPDAT4FDAT_ACT=FP3BAS(#CP_PARAMS,2)LIN XP3;ENDFOLDGripper_SetStatus(1,"C")LIN_REL{Z 7.5}Gripper_SetStatus(1,"O");FOLD LIN P3 Vel=2 m/s CPDAT0 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P3, 3:, 5:2, 7:CPDAT0$BWDSTART=FALSELDAT_ACT=LCPDAT0FDAT_ACT=FP3BAS(#CP_PARAMS,2)LIN XP3;ENDFOLDGripper_SetStatus(1,"C")Part2InLeft = TRUE;FOLD LIN P2 CONT Vel=0.05 m/s CPDAT3 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P2, 3:C_DIS C_DIS, 5:0.05, 7:CPDAT3$BWDSTART=FALSELDAT_ACT=LCPDAT3FDAT_ACT=FP2BAS(#CP_PARAMS,0.05)LIN XP2 C_DIS C_DIS;ENDFOLDIF NOT $IN[10] THENGOTO markENDIF;FOLD PTP P4 CONT Vel=100 % PDAT7 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P4, 3:C_DIS, 5:100, 7:PDAT7$BWDSTART=FALSEPDAT_ACT=PPDAT7FDAT_ACT=FP4BAS(#PTP_PARAMS,100)PTP XP4 C_DIS;ENDFOLD;FOLD PTP P5 CONT Vel=100 % PDAT14 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P5, 3:C_DIS, 5:100, 7:PDAT14$BWDSTART=FALSEPDAT_ACT=PPDAT14FDAT_ACT=FP5BAS(#PTP_PARAMS,100)PTP XP5 C_DIS;ENDFOLD;FOLD PTP P6 CONT Vel=100 % PDAT9 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P6, 3:C_DIS, 5:100, 7:PDAT9$BWDSTART=FALSEPDAT_ACT=PPDAT9FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 C_DIS;ENDFOLD;FOLD LIN P7 Vel=0.25 m/s CPDAT7 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P7, 3:, 5:0.25, 7:CPDAT7$BWDSTART=FALSELDAT_ACT=LCPDAT7FDAT_ACT=FP7BAS(#CP_PARAMS,0.25)LIN XP7;ENDFOLDGripper_SetStatus(2,"O");FOLD PTP P6 CONT Vel=100 % PDAT11 Tool[1] Base[0];%{PE}%R8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P6, 3:C_DIS, 5:100, 7:PDAT11$BWDSTART=FALSEPDAT_ACT=PPDAT11FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 C_DIS;ENDFOLD;FOLD PTP P8 CONT Vel=100 % PDAT12 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P8, 3:C_DIS, 5:100, 7:PDAT12$BWDSTART=FALSEPDAT_ACT=PPDAT12FDAT_ACT=FP8BAS(#PTP_PARAMS,100)PTP XP8 C_DIS;ENDFOLDELSE;FOLD PTP P5 CONT Vel=100 % PDAT13 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P5, 3:C_DIS, 5:100, 7:PDAT13$BWDSTART=FALSEPDAT_ACT=PPDAT13FDAT_ACT=FP5BAS(#PTP_PARAMS,100)PTP XP5 C_DIS;ENDFOLD;FOLD PTP P6 CONT Vel=100 % PDAT1 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P6, 3:C_DIS, 5:100, 7:PDAT1$BWDSTART=FALSEPDAT_ACT=PPDAT1FDAT_ACT=FP6BAS(#PTP_PARAMS,100)PTP XP6 C_DIS;ENDFOLD;FOLD LIN P7 Vel=0.25 m/s CPDAT1 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P7, 3:, 5:0.25, 7:CPDAT1$BWDSTART=FALSELDAT_ACT=LCPDAT1FDAT_ACT=FP7BAS(#CP_PARAMS,0.25)LIN XP7;ENDFOLDGripper_SetStatus(2,"O");FOLD LIN P6 CONT Vel=2 m/s CPDAT2 Tool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P6, 3:C_DIS C_DIS, 5:2, 7:CPDAT2$BWDSTART=FALSELDAT_ACT=LCPDAT2FDAT_ACT=FP6BAS(#CP_PARAMS,2)LIN XP6 C_DIS C_DIS;ENDFOLD;FOLD PTP P8 CONT Vel=100 % PDAT6 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P8, 3:C_DIS, 5:100, 7:PDAT6$BWDSTART=FALSEPDAT_ACT=PPDAT6FDAT_ACT=FP8BAS(#PTP_PARAMS,100)PTP XP8 C_DIS;ENDFOLDMachine_Start(2);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=FALSEPDAT_ACT=PDEFAULTFDAT_ACT=FHOMEBAS(#PTP_PARAMS,100)$H_POS=XHOMEPTP XHOME;ENDFOLDENDIFIF Machine2_OnlyDrop THENMachine2_OnlyDrop = FALSEENDIFMachine_Start(2)END5、机台3取放料程序（完成机台3取放料动作与信号交互）&ACCESS RVP1&REL 6DEF Machine3( );FOLD INI;%{PE};FOLD BASISTECH INIGLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( ) INTERRUPT ON 3BAS (#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_ACTSevenAxis_Run(3);FOLD OUT 11 '' State=TRUE ;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:11, 3:, 5:TRUE, 6:$OUT[11]=TRUE;ENDFOLD;FOLD PTP P1 CONT Vel=100 % PDAT6 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P1, 3:C_DIS, 5:100, 7:PDAT6$BWDSTART=FALSEPDAT_ACT=PPDAT6FDAT_ACT=FP1BAS(#PTP_PARAMS,100)PTP XP1 C_DIS;ENDFOLD;FOLD PTP P2 CONT Vel=100 % PDAT2 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P2, 3:C_DIS, 5:100, 7:PDAT2$BWDSTART=FALSEPDAT_ACT=PPDAT2FDAT_ACT=FP2BAS(#PTP_PARAMS,100)PTP XP2 C_DIS;ENDFOLD;FOLD LIN P3 Vel=0.25 m/s CPDAT1 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P3, 3:, 5:0.25, 7:CPDAT1$BWDSTART=FALSELDAT_ACT=LCPDAT1FDAT_ACT=FP3BAS(#CP_PARAMS,0.25)LIN XP3;ENDFOLDGripper_SetStatus(1,"O");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=FALSELDAT_ACT=LCPDAT2FDAT_ACT=FP2BAS(#CP_PARAMS,2)LIN XP2 C_DIS C_DIS;ENDFOLD;FOLD PTP P4 CONT Vel=100 % PDAT3 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P4, 3:C_DIS, 5:100, 7:PDAT3$BWDSTART=FALSEPDAT_ACT=PPDAT3FDAT_ACT=FP4BAS(#PTP_PARAMS,100)PTP XP4 C_DIS;ENDFOLD;FOLD PTP P5 CONT Vel=100 % PDAT5 T ool[1] Base[0];%{PE}%R 8.3.40,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P5, 3:C_DIS, 5:100, 7:PDAT5$BWDSTART=FALSEPDAT_ACT=PPDAT5FDAT_ACT=FP5BAS(#PTP_PARAMS,100)PTP XP5 C_DIS;ENDFOLDMachine_Start(3);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=FALSEPDAT_ACT=PDEFAULTFDAT_ACT=FHOMEBAS(#PTP_PARAMS,100)$H_POS=XHOMEPTP XHOME;ENDFOLD$TIMER_STOP[1]=FALSEIF $TIMER[1]>0 THEN$TIMER_STOP[1]=TRUEENDIFIF Machine3_OnlyDrop THENMachine3_OnlyDrop = FALSEENDIFCount_Part=Count_Part+1IF Count_Part>=40 THENCount_Part=0SevenAxis_Run(4)WAIT SEC 0.1ENDIFEND6、地轨运动程序（完成与地轨信号交互及运动交互）&ACCESS RV2&REL 8&PARAM EDITMASK = *&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabeDEF SevenAxis_Run(Num:IN)INT NumSWITCH NumCASE 1;FOLD PULSE 1 '' State=TRUE Time=1 sec;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VPULSE,%P 2:1, 3:, 5:TRUE, 6:, 8:1PULSE($OUT[1], TRUE,1);ENDFOLD;FOLD PULSE 14 '' State=TRUE Time=1 sec;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VPULSE,%P 2:14, 3:, 5:TRUE, 6:, 8:1PULSE($OUT[14], TRUE,1);ENDFOLDWAIT FOR $IN[2]$OUT[1] = FALSE$OUT[14] = FALSECASE 2;FOLD PULSE 2 '' State=TRUE Time=1 sec;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VPULSE,%P 2:2, 3:, 5:TRUE, 6:, 8:1PULSE($OUT[2], TRUE,1);ENDFOLD;FOLD PULSE 14 '' State=TRUE Time=1 sec;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VPULSE,%P 2:14, 3:, 5:TRUE, 6:, 8:1PULSE($OUT[14], TRUE,1);ENDFOLDWAIT SEC 0.5WAIT FOR $IN[2]$OUT[2] = FALSE$OUT[14] = FALSECASE 3;FOLD PULSE 3 '' State=TRUE Time=1 sec;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VPULSE,%P 2:3, 3:, 5:TRUE, 6:, 8:1PULSE($OUT[3], TRUE,1);ENDFOLD;FOLD PULSE 14 '' State=TRUE Time=1 sec;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VPULSE,%P 2:14, 3:, 5:TRUE, 6:, 8:1PULSE($OUT[14], TRUE,1);ENDFOLDWAIT SEC 0.5WAIT FOR $IN[2]$OUT[3] = FALSE$OUT[14] = FALSECASE 4;FOLD PULSE 4 '' State=TRUE Time=1 sec;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VPULSE,%P 2:4, 3:, 5:TRUE, 6:, 8:1PULSE($OUT[4], TRUE,1);ENDFOLD;FOLD PULSE 14 '' State=TRUE Time=1 sec;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VPULSE,%P 2:14, 3:, 5:TRUE, 6:, 8:1PULSE($OUT[14], TRUE,1);ENDFOLDWAIT SEC 0.5WAIT FOR $IN[2]$OUT[4] = FALSE$OUT[14] = FALSEDEFAULTMsgQuit("Error")ENDSWITCHEND7、机台启动控制程序（完成机台启动，取消工作中信号给开始加工信号）&ACCESS RV2&REL 3&PARAM EDITMASK = *&PARAM TEMPLATE = C:\KRC\Roboter\Template\ExpertVorgabeDEF Machine_Start(Num:IN)INT NumSWITCH NumCASE 1;FOLD OUT 5 '' State=FALSE ;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:5, 3:, 5:FALSE, 6: $OUT[5]=FALSE;ENDFOLD;FOLD PULSE 6 '' State=TRUE Time=0.5 sec;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VPULSE,%P 2:6, 3:, 5:TRUE, 6:, 8:0.5PULSE($OUT[6], TRUE,0.5);ENDFOLDCASE 2;FOLD OUT 8 '' State=FALSE ;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:8, 3:, 5:FALSE, 6: $OUT[8]=FALSE;ENDFOLD;FOLD PULSE 9 '' State=TRUE Time=0.5 sec;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VPULSE,%P 2:9, 3:, 5:TRUE, 6:, 8:0.5PULSE($OUT[9], TRUE,0.5);ENDFOLDCASE 3;FOLD OUT 11 '' State=FALSE ;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:11, 3:, 5:FALSE,6:$OUT[11]=FALSE;ENDFOLD;FOLD PULSE 12 '' State=TRUE Time=0.5 sec;%{PE}%R 8.3.40,%MKUKATPBASIS,%COUT,%VPULSE,%P 2:12, 3:, 5:TRUE, 6:, 8:0.5PULSE($OUT[12], TRUE,0.5);ENDFOLDDEFAULTMsgQuit("Error")ENDSWITCHEND8、手爪控制程序（完成对手爪的控制）手爪1控制点位为OUT[16]，对应检测为IN[10]，对应中断4手爪2控制点位为OUT[15]，对应检测为IN[11]，对应中断5&ACCESS RV2&REL 50&PARAM EDITMASK = *&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabeDEF Gripper_SetStatus(Num:IN,Status:IN)INT NumCHAR StatusSWITCH NumCASE 1SWITCH StatusCASE "O"IF NOT $T1 THENINTERRUPT OFF 4ENDIF$OUT[16] = FALSEWAIT SEC 0.3WAIT FOR NOT $IN[10]Part0InLeft = FALSEPart2InLeft = FALSECASE "C"IF NOT $T1 THENINTERRUPT ON 4ENDIF$OUT[16] = TRUEWAIT SEC 0.5WAIT FOR $IN[10]DEFAULTMsgQuit("Element Error") ENDSWITCHCASE 2SWITCH StatusCASE "O"IF NOT $T1 THENINTERRUPT OFF 5ENDIFINTERRUPT OFF 89$OUT[15] = FALSEWAIT SEC 0.3WAIT FOR NOT $IN[11]Part1InRight = FALSECASE "C"IF NOT $T1 THENINTERRUPT ON 5ENDIF$OUT[15] = TRUEWAIT SEC 0.5WAIT FOR $IN[11]Part1InRight = TRUEDEFAULTMsgQuit("Element Error")ENDSWITCHDEFAULTMsgQuit("Element Error") ENDSWITCHEND9、手爪真空检测（完成对掉料状况的检测）&ACCESS RV2&REL 10&PARAM EDITMASK = *&PARAM TEMPLATE = C:\KRC\Roboter\Template\FunctionVorgabe DEFFCT BOOL Gripper_ChkStatus(Num:IN,Status:IN)INT NumCHAR StatusSWITCH NumCASE 1SWITCH StatusCASE "O"IF NOT $OUT[16] AND NOT $IN[10] THENRETURN TRUEELSERETURN FALSEENDIFCASE "C"IF $OUT[16] AND $IN[10] THENRETURN TRUEELSERETURN FALSEENDIFDEFAULTMsgQuit("Element Error")ENDSWITCHCASE 2SWITCH StatusCASE "O"IF NOT $OUT[15] AND NOT $IN[11] THENRETURN TRUEELSERETURN FALSEENDIFCASE "C"IF $OUT[15] AND $IN[11] THENRETURN TRUEELSERETURN FALSEENDIFDEFAULTMsgQuit("Element Error")RETURN FALSEENDSWITCHCASE 3SWITCH StatusCASE "O"IF NOT $OUT[15] AND NOT $OUT[16] AND NOT $IN[10] AND NOT $IN[11] THENELSERETURN FALSEENDIFCASE "C"IF $OUT[15] AND $OUT[16] AND $IN[10] AND $IN[11] THEN RETURN TRUEELSERETURN FALSEENDIFDEFAULTMsgQuit("Element Error")RETURN FALSEENDSWITCHDEFAULTMsgQuit("Element Error")RETURN FALSEENDSWITCHENDFCT10、装夹检测子程序（完成对每台机的装夹检测与装夹异常信号交互）11、后台监视程序（对安全姿态信号自动触发等后台程序进行配置）&ACCESS R&COMMENT HandlerOnRobotFaultDEF 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 DECLBOOL ApplicationRunFlagDECL CHAR ID[3];ENDFOLD (BASISTECH DECL);ENDFOLD (DECLARATIONS);FOLD BASISTECH INITINTERRUPT OFF 3STOPM_FLAG=TRUE ;Reflects state of interrupt 3 to activate/deactivate $Stopmess interrupt ID[]="CTL"If ($STOPMESS==TRUE) THENBRAKE;ENDFOLD (BASISTECH INIT);FOLD USER STOP;Make your modifications here;ENDFOLD (USER STOP);FOLD BASISTECH STOPP00 (#EXT_ERR,#PGNO_GET,ID[],128 )ApplicationRunFlag=FALSEIF (Appl_Run>0) THENIF $OUT[Appl_Run] THENApplicationRunFlag=TRUE$OUT[Appl_Run]=FALSEENDIFENDIFREPEATPOWER=SYNC()HALTUNTIL (($STOPMESS==FALSE) AND ($POWER_FAIL==FALSE)) ;ENDFOLD (BASISTECH STOP);FOLD BASISTECH RESTARTP00 (#EXT_ERR,#PGNO_GET,ID[],0 )IF (ApplicationRunFlag==TRUE) THENIF (Appl_Run>0) THEN$OUT[Appl_Run]=TRUEENDIFENDIF;ENDFOLD (BASISTECH RESTART);FOLD USER RESTART;Make your modifications here;ENDFOLD (USER RESTART);FOLD BASISTECH REACTIVATEEndifINTERRUPT ON 3STOPM_FLAG=FALSE ;Reflects state of interrupt 3 to activate/deactivate $Stopmess interrupt ;ENDFOLD (BASISTECH REACTIVATE)END。