机器人使用Wrold Zone选项检测是否为原点程序

MODULE Checkhome
    CONST num nJointAngle:=5;
    VAR wzstationary home_limitB:=[-1];
    VAR shapedata home_spaceA;
    PERS robtarget HomeP:=[[1635.71,0.00,2005.00],[0.5,0,0.866025,0],[0,0,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
    CONST jointtarget phome1:=[[0.426531,-12.4399,17.8691,0.518168,84.5718,-90.357],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
    PERS num JudgeP;

    FUNC bool CheckHomeR(robtarget pHome,num Delta)
        VAR num count1;
        VAR robtarget Temp;
        Temp:=CRobT();
        IF Abs(Temp.trans.x-phome.trans.x)>Delta count1:=count1+1;
        IF Abs(Temp.trans.y-phome.trans.y)>Delta count1:=count1+1;
        IF Abs(Temp.trans.z-phome.trans.z)>Delta count1:=count1+1;
        IF Abs(Temp.rot.q1-phome.rot.q1)>Delta count1:=count1+1;
        IF Abs(Temp.rot.q2-phome.rot.q2)>Delta count1:=count1+1;
        IF Abs(Temp.rot.q3-phome.rot.q3)>Delta count1:=count1+1;
        IF Abs(Temp.rot.q4-phome.rot.q4)>Delta count1:=count1+1;
        IF count1=7 THEN
            RETURN FALSE;
        ELSE
            RETURN TRUE;
        ENDIF
    ENDFUNC

    PROC JudgeHome()
        IF CheckHomeR(HomeP,10) THEN
            TPWrite "Robot is not Home location,please go home";
            TPReadFK JudgeP,"Do you want to robot move to home?","","","","yes","no";
            IF JudgeP=4 THEN

                MoveJ HomeP,v200,z50,tool0\WObj:=wobj0;
                WaitTime 1;
                !PulseDO\PLength:=0.5, Local_IO_RobotHome_DO7;
            ELSEIF JudgeP=5 THEN
                Stop;
            ENDIF
        ELSE
            TPWrite "Robot in the home location";
            !PulseDO\PLength:=0.5, Local_IO_RobotHome_DO7;

        ENDIF
    ENDPROC

    PROC Power_ON()

        CONST jointtarget home_high_posA:=[[phome1.robax.rax_1+nJointAngle,phome1.robax.rax_2+nJointAngle,phome1.robax.rax_3+nJointAngle,phome1.robax.rax_4+nJointAngle,phome1.robax.rax_5+nJointAngle,phome1.robax.rax_6+nJointAngle],[phome1.extax.eax_a+nJointAngle,9E+9,9E+9,9E+9,9E+9,9E+9]];
        CONST jointtarget home_low_posA:=[[phome1.robax.rax_1-nJointAngle,phome1.robax.rax_2-nJointAngle,phome1.robax.rax_3-nJointAngle,phome1.robax.rax_4-nJointAngle,phome1.robax.rax_5-nJointAngle,phome1.robax.rax_6-nJointAngle],[phome1.extax.eax_a-nJointAngle,9E+9,9E+9,9E+9,9E+9,9E+9]];
        !        WZLimJointDef\Inside,home_spaceA,home_low_posA,home_high_posA;
        !        WZDOSet\Stat,home_limitB\Inside,home_spaceA,Local_IO_RobotHome_DO7,1;

    ENDPROC
ENDMODULE