MODULE MainModule

        CONST robtarget Home:=[[486.99,7.93,590.21],[0.0176087,-0.721082,0.0159713,-0.692442],[0,0,-3,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        CONST robtarget GetMtrl:=[[639.46,132.55,378.33],[0.00284426,-0.712476,0.000492174,-0.70169],[0,0,-3,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        CONST robtarget ToAssembly:=[[546.60,277.25,511.37],[0.002885,-0.712511,0.0006212,-0.701655],[0,0,-3,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        CONST robtarget Rotate:=[[546.46,394.47,412.57],[0.701944,0.00107392,0.712226,-0.00264609],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        CONST robtarget Mounting:=[[557.99,396.06,376.53],[0.715979,0.000937868,0.698116,-0.00273462],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        CONST robtarget PickMounted:=[[557.97,396.06,451.72],[0.715987,0.000930153,0.698108,-0.0027456],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        CONST robtarget ToStorage:=[[488.50,-224.24,342.06],[0.684693,-0.0142713,0.728692,-0.000822082],[-1,-1,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        CONST robtarget AtStorage:=[[681.78,-224.24,342.05],[0.684684,-0.0142849,0.7287,-0.000820676],[-1,-1,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
        CONST robtarget IntoStorage:=[[509.75,-224.23,348.14],[0.684692,-0.014292,0.728692,-0.000800338],[-1,-1,0,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

    !Main program
    PROC main()
        MoveJ Home, v1000, z50, tool_assembly;
            GripperOpen;
            ResetDoSignals;
        TPErase;
        SetDo PLC_ETH_DO0, 1; != ready
        TPWrite "Robot in ready state, press start to continue";
        WaitUntil PLC_ETH_DI4 = 1 OR PLC_ETH_DI5 = 1 OR PLC_ETH_DI6 = 1 OR PLC_ETH_DI7 = 1;
        IF PLC_ETH_DI4 = 1 THEN
            ResetDoSignals;
            MakeWave;
        ELSEIF PLC_ETH_DI5 = 1 THEN
            ResetDoSignals;
            MakeSmiley;
        ELSEIF PLC_ETH_DI6 = 1 THEN
            ResetDoSignals;
            MakeCross;
        ELSEIF PLC_ETH_DI7 = 1 THEN
            ResetDoSignals;
            TPWrite "Incorrect value";
            Stop;
        ENDIF
    ENDPROC

    PROC MakeWave()
        TPWrite "Starting the creation of part Wave";
        MoveJ Offs(GetMtrl,0,0,100), v600, z50, tool_assembly;
        MoveL GetMtrl, v100, fine, tool_assembly;
        GripperClose;
        MoveL Offs(GetMtrl,0,0,100), v600, z50, tool_assembly;
        TPWrite "Moving part Wave to assembly";
        Assembly;
        StorageWave;
    ENDPROC

    PROC MakeSmiley()
        TPWrite "Starting the creation of part Smiley";
        MoveJ Offs(GetMtrl,0,-115,100), v600, z50, tool_assembly;
        MoveL Offs(GetMtrl,0,-115,0), v100, fine, tool_assembly;
        GripperClose;
        MoveL Offs(GetMtrl,0,-115,100), v600, z50, tool_assembly;
        TPWrite "Moving part Smiley to assembly";
        Assembly;
        StorageWave;
    ENDPROC

    PROC MakeCross()
        TPWrite "Starting the creation of part Cross";
        MoveJ Offs(GetMtrl,0,-235,100), v600, z50, tool_assembly;
        MoveL Offs(GetMtrl,0,-235,0), v100, fine, tool_assembly;
        GripperClose;
        MoveL Offs(GetMtrl,0,-235,100), v600, z50, tool_assembly;
        TPWrite "Moving part Cross to assembly";
        Assembly;
        StorageWave;
    ENDPROC

    !Assembly
    PROC Assembly()
        MoveJ ToAssembly, v1000, z50, tool_assembly;
        MoveJ Rotate, v1000, z50, tool_assembly;
        MoveL Offs(Mounting,0,0,50), v600, z50, tool_assembly;
        MoveL Mounting, v600, z50, tool_assembly;
            GripperOpen;
        MoveL PickMounted, v600, z50, tool_assembly;
            GripperClose;
        MoveL Offs(PickMounted,0,0,200), v600, z50, tool_assembly;
        TPwrite "Moving assemblied part to storage";
        MoveJ ToStorage, v1000, z50, tool_assembly;
    ENDPROC

    !Storage difference
    PROC StorageWave()
        MoveJ AtStorage, v200, z50, tool_assembly;
        MoveL IntoStorage, v200, fine, tool_assembly;
        GripperOpen;
        TPWrite "Operation done";
        MoveL Offs(IntoStorage,0,0,100), v200, fine, tool_assembly;
        MoveJ Home, v1000, z50, tool_assembly;
    ENDPROC

    PROC StorageSmiley()
        MoveJ Offs(AtStorage, 0,-100,0), v200, z50, tool_assembly;
        MoveL Offs(IntoStorage,0,-100,0), v200, fine, tool_assembly;
        GripperOpen;
        TPWrite "Operation done";
        MoveL Offs(IntoStorage,0,-100,100), v200, fine, tool_assembly;
        MoveJ Home, v1000, z50, tool_assembly;
    ENDPROC

    PROC StorageCross()
        MoveJ Offs(AtStorage,0,-200,0), v200, z50, tool_assembly;
        MoveL Offs(IntoStorage,0,-200,0), v200, fine, tool_assembly;
        GripperOpen;
        TPWrite "Operation done";
        MoveL Offs(IntoStorage,0,-200,100), v200, fine, tool_assembly;
        MoveJ Home, v1000, z50, tool_assembly;
    ENDPROC

    PROC GripperOpen()
        SetDO DO_Gripper_Close,0;
        SetDO DO_Gripper_Open,1;
        Waittime 2;
    ENDPROC

    PROC GripperClose()
        SetDO DO_Gripper_Open,0;
        SetDO DO_Gripper_Close,1;
        Waittime 2;
    ENDPROC
    PROC ResetDoSignals()
        Reset PLC_ETH_DO4;
        Reset PLC_ETH_DO5;
        Reset PLC_ETH_DO6;
        Reset PLC_ETH_DO7;
    ENDPROC
ENDMODULE