abb机器人码垛程序

吸盘DO8
推料DO9
传送带DO10
传感器DI10

MODULE Module1
    CONST robtarget P1:=[[429.199986758,0,529.999961927],[0.000000004,0.000000002,1,0.000000003],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget P2:=[[429.199983184,154.818507443,529.99996115],[-0.000000011,0.000000001,1,-0.000000002],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    
    !***********************************************************
    !
    ! Module:  Module1
    !
    ! Description:
    !   <Insert description here>
    !
    ! Author: Admin
    !
    ! Version: 1.0
    !
    !***********************************************************
    
    
    !***********************************************************
    !
    ! Procedure main
    !
    !   This is the entry point of your program
    !
    !***********************************************************
    VAR robtarget P3;
    VAR robtarget P4;
    VAR num a;
    
    PROC main()
        a:=0;
        
        WHILE TRUE DO
            a:=a+1;
            set do10;
            set do9;
            WaitTime 0.5;
            Reset do9;
            IF a<=5 THEN
            MoveJ offs(P1,0,0,200),v1000,fine,MyNewTool;
            WaitDI di10,1;
            MoveL P1,v1000,fine,MyNewTool;
            WaitTime 0.5;
            set do8;
            WaitTime 0.5;
            MoveJ offs(P1,0,0,200),v1000,fine,MyNewTool;
            Path_10;
            MoveJ P4,v1000,fine,MyNewTool;
            MoveJ P3,v1000,fine,MyNewTool;
            WaitTime 0.5;
            Reset do8;
            WaitTime 0.5;
            MoveJ P4,v1000,fine,MyNewTool;
            ELSE
                Stop;
            ENDIF    
        ENDWHILE
    ENDPROC     
    PROC Path_10()
        TEST a
        CASE 1:
            P3:=offs(P2,0,0,0);
        CASE 2:
            P3:=offs(P2,0,31,0);
        CASE 3:
            P3:=offs(P2,46,15,0);
            P3:=RelTool(P3,0,0,0\RZ:=90);
        CASE 4:
            P3:=offs(P2,77,15,0);
            P3:=RelTool(P3,0,0,0\RZ:=90);
        CASE 5:
            P3:=offs(P2,-15,15,31);
            P3:=RelTool(P3,0,0,0\RZ:=90);
        ENDTEST
        p4:=Offs(p3,0,0,200);
     
        
        
     
    ENDPROC
    
    ENDMODULE