ABB 机械手模板二

这种模板适合把机器人当作一套运动控制单元，顺控逻辑放在PLC里，机器人只做动作逻辑。机器人和PLC通讯可以用profinet，或直接用电缆连接。

下面是PLC和机械手通讯用到的两个任务信号：

机械手在接受D2_Gi_5_8_TaskReceive任务号后，执行动作，完成后把D2_Go5_8_TaskReceived的值设置成刚执行的任务号，告诉PLC，任务已完成。PLC判断发送任务号和接受任务号相等，知道任务已完成，置D2_Gi_5_8_TaskReceive为零，机械手执行D2_Go5_8_TaskReceived置零，这一步其实是多余的，主要是保证安全。机械手再次等待PLC的下次任务号。

机械手程序比较简单，大部分工作在PLC侧。PLC侧的顺控程序可以看我以前的博客。在此不再赘述。

MODULE MainModule
    PROC main()

        Init;
        WHILE TRUE DO
            WaitUntil D2_Gi_5_8_TaskReceive<>0;
            WaitTime 0.1;
            TEST D2_Gi_5_8_TaskReceive
            CASE 1:
                WaitTime 1;
                ! 这些为子程序，需要在后面实现机器人动作
                PickButtomCover;   
            CASE 2:
                WaitTime 1;
                PutButtomCover;  
            CASE 3:
                WaitTime 1;
                PickTopCover;    
            CASE 4:
                WaitTime 1;
                PutTopCover;   
            CASE 5:
                WaitTime 1;
                PickEndIterms;    
            CASE 6:
                WaitTime 1;
                PutOk;           
            CASE 7:
                WaitTime 1;
                PutNG;            
            DEFAULT:
                TPWrite "wrong task step";
                WaitTime WAIT_MAX;
            ENDTEST
            SetGO D2_Go5_8_TaskReceived, D2_Gi_5_8_TaskReceive;
            WaitGI D2_Gi_5_8_TaskReceive, 0;
            SetGO D2_Go5_8_TaskReceived, 0;
        ENDWHILE

    ENDPROC

    PROC Init()

        Reset do_01;
        Reset do_02;
        SetGO D2_Go5_8_TaskReceived,0;
        GoHome;
        
    ENDPROC

    PROC GoHome()
        VAR robtarget pactualPos;
        IF NOT CheckAtHomePos(pHome,Tool_Sucker) THEN
            pactualPos:=CRobT(\Tool:=Tool_Sucker\WObj:=wobj0);
            MoveL Offs(pactualPos,0,0,100),v200,z10,Tool_Sucker;
            MoveJ pHome,v1000,fine,Tool_Sucker;
        ENDIF
    ENDPROC

    FUNC bool CheckAtHomePos(robtarget comparePos,INOUT tooldata TCP)
        VAR robtarget actualPos;
        VAR bool isAtHomePos:=TRUE;
        actualPos:=CRobT(\Tool:=TCP\Wobj:=wobj0);

        IF actualPos.trans.x<comparePos.trans.x-200 OR actualPos.trans.x>comparePos.trans.x+200 THEN
            isAtHomePos:=FALSE;
        ELSEIF actualPos.trans.y<comparePos.trans.y-200 OR actualPos.trans.y>comparePos.trans.y+200 THEN
            isAtHomePos:=FALSE;
        ELSEIF actualPos.trans.z<comparePos.trans.z-200 OR actualPos.trans.z>comparePos.trans.z+200 THEN
            isAtHomePos:=FALSE;
        ELSEIF actualPos.rot.q1<comparePos.rot.q1-200 OR actualPos.rot.q1>comparePos.rot.q1+200 THEN
            isAtHomePos:=FALSE;
        ELSEIF actualPos.rot.q2<comparePos.rot.q2-200 OR actualPos.rot.q2>comparePos.rot.q2+200 THEN
            isAtHomePos:=FALSE;
        ELSEIF actualPos.rot.q3<comparePos.rot.q3-200 OR actualPos.rot.q3>comparePos.rot.q3+200 THEN
            isAtHomePos:=FALSE;
        ELSEIF actualPos.rot.q4<comparePos.rot.q4-200 OR actualPos.rot.q4>comparePos.rot.q4+200 THEN
            isAtHomePos:=FALSE;
        ENDIF

        RETURN isAtHomePos;

    ENDFUNC

ENDMODULE