RAPID %%% VERSION:1 LANGUAGE:ENGLISH %%% MODULE mainprg CONST robtarget pHome:=[[517.87,-0.01,708.53],[0.506292,-0.4935,0.509881,-0.490049],[-1,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]; rP1 rP2 rCheckHOMEPos DI01=1 DI02=1 Initall PROC main() !************************************* ! Main program for !************************************* Initall; WHILE TRUE DO IF DI011=1 THEN rP1; ELSEIF DI02=1 THEN rP2; ENDIF WaitTime 0.3; ENDWHILE ENDPROC PROC Initall() AccSet 100,100; VelSet 100, 2000; rCheckHOMEPos; ENDPROC PROC rCheckHOMEPos() TPErase; TPWrite "Robot is not in the Wait-Position"; TPWrite "Please jog the robot around the Wait position in manual"; TPWrite "And execute the aHome routine."; WaitTime 0.5; EXIT; ENDIF ENDPROC IF NOT CurrentPos(pHome,tool0) THEN 29 

FUNC bool CurrentPos( robtarget ComparePos, INOUT tooldata TCP) VAR num Counter:=0; VAR robtarget ActualPos; ! !--------------------------------------------------------------------------------- !Abstract : Function to compare current manipulator position with a given position !--------------------------------------------------------------------------------- ! ActualPos:=CRobT(\Tool:=tool0\WObj:=wobj0); IF ActualPos.trans.x>ComparePos.trans.x-25 AND ActualPos.trans.xComparePos.trans.y-25 AND ActualPos.trans.yComparePos.trans.z-25 AND ActualPos.trans.zComparePos.rot.q1-0.1 AND ActualPos.rot.q1ComparePos.rot.q2-0.1 AND ActualPos.rot.q2ComparePos.rot.q3-0.1 AND ActualPos.rot.q3ComparePos.rot.q4-0.1 AND ActualPos.rot.q4