VRM_20-21/Project_1/rapid_programs/ROB2/Module1.mod

MODULE Module1
    CONST robtarget T_Vacuum1_0:=[[590.690255644,-3249.342571232,825.6],[0,0,1,0],[-1,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_Servo1_0:=[[590.690255644,-3249.342571232,825.6],[0,0.707106781,0.707106781,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_Box_Lift_approach:=[[534.319,-3218.803,434.087],[0,0,1,0],[0,0,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_Box_Lift:=[[534.319,-3218.803,34.087],[0,0,1,0],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_Box_Intermediate:=[[517.234522163,-3699.371524499,310.765429546],[0.000260405,-0.388811853,-0.921316982,0.000542116],[-1,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_Box_Conv_approach:=[[-92.911,-3931.314,423.19],[0,0.707106781,0.707106781,0],[-2,-1,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_Box_Conv_approach1:=[[-65.83563274,-3871.631380514,353.993535883],[0.006565919,-0.767745501,-0.640511913,0.016377505],[-2,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_Box_Conv:=[[-92.879,-3829.582,173.19],[0,0.707106781,0.707106781,0],[-2,-1,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

    CONST robtarget T_Plate_ConvEnd_ap:=[[188.881,-2718.465,334],[0,0,1,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_Plate_ConvEnd:=[[188.881,-2718.465,184.03],[0,0,1,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_Plate_Boxed_approach:=[[-93.122,-3792.117,412.433],[0,0.707106781,0.707106781,0],[-2,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_Plate_Boxed:=[[-93.122,-3792.117,212.433],[0,0.707106781,0.707106781,0],[-2,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    
    CONST robtarget T_Lid_approach:=[[425.927,-3606.283,267.2],[0,0,1,0],[-1,-1,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_Lid:=[[424.931,-3606.285,17.2],[0,0,1,0],[-1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_LidOn_approach2:=[[-92.859,-3792.015,331.34],[0,0,1,0],[-2,-2,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    CONST robtarget T_LidOn:=[[-121.479,-3791.215,237.49],[0,0,1,0],[-2,-2,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    
    VAR num robot1_state := 0;

    PROC main()
        TEST robot1_state
            CASE 0:
                MoveAbsJ [[0,0,0,0,0,0], [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, fine, tool0;
                WaitTime 0.5;
                Path_10_act;
                MoveAbsJ [[1.86,22.04,-43.98,0,112,1.86], [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, fine, tool0;
                robot1_state := 1;
            CASE 1:
                WaitDI di_plateArr, 1;
                Path_20_act;
                robot1_state := 2;
            CASE 2:
                Path_30_act;
                robot1_state := 3;
            CASE 3:
                MoveAbsJ [[0,0,0,0,0,0], [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v1000, fine, tool0;
        ENDTEST
    ENDPROC
    PROC Path_10_act()
        MoveJ T_Vacuum1_0,v1000,z100,VaccumOne\WObj:=wobj0;
        MoveJ T_Box_Lift_approach,v1000,z100,VaccumOne\WObj:=wobj0;
        MoveJ T_Box_Lift,v1000,fine,VaccumOne\WObj:=wobj0;
        WaitTime 0.1;
        PulseDO do_attBox;
        WaitTime 0.1;
        MoveJ T_Box_Lift_approach,v1000,z100,VaccumOne\WObj:=wobj0;
        MoveL T_Box_Intermediate,v1000,z200,VaccumOne\WObj:=wobj0;
        MoveL T_Box_Conv_approach1,v1000,z100,VaccumOne\WObj:=wobj0;
        MoveJ T_Box_Conv,v1000,fine,VaccumOne\WObj:=wobj0;
        WaitTime 0.1;
        PulseDO do_detBox;
        WaitTime 0.1;
        MoveJ T_Box_Conv_approach,v1000,z100,VaccumOne\WObj:=wobj0;
        MoveJ T_Servo1_0,v1000,z100,Servo\WObj:=wobj0;
    ENDPROC
    PROC Path_20_act()
        MoveJ T_Plate_ConvEnd_ap,v1000,z100,Servo\WObj:=wobj0;
        PulseDO do_readyGrip1;
        MoveJ T_Plate_ConvEnd,v1000,fine,Servo\WObj:=wobj0;
        WaitTime 0.1;
        PulseDO do_gripGrip1;
        PulseDO do_attPlate1;
        WaitTime 0.1;
        MoveJ T_Plate_ConvEnd_ap,v1000,z100,Servo\WObj:=wobj0;
        !MoveJ T_Plate_Intermediate,v1000,z100,Servo\WObj:=wobj0;
        MoveJ T_Plate_Boxed_approach,v1000,z100,Servo\WObj:=wobj0;
        MoveJ T_Plate_Boxed,v1000,fine,Servo\WObj:=wobj0;
        WaitTime 0.1;
        PulseDO do_readyGrip1;
        PulseDO do_detPlate1;
        WaitTime 0.1;
        MoveJ T_Plate_Boxed_approach,v1000,z20,Servo\WObj:=wobj0;
        WaitTime 0.3;
        PulseDO do_homeGrip1;
    ENDPROC
    PROC Path_30_act()
        MoveJ T_Lid_approach,v1000,z100,VaccumOne\WObj:=wobj0;
        MoveJ T_Lid,v1000,fine,VaccumOne\WObj:=wobj0;
        WaitTime 0.1;
        PulseDO do_attLid;
        WaitTime 0.1;
        MoveJ T_Lid_approach,v1000,z100,VaccumOne\WObj:=wobj0;
        MoveJ T_LidOn_approach2,v1000,z100,VaccumOne\WObj:=wobj0;
        MoveJ T_LidOn,v1000,fine,VaccumOne\WObj:=wobj0;
        WaitTime 0.1;
        PulseDO do_detLid;
        WaitTime 0.1;
        MoveJ T_LidOn_approach2,v1000,z50,VaccumOne\WObj:=wobj0;
        WaitTime 0.3;
        PulseDO do_boxRdy;
    ENDPROC
ENDMODULE