ABB机器人双工位预约程序编写

MODULE Module1

    PERS tooldata MyTool:=[TRUE,[[31.792631019,0,229.638935148],[0.945518576,0,0.325568154,0]],[1,[0,0,1],[1,0,0,0],0,0,0]];
    CONST jointtarget jzero:=[[0,0,0,0,30,8.25295E-07],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    PERS wobjdata wobj1:=[FALSE,TRUE,"",[[700,300,600],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];
    PERS wobjdata wobj2:=[FALSE,TRUE,"",[[700,400,600],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];

    VAR robtarget pVar1:=[[0.00,0.00,0.00],[0.190809,-2.8201E-7,0.981627,-2.1482E-7],[0,1,-2,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
    VAR robtarget pVar2:=[[300.00,-0.00,0.00],[0.190809,4.44517E-8,0.981627,1.37478E-8],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
    VAR robtarget pVar3:=[[300.00,300.00,-0.00],[0.190809,-8.73085E-8,0.981627,-3.59698E-8],[0,1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
    VAR robtarget pVar4:=[[-0.00,300.00,-0.00],[0.190808,2.30957E-7,0.981627,8.12347E-8],[0,1,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];

    VAR intnum intno1:=0;
    VAR intnum intno2:=0;
    VAR bool flag1:=false;
    VAR bool flag2:=false;
    PERS wobjdata wobj_temp;


    PROC main()
        TPErase;
        trap_init;
        gohome;
        WHILE TRUE DO
            IF flag1 THEN
                flag1:=FALSE;
                wobj_temp:=wobj1;
                draw_squre;
                gohome;
            ENDIF
            IF flag2 THEN
                flag2:=FALSE;
                wobj_temp:=wobj2;
                draw_squre;
                gohome;
            ENDIF
        ENDWHILE
    ENDPROC

    PROC gohome()
        MoveAbsJ jzero,v1000,fine,MyTool;
    ENDPROC

    PROC draw_squre()
        MoveJ Offs(pVar1,0,0,200),v1000,fine,MyTool\WObj:=wobj_temp;
        MoveL pVar1,v1000,fine,MyTool\WObj:=wobj_temp;
        MoveL pVar2,v1000,fine,MyTool\WObj:=wobj_temp;
        MoveL pVar3,v1000,fine,MyTool\WObj:=wobj_temp;
        MoveL pVar4,v1000,fine,MyTool\WObj:=wobj_temp;
        MoveL pVar1,v1000,fine,MyTool\WObj:=wobj_temp;
        MoveJ Offs(pVar1,0,0,200),v1000,fine,MyTool\WObj:=wobj_temp;
    ENDPROC

    TRAP trap_1
        flag1:=TRUE;
    ENDTRAP

    TRAP trap_2
        flag2:=TRUE;
    ENDTRAP

    PROC trap_init()
        IDelete intno1;
        CONNECT intno1 WITH trap_1;
        ISignalDI di_Left,1,intno1;
        IDelete intno2;
        CONNECT intno2 WITH trap_2;
        ISignalDI di_Right,1,intno2;
    ENDPROC

ENDMODULE