ABB机械手 机器人 码垛 简单垛型代码 一台机器码两条生产线

MODULE MainModule
    CONST robtarget yuandian:=[[1271.38,-25.11,2023.30],[0.0381007,0.00383459,0.999265,-0.00185829],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
   !-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------    
    
    CONST robtarget qutuopanweizhiup:=[[2361.80,-595.80,2189.34],[0.0323203,0.00266198,0.999454,0.00625964],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget qutuopanweizhimiddle:=[[2361.88,-595.82,1593.39],[0.032351,0.00265054,0.999453,0.0062529],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget qutuopanweizhidow:=[[2362.01,-595.86,-163.83],[0.0324003,0.00264856,0.999452,0.0062541],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    !-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------    
  CONST robtarget shuipingjizundian:=[[2335.47,-339.90,554.70],[0.0377045,0.00379575,0.99928,-0.00177707],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

    !-------------A 
    CONST robtarget Axian_quliao:=[[1113.02,1812.46,600.72],[0.0177786,-0.0179848,0.999605,0.0122313],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; 
    CONST robtarget Axian_mafangweizhi:=[[-1047.73,1600.34,-15.02],[0.00829455,0.00552996,-0.999769,-0.0190338],[1,-1,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    !-------------A
    !-------------B 
    CONST robtarget Bxian_quliao:=[[1104.67,-1889.35,600.62],[0.0315602,0.019157,0.99918,-0.0166505],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; 
    CONST robtarget Bxian_mafangweizhi:=[[-1018.34,-1574.04,-15.47],[3.96022E-05,0.00997105,0.999847,-0.0143956],[-2,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    
    VAR robtarget safe_point;
    !-------------B
    !-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
    !----------------------------------System Variables

    !-------------A
    VAR num A_x:=0;
    VAR num A_y:=0;
    VAR num A_z:=0;


    VAR num A_reclaimer_offset_x:=0;
    VAR num A_reclaimer_offset_y:=0;


    VAR num A_reclaimer_pallet_dow:=0;
    VAR bool A_reclaimer_pallet_dow_flage:=true;


    !-------------A

    !-------------B
    VAR num B_x:=0;
    VAR num B_y:=0;
    VAR num B_z:=0;


    VAR num B_reclaimer_offset_x:=0;
    VAR num B_reclaimer_offset_y:=0;

    VAR num B_reclaimer_pallet_dow:=0;
    VAR bool B_reclaimer_pallet_dow_flage:=true;


    !-------------B

    

    VAR bool Aflage:=false;
    VAR bool Bflage:=true;

    VAR bool Amaterial:=false;
    VAR bool Bmaterial:=false;

    VAR num sucker_x:=550;
    VAR num sucker_y:=400;

    VAR num System_pallet_sizes_x:=1190;
    VAR num System_pallet_sizes_y:=1190;

    VAR bool go_home:=false;
    VAR bool safe_point_flage:=false;
    VAR num safe_point_dow:=0;


    !----------------------------------System Variables

    !-------------------------------------------------------------------------------------------------------------------------------------------------------------

    !----------------------------------Socket Variables
    !-------------A
    VAR num Socket_A_box_num_x:=3;
    VAR num Socket_A_box_num_y:=3;
    VAR num Socket_A_box_num_z:=3;


    !Socket_A_box_x<=>Width   Socket_A_box_y<=>Long     Socket_A_box_z<=>Hight   
    VAR num Socket_A_box_x:=400;
    VAR num Socket_A_box_y:=400;
    VAR num Socket_A_box_z:=400;

    VAR num Socket_A_box_offset_x:=0;
    VAR num Socket_A_box_offset_y:=0;
    VAR num Socket_A_box_offset_z:=50;
    VAR num A_box_wile:=0;
    VAR num A_while:=2;
    !-------------A

    !-------------B

    VAR num Socket_B_box_num_x:=3;
    VAR num Socket_B_box_num_y:=3;
    VAR num Socket_B_box_num_z:=3;


    !Socket_B_box_x<=>Width   Socket_B_box_y<=>Long     Socket_B_box_z<=>Hight   
    VAR num Socket_B_box_x:=400;
    VAR num Socket_B_box_y:=400;
    VAR num Socket_B_box_z:=400;

    VAR num Socket_B_box_offset_x:=0;
    VAR num Socket_B_box_offset_y:=0;
    VAR num Socket_B_box_offset_z:=50;
    VAR num B_box_wile:=0;
    VAR num B_while:=2;

    !-------------B

    PROC main()
        AccSet 60,60;
          !   MoveJ Bxian_mafangweizhi,v500,z50,tool0;

          
        !            MoveJ Bxian_mafangwait,v500,z50,tool0;
 
        !            MoveJ Bxian_mafangweizhi,v500,z50,tool0;

        go_home:=TRUE;

        WHILE TRUE DO
    A_box_wile:=0;
    B_box_wile:=0;

    
            IF di_A_while_num>1 THEN
                A_while:=di_A_while_num;
SetGO do_A_while_num,(A_while); 
            ENDIF
            
           IF di_B_while_num>1 THEN
                B_while:=di_B_while_num;
SetGO do_B_while_num,(B_while);  
            ENDIF
            
            IF di_go_home=1 THEN
                go_home:=TRUE;
            ENDIF

            IF id_sys_reset=1 THEN
                Set do_sys_reset;
                Reset do_Astock_num_update;
                Reset do_Aflage_update;
                Reset do_Bstock_num_update;
                Reset do_Bflage_update;

                WaitTime 0.3;
                Reset do_sys_reset;
            ENDIF

            IF go_home=TRUE THEN
                gohome_point;
                go_home:=FALSE;
            ENDIF

            IF di_Aflage=1 THEN
                Aflage:=TRUE;
            else
                Aflage:=FALSE;
            ENDIF

            IF di_Bflage=1 THEN
                Bflage:=TRUE;
            else
                Bflage:=FALSE;
            ENDIF


            !-------------A
            WHILE A_box_wile<A_while DO
                A_box_wile:=A_box_wile+1;

                    IF Aflage=TRUE  THEN

                        Set do_A_cycle;

                        !IF di_Astock_num=0 THEN
                            ! AND di_Apallet=0 and di_Abpallet=1
                            !                    Set do03_GripUp;
                            !                    Reset do04_GripDown;
                            !                    WaitTime 1;
                            !                    !A_reclaimer_pallet;

                            !                    Reset do03_GripUp;
                            !                    Set do04_GripDown;
                            !                    WaitTime 1;

                            IF di_Socket_A_box_x<>0 AND di_Socket_A_box_y<>0 AND di_Socket_A_box_z<>0 AND di_Socket_A_box_num_x<>0 AND di_Socket_A_box_num_y<>0 AND di_Socket_A_box_num_z<>0 THEN
                                
                                IF di_Socket_A_box_x>300 and di_Socket_A_box_y>300 and di_Socket_A_box_z>300 and di_Socket_A_box_num_x>1 and di_Socket_A_box_num_y>1 and di_Socket_A_box_num_z>1 THEN
                                    Socket_A_box_x:=di_Socket_A_box_x;
                                    Socket_A_box_y:=di_Socket_A_box_y;
                                    Socket_A_box_z:=di_Socket_A_box_z;
                                    Socket_A_box_num_x:=di_Socket_A_box_num_x;
                                    Socket_A_box_num_y:=di_Socket_A_box_num_y;
                                    Socket_A_box_num_z:=di_Socket_A_box_num_z;
                                    
                                    SetGO do_Socket_A_box_x,di_Socket_A_box_x;
                                    SetGO do_Socket_A_box_y,di_Socket_A_box_y;
                                    SetGO do_Socket_A_box_z,di_Socket_A_box_z;
                                    SetGO do_Socket_A_box_num_x,di_Socket_A_box_num_x;
                                    SetGO do_Socket_A_box_num_y,di_Socket_A_box_num_y;
                                    SetGO do_Socket_A_box_num_z,di_Socket_A_box_num_z;
                                     
                                    Amaterial:=TRUE;
                                ENDIF
                            ENDIF
                            Socket_A_box_offset_x:=Socket_A_box_x*((Socket_A_box_num_x-1)/2);
                            Socket_A_box_offset_y:=Socket_A_box_y*((Socket_A_box_num_y-1)/2);
                            A_reclaimer_offset_x:=(Socket_A_box_x-sucker_x)/2;
                            A_reclaimer_offset_y:=(Socket_A_box_y-sucker_y)/2;
                            IF di_Astock_num>=Socket_A_box_num_x*Socket_A_box_num_y*Socket_A_box_num_z THEN
                                Amaterial:=FALSE;
                                Aflage:=FALSE;
                                SetGO do_Astock_num,(0);
                                Set do_Astock_num_update;
                                Set do_Aflage_update;

                                WaitTime 0.3;
                                Reset do_Astock_num_update;
                                Reset do_Aflage_update;
                            ENDIF
                        !ENDIF

                        IF Amaterial=TRUE AND di_Apallet=1 and di_Amaterial=1 THEN

                            SetGO do_Astock_num,(di_Astock_num+1);
                            Set do_Astock_num_update;
                            WaitTime 0.3;
                            Reset do_Astock_num_update;
                            Axian_stock(di_Astock_num-1);

                        ENDIF
                        Reset do_A_cycle;
                    ENDIF
            ENDWHILE
            !-------------A

            !-------------B
             WHILE B_box_wile<B_while DO
            B_box_wile:=B_box_wile+1;


                    IF Bflage=TRUE  THEN

                        Set do_B_cycle;
                        !IF di_Bstock_num=0 THEN
                            !AND di_Bpallet=0 AND di_Abpallet=1
                            !                    Set do03_GripUp;
                            !                    Reset do04_GripDown;
                            !                    WaitTime 1;
                            !                    !B_reclaimer_pallet;

                            !                    Reset do03_GripUp;
                            !                    Set do04_GripDown;
                            !                    WaitTime 1;


                            IF di_Socket_B_box_x<>0 AND di_Socket_B_box_y<>0 AND di_Socket_B_box_z<>0 AND di_Socket_B_box_num_x<>0 AND di_Socket_B_box_num_y<>0 AND di_Socket_B_box_num_z<>0 THEN
                                IF di_Socket_B_box_x>300 AND di_Socket_B_box_y>300 AND di_Socket_B_box_z>300 AND di_Socket_B_box_num_x>1 AND di_Socket_B_box_num_y>1 AND di_Socket_B_box_num_z>1 THEN
                                    Socket_B_box_x:=di_Socket_B_box_x;
                                    Socket_B_box_y:=di_Socket_B_box_y;
                                    Socket_B_box_z:=di_Socket_B_box_z;
                                    Socket_B_box_num_x:=di_Socket_B_box_num_x;
                                    Socket_B_box_num_y:=di_Socket_B_box_num_y;
                                    Socket_B_box_num_z:=di_Socket_B_box_num_z;
                                    
                                    SetGO do_Socket_B_box_x,di_Socket_B_box_x;
                                    SetGO do_Socket_B_box_y,di_Socket_B_box_y;
                                    SetGO do_Socket_B_box_z,di_Socket_B_box_z;
                                    SetGO do_Socket_B_box_num_x,di_Socket_B_box_num_x;
                                    SetGO do_Socket_B_box_num_y,di_Socket_B_box_num_y;
                                    SetGO do_Socket_B_box_num_z,di_Socket_B_box_num_z;
                                     
                                    Bmaterial:=TRUE;
                                ENDIF
                            ENDIF
                            Socket_B_box_offset_x:=Socket_B_box_x*((Socket_B_box_num_x-1)/2);
                            Socket_B_box_offset_y:=Socket_B_box_y*((Socket_B_box_num_y-1)/2);
                            B_reclaimer_offset_x:=(Socket_B_box_x-sucker_x)/2;
                            B_reclaimer_offset_y:=(sucker_y-Socket_B_box_y)/2;
                            IF di_Bstock_num>=Socket_B_box_num_x*Socket_B_box_num_y*Socket_B_box_num_z THEN
                                Bmaterial:=FALSE;
                                Bflage:=FALSE;
                                SetGO do_Bstock_num,(0);
                                Set do_Bstock_num_update;
                                Set do_Bflage_update;
                                WaitTime 0.3;
                                Reset do_Bstock_num_update;
                                Reset do_Bflage_update;
                            ENDIF
                        !ENDIF


                        IF Bmaterial=TRUE AND di_Bpallet=1 and di_Bmaterial=1 THEN
                            SetGO do_Bstock_num,(di_Bstock_num+1);

                            Set do_Bstock_num_update;
                            WaitTime 0.3;
                            Reset do_Bstock_num_update;
                            Bxian_stock(di_Bstock_num-1);

                        ENDIF
                        Reset do_B_cycle;
                    ENDIF
            ENDWHILE
            !-------------B
        ENDWHILE
    ENDPROC

    PROC gohome_point()
        safe_point:=CRobT();
        safe_point_flage:=FALSE;
        WHILE safe_point_flage=FALSE DO
            safe_point_dow:=safe_point_dow+100;
            MoveJ offs(safe_point,0,0,safe_point_dow),v200,fine,tool0;
            safe_point:=CRobT();
            IF safe_point.trans.z>2000 THEN
                safe_point_flage:=TRUE;
                safe_point_dow:=0;
            ENDIF
        ENDWHILE

        MoveJ yuandian,v500,z30,tool0;
        Reset do03_GripUp;
        Set do04_GripDown;
        Set do_go_home_flage;
        WaitTime 0.3;
        Reset do_go_home_flage;
        Reset do_A_cycle;
        Reset do_B_cycle;
        go_home:=false;
    ENDPROC

    PROC Axian_stock(num stock_num)
        A_x:=(stock_num MOD (Socket_A_box_num_x*Socket_A_box_num_y) DIV Socket_A_box_num_y)*Socket_A_box_x-Socket_A_box_offset_x;
        A_y:=((stock_num MOD (Socket_A_box_num_x*Socket_A_box_num_y)) MOD Socket_A_box_num_y)*(-Socket_A_box_y)+Socket_A_box_offset_y;
        A_z:=((stock_num div (Socket_A_box_num_x*Socket_A_box_num_y))+1)*Socket_A_box_z-40;
        MoveJ Offs(Axian_quliao,A_reclaimer_offset_x,A_reclaimer_offset_y,Socket_A_box_z+956),v1500,fine,tool0;
        WaitDI di_Amaterial,1;
        MoveJ Offs(Axian_quliao,A_reclaimer_offset_x,A_reclaimer_offset_y,Socket_A_box_z),v800,fine,tool0;
        Set do_rob_Vacuum_open;
        WaitTime 0.5;
        Reset do_rob_Vacuum_open;

        MoveJ Offs(Axian_quliao,A_reclaimer_offset_x,A_reclaimer_offset_y,Socket_A_box_z+550),v800,fine,tool0;
        MoveJ Offs(Axian_quliao,A_reclaimer_offset_x,A_reclaimer_offset_y+A_y-230,Socket_A_box_z+956),v800,fine,tool0;
        MoveL Offs(Axian_mafangweizhi,A_x+30,A_y-60,A_z+Socket_A_box_offset_z),v800,fine,tool0;
        MoveJ Offs(Axian_mafangweizhi,A_x,A_y,A_z),v200,fine,tool0;
        WaitTime 0.5;
        Set do_rob_Vacuum_close;
        WaitTime 0.3;
        Reset do_rob_Vacuum_close;
        MoveJ Offs(Axian_mafangweizhi,A_x,A_y,A_z+Socket_A_box_offset_z),v200,fine,tool0;
        MoveJ Offs(Axian_mafangweizhi,0,0,2300),v800,fine,tool0;
    ENDPROC

    PROC Bxian_stock(num stock_num)
        B_x:=(stock_num MOD (Socket_B_box_num_x*Socket_B_box_num_y) DIV Socket_B_box_num_y)*Socket_B_box_x-Socket_B_box_offset_x;
        B_y:=((stock_num MOD (Socket_B_box_num_x*Socket_B_box_num_y)) MOD Socket_B_box_num_y)*(Socket_B_box_y)-Socket_B_box_offset_y;
        B_z:=((stock_num div (Socket_B_box_num_x*Socket_B_box_num_y))+1)*Socket_B_box_z-40;
        MoveJ Offs(Bxian_quliao,B_reclaimer_offset_x,B_reclaimer_offset_y,Socket_B_box_z+956),v1500,fine,tool0;
        WaitDI di_Bmaterial,1;
        MoveJ Offs(Bxian_quliao,B_reclaimer_offset_x,B_reclaimer_offset_y,Socket_B_box_z-25),v800,fine,tool0;
        Set do_rob_Vacuum_open;
        WaitTime 0.5;
        Reset do_rob_Vacuum_open;
        MoveJ Offs(Bxian_quliao,B_reclaimer_offset_x,B_reclaimer_offset_y,Socket_B_box_z+550),v800,fine,tool0;
        MoveJ Offs(Bxian_quliao,B_reclaimer_offset_x,B_reclaimer_offset_y+B_y+230,Socket_B_box_z+956),v800,fine,tool0;
        MoveL Offs(Bxian_mafangweizhi,B_x+30,B_y+60,B_z+Socket_B_box_offset_z),v800,fine,tool0;
        MoveJ Offs(Bxian_mafangweizhi,B_x,B_y,B_z),v200,fine,tool0;
        WaitTime 0.5;
        Set do_rob_Vacuum_close;
        WaitTime 0.3;
        Reset do_rob_Vacuum_close;
        MoveJ Offs(Bxian_mafangweizhi,B_x,B_y,B_z+Socket_B_box_offset_z),v200,fine,tool0;
        MoveJ Offs(Bxian_mafangweizhi,0,0,2300),v800,fine,tool0;
    ENDPROC

    PROC A_reclaimer_pallet()

        MoveJ qutuopanweizhiup,v500,z50,tool0;
        MoveJ qutuopanweizhimiddle,v200,z50,tool0;
        A_reclaimer_pallet_dow_flage:=FALSE;
        WHILE A_reclaimer_pallet_dow_flage=FALSE DO
            A_reclaimer_pallet_dow:=A_reclaimer_pallet_dow-5;
            MoveJ offs(qutuopanweizhimiddle,0,0,A_reclaimer_pallet_dow),v30,fine,tool0;

            IF di01_PalCheck=1 THEN
                WaitTime 0.5;
                Set do6_PalclampInout;
                Reset do5_PalclampOut;
                WaitTime 3;
                A_reclaimer_pallet_dow_flage:=TRUE;
                A_reclaimer_pallet_dow:=0;
            ENDIF
        ENDWHILE

        MoveJ qutuopanweizhiup,v500,z50,tool0;
        MoveJ offs(Axian_mafangweizhi,0,0,2100),v800,fine,tool0;
        MoveJ offs(Axian_mafangweizhi,0,0,-120),v500,fine,tool0;
        Set do5_PalclampOut;
        Reset do6_PalclampInout;
        WaitTime 3;
        MoveJ offs(Axian_mafangweizhi,0,0,2100),v500,fine,tool0;
    ENDPROC

    PROC B_reclaimer_pallet()
        MoveJ qutuopanweizhiup,v200,z50,tool0;
        MoveJ qutuopanweizhimiddle,v200,z50,tool0;
        B_reclaimer_pallet_dow_flage:=FALSE;
        WHILE B_reclaimer_pallet_dow_flage=FALSE DO
            B_reclaimer_pallet_dow:=B_reclaimer_pallet_dow-5;
            MoveJ offs(qutuopanweizhimiddle,0,0,B_reclaimer_pallet_dow),v30,fine,tool0;
            IF di01_PalCheck=1 THEN
                WaitTime 0.5;
                Set do6_PalclampInout;
                Reset do5_PalclampOut;
                WaitTime 3;
                B_reclaimer_pallet_dow_flage:=TRUE;
                B_reclaimer_pallet_dow:=0;
            ENDIF
        ENDWHILE
        MoveJ qutuopanweizhiup,v200,z50,tool0;
        MoveJ offs(Bxian_mafangweizhi,0,0,2100),v200,fine,tool0;
        MoveJ offs(Bxian_mafangweizhi,0,0,-120),v200,fine,tool0;
        Set do5_PalclampOut;
        Reset do6_PalclampInout;
        WaitTime 3;
        MoveJ offs(Bxian_mafangweizhi,0,0,2100),v200,fine,tool0;
    ENDPROC
ENDMODULE

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值