以下为我编的程序:
MODULE MainMoudle
PERS wobjdata WobjPallet_L:=[FALSE,TRUE,"",[[-456.216,-2058.49,-233.373],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];
PERS wobjdata WobjPallet_R:=[FALSE,TRUE,"",[[-421.764,1102.39,-233.373],[1,0,0,0]],[[0,0,0],[1,0,0,0]]];
PERS tooldata tGripper:=[TRUE,[[0,0,527],[1,0,0,0]],[40,[0,0,150],[1,0,0,0],0,0,0]];
PERS loaddata LoadFull:=[20,[0,0,300],[1,0,0,0],0,0,0.1];
PERS wobjdata CurWobj;
PERS jointtarget jposHome:=[[-6.68707E-05,5.57038E-05,-0.000191306,0,0,-28.1719],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPlaceBase0_L:=[[341.98,1190.11,-618.57],[9.69378E-07,0.269112,-0.963109,1.53836E-06],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPlaceBase90_L:=[[-335.46,1280.3,-455.32],[1.28548E-06,0.867254,-0.497866,1.26441E-06],[1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPlaceBase0_R:=[[296.473529255,212.21064316,3.210904169],[0,0.707221603,-0.70699194,0],[1,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
CONST robtarget pPlaceBase90_R:=[[218.407102669,695.953395421,3.210997808],[0,-0.00038594,0.999999926,0],[1,0,1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
CONST robtarget pPick_L:=[[2182.03,-26.69,-633.76],[1.77017E-06,0.268979,-0.963146,-5.05762E-07],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPick_R:=[[1611.055992534,442.364097921,-26.736584068],[0,0.707220363,-0.706993181,0],[0,0,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
CONST robtarget pHome:=[[2182.04,-26.69,564.94],[1.72802E-06,0.268979,-0.963146,-5.05762E-07],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS robtarget pPlaceBase0;
PERS robtarget pPlaceBase90;
PERS robtarget pPick;
PERS robtarget pPlace;
PERS robtarget pPickSafe;
PERS num nCycleTime:=16.75;
PERS num nCount_L:=1;
PERS num nCount_R:=1;
PERS num nPallet:=2;
PERS num nPalletNo:=1;
PERS num nPickH:=300;
PERS num nPlaceH:=250;
PERS num nBoxL:=715;
PERS num nBoxW:=510;
PERS num nBoxH:=140;
VAR clock Timer1;
PERS bool bReady:=TRUE;
PERS bool bPalletFull_L:=TRUE;
PERS bool bPalletFull_R:=FALSE;
PERS bool bGetPosition:=TRUE;
VAR triggdata HookAct;
VAR triggdata HookOff;
VAR intnum iPallet_L;
VAR intnum iPallet_R;
PERS speeddata vMinEmpty:=[1000,100,6000,1000];
PERS speeddata vMidEmpty:=[1100,100,6000,1000];
PERS speeddata vMaxEmpty:=[1000,100,6000,1000];
PERS speeddata vMinLoad:=[1000,100,6000,1000];
PERS speeddata vMidLoad:=[1000,300,6000,1000];
PERS speeddata vMaxLoad:=[1000,300,6000,1000];
PERS num Compensation{32,3}:=[[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0]];
VAR pos pos1:=[0,0,0];
PROC main()
rInitAll;
WHILE TRUE DO
IF bReady THEN
rPick;
rPlace;
ENDIF
rCycleCheck;
ENDWHILE
ENDPROC
PROC rInitAll()
rCheckHomePos;
ConfL\OFF;
ConfJ\OFF;
nCount_L:=1;
nCount_R:=1;
nPallet:=1;
nPalletNo:=1;
bPalletFull_L:=FALSE;
bPalletFull_R:=FALSE;
bGetPosition:=FALSE;
Reset do00_ClampAct;
Reset do01_HookAct;
ClkStop Timer1;
ClkReset Timer1;
TriggEquip HookAct,100,0.1\DOp:=do01_HookAct,1;
TriggEquip HookOff,100\Start,0.1\DOp:=do01_HookAct,0;
IDelete iPallet_L;
CONNECT iPallet_L WITH tEjectPallet_L;
ISignalDI di02_PalletInPos_L,0,iPallet_L;
IDelete iPallet_R;
CONNECT iPallet_R WITH tEjectPallet_R;
ISignalDI di03_PalletInPos_R,0,iPallet_R;
ENDPROC
PROC rPick()
ClkReset Timer1;
ClkStart Timer1;
rCalPosition;
MoveJ Offs(pPick,0,0,nPickH),vMaxEmpty,z50,tGripper\WObj:=wobj0;
MoveL pPick,vMinLoad,fine,tGripper\WObj:=wobj0;
Set do00_ClampAct;
Waittime 0.3;
GripLoad LoadFull;
TriggL Offs(pPick,0,0,nPickH),vMinLoad,HookAct,z50,tGripper\WObj:=wobj0;
MoveL pPickSafe,vMaxLoad,z100,tGripper\WObj:=wobj0;
MoveL Offs(pPicksafe,-350,0,0),vMaxLoad,z100,tGripper\WObj:=wobj0;
MoveL Offs(pPicksafe,-350,0,400),vMaxLoad,z100,tGripper\WObj:=wobj0;
ENDPROC
PROC rPlace()
MoveJ Offs(pPlace,0,0,nPlaceH),vMaxLoad,z50,tGripper\WObj:=CurWobj;
TriggL pPlace,vMinLoad,HookOff,fine,tGripper\WObj:=CurWobj;
Reset do00_ClampAct;
Waittime 0.5;
GripLoad Load0;
MoveL Offs(pPlace,0,0,nPlaceH),vMinEmpty,z50,tGripper\WObj:=CurWobj;
rPlaceRD;
MoveJ pPickSafe,vMaxEmpty,z50,tGripper\WObj:=wobj0;
ClkStop Timer1;
nCycleTime:=ClkRead(Timer1);
ENDPROC
PROC rCycleCheck()
TPErase;
TPWrite "The Robot is running!";
TPWrite "Last cycle time is : "\Num:=nCycleTime;
TPWrite "The number of the Boxes in the Left pallet is:"\Num:=nCount_L-1;
TPWrite "The number of the Boxes in the Right pallet is:"\Num:=nCount_R-1;
IF (bPalletFull_L=FALSE AND di02_PalletInPos_L=1 AND di00_BoxInPos_L=1) OR (bPalletFull_R=FALSE AND di03_PalletInPos_R=1 AND di01_BoxInPos_R=1) THEN
bReady:=TRUE;
ELSE
bReady:=FALSE;
WaitTime 0.1;
ENDIF
ENDPROC
PROC rCalPosition()
bGetPosition:=FALSE;
WHILE bGetPosition=FALSE DO
TEST nPallet
CASE 1:
IF bPalletFull_L=FALSE AND di02_PalletInPos_L=1 AND di00_BoxInPos_L=1 THEN
pPick:=pPick_L;
pPlaceBase0:=pPlaceBase0_L;
pPlaceBase90:=pPlaceBase90_L;
CurWobj:=Wobj0;
pPlace:=pPattern(nCount_L);
bGetPosition:=TRUE;
nPalletNo:=1;
ELSE
bGetPosition:=FALSE;
ENDIF
nPallet:=2;
CASE 2:
IF bPalletFull_R=FALSE AND di03_PalletInPos_R=1 AND di01_BoxInPos_R=1 THEN
pPick:=pPick_R;
pPlaceBase0:=pPlaceBase0_R;
pPlaceBase90:=pPlaceBase90_R;
CurWobj:=WobjPallet_R;
pPlace:=pPattern(nCount_R);
bGetPosition:=TRUE;
nPalletNo:=2;
ELSE
bGetPosition:=FALSE;
ENDIF
nPallet:=1;
DEFAULT:
TPERASE;
TPWRITE "The data 'nPallet' is error,please check it!";
Stop;
ENDTEST
ENDWHILE
ENDPROC
FUNC robtarget pPattern(num nCount)
VAR robtarget pTarget;
IF nCount>=1 AND nCount<=4 THEN
pPickSafe:=Offs(pPick,0,0,400);
ELSEIF nCount>=5 AND nCount<=8 THEN
pPickSafe:=Offs(pPick,0,0,400);
ELSEIF nCount>=9 AND nCount<=12 THEN
pPickSafe:=Offs(pPick,0,0,400);
ELSEIF nCount>=13 AND nCount<=16 THEN
pPickSafe:=Offs(pPick,0,0,400);
ELSEIF nCount>=17 AND nCount<=20 THEN
pPickSafe:=Offs(pPick,0,0,400);
ELSEIF nCount>=21 AND nCount<=24 THEN
pPickSafe:=Offs(pPick,0,0,400);
ELSEIF nCount>=25 AND nCount<=28 THEN
pPickSafe:=Offs(pPick,0,0,400);
ELSEIF nCount>=29 AND nCount<=32 THEN
pPickSafe:=Offs(pPick,0,0,400);
ENDIF
TEST nCount
CASE 1:
pTarget.trans.x:=pPlaceBase0.trans.x;
pTarget.trans.y:=pPlaceBase0.trans.y;
pTarget.trans.z:=pPlaceBase0.trans.z;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 2:
pTarget.trans.x:=pPlaceBase90.trans.x+nBoxL+20;
pTarget.trans.y:=pPlaceBase90.trans.y+nBoxW-30;
pTarget.trans.z:=pPlaceBase90.trans.z+nBoxH-120;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 3:
pTarget.trans.x:=pPlaceBase0.trans.x-nBoxW-15;
pTarget.trans.y:=pPlaceBase0.trans.y+nBoxL-50;
pTarget.trans.z:=pPlaceBase0.trans.z+nBoxH+70;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 4:
pTarget.trans.x:=pPlaceBase90.trans.x;
pTarget.trans.y:=pPlaceBase90.trans.y;
pTarget.trans.z:=pPlaceBase90.trans.z+nBoxH-180;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 5:
pTarget.trans.x:=pPlaceBase0.trans.x-nBoxW-40;
pTarget.trans.y:=pPlaceBase0.trans.y;
pTarget.trans.z:=pPlaceBase0.trans.z+nBoxH;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 6:
pTarget.trans.x:=pPlaceBase90.trans.x;
pTarget.trans.y:=pPlaceBase90.trans.y+nBoxW-30;
pTarget.trans.z:=pPlaceBase90.trans.z+2*nBoxH-100;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 7:
pTarget.trans.x:=pPlaceBase0.trans.x+15;
pTarget.trans.y:=pPlaceBase0.trans.y+nBoxL-70;
pTarget.trans.z:=pPlaceBase0.trans.z+2*nBoxH;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 8:
pTarget.trans.x:=pPlaceBase90.trans.x+nBoxL+15;
pTarget.trans.y:=pPlaceBase90.trans.y;
pTarget.trans.z:=pPlaceBase90.trans.z+2*nBoxH-140;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 9:
pTarget.trans.x:=pPlaceBase0.trans.x;
pTarget.trans.y:=pPlaceBase0.trans.y;
pTarget.trans.z:=pPlaceBase0.trans.z+2*nBoxH+30;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 10:
pTarget.trans.x:=pPlaceBase90.trans.x+nBoxL+25;
pTarget.trans.y:=pPlaceBase90.trans.y+nBoxW;
pTarget.trans.z:=pPlaceBase90.trans.z+3*nBoxH-140;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 11:
pTarget.trans.x:=pPlaceBase0.trans.x-nBoxW-15;
pTarget.trans.y:=pPlaceBase0.trans.y+nBoxL-50;
pTarget.trans.z:=pPlaceBase0.trans.z+3*nBoxH+80;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 12:
pTarget.trans.x:=pPlaceBase90.trans.x+30;
pTarget.trans.y:=pPlaceBase90.trans.y;
pTarget.trans.z:=pPlaceBase90.trans.z+3*nBoxH-180;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 13:
pTarget.trans.x:=pPlaceBase0.trans.x-nBoxW-40;
pTarget.trans.y:=pPlaceBase0.trans.y-15;
pTarget.trans.z:=pPlaceBase0.trans.z+3*nBoxH+70;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 14:
pTarget.trans.x:=pPlaceBase90.trans.x;
pTarget.trans.y:=pPlaceBase90.trans.y+nBoxW-65;
pTarget.trans.z:=pPlaceBase90.trans.z+4*nBoxH-110;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 15:
pTarget.trans.x:=pPlaceBase0.trans.x-15;
pTarget.trans.y:=pPlaceBase0.trans.y+nBoxL-120;
pTarget.trans.z:=pPlaceBase0.trans.z+4*nBoxH+30;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 16:
pTarget.trans.x:=pPlaceBase90.trans.x+nBoxL+30;
pTarget.trans.y:=pPlaceBase90.trans.y-15;
pTarget.trans.z:=pPlaceBase90.trans.z+4*nBoxH-130;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 17:
pTarget.trans.x:=pPlaceBase0.trans.x;
pTarget.trans.y:=pPlaceBase0.trans.y-20;
pTarget.trans.z:=pPlaceBase0.trans.z+4*nBoxH+80;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 18:
pTarget.trans.x:=pPlaceBase90.trans.x+nBoxL-15;
pTarget.trans.y:=pPlaceBase90.trans.y+nBoxW-60;
pTarget.trans.z:=pPlaceBase90.trans.z+5*nBoxH-140;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 19:
pTarget.trans.x:=pPlaceBase0.trans.x-nBoxW-15;
pTarget.trans.y:=pPlaceBase0.trans.y+nBoxL-70;
pTarget.trans.z:=pPlaceBase0.trans.z+5*nBoxH+90;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 20:
pTarget.trans.x:=pPlaceBase90.trans.x;
pTarget.trans.y:=pPlaceBase90.trans.y-20;
pTarget.trans.z:=pPlaceBase90.trans.z+5*nBoxH-90;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 21:
pTarget.trans.x:=pPlaceBase0.trans.x-nBoxW-40;
pTarget.trans.y:=pPlaceBase0.trans.y-20;
pTarget.trans.z:=pPlaceBase0.trans.z+6*nBoxH;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 22:
pTarget.trans.x:=pPlaceBase90.trans.x+30;
pTarget.trans.y:=pPlaceBase90.trans.y+nBoxW-50;
pTarget.trans.z:=pPlaceBase90.trans.z+6*nBoxH-70;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 23:
pTarget.trans.x:=pPlaceBase0.trans.x+15;
pTarget.trans.y:=pPlaceBase0.trans.y+nBoxL-90;
pTarget.trans.z:=pPlaceBase0.trans.z+6*nBoxH+40;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 24:
pTarget.trans.x:=pPlaceBase90.trans.x+nBoxL+10;
pTarget.trans.y:=pPlaceBase90.trans.y-20;
pTarget.trans.z:=pPlaceBase90.trans.z+6*nBoxH-110;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 25:
pTarget.trans.x:=pPlaceBase0.trans.x;
pTarget.trans.y:=pPlaceBase0.trans.y-30;
pTarget.trans.z:=pPlaceBase0.trans.z+6*nBoxH+100;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 26:
pTarget.trans.x:=pPlaceBase90.trans.x+nBoxL-5;
pTarget.trans.y:=pPlaceBase90.trans.y+nBoxW-60;
pTarget.trans.z:=pPlaceBase90.trans.z+7*nBoxH-130;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 27:
pTarget.trans.x:=pPlaceBase0.trans.x-nBoxW-15;
pTarget.trans.y:=pPlaceBase0.trans.y+nBoxL-95;
pTarget.trans.z:=pPlaceBase0.trans.z+7*nBoxH+145;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 28:
pTarget.trans.x:=pPlaceBase90.trans.x;
pTarget.trans.y:=pPlaceBase90.trans.y-30;
pTarget.trans.z:=pPlaceBase90.trans.z+7*nBoxH;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 29:
pTarget.trans.x:=pPlaceBase0.trans.x-nBoxW-40;
pTarget.trans.y:=pPlaceBase0.trans.y-40;
pTarget.trans.z:=pPlaceBase0.trans.z+7*nBoxH+180;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 30:
pTarget.trans.x:=pPlaceBase90.trans.x;
pTarget.trans.y:=pPlaceBase90.trans.y+nBoxW-70;
pTarget.trans.z:=pPlaceBase90.trans.z+8*nBoxH-70;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 31:
pTarget.trans.x:=pPlaceBase0.trans.x+15;
pTarget.trans.y:=pPlaceBase0.trans.y+nBoxL-110;
pTarget.trans.z:=pPlaceBase0.trans.z+8*nBoxH+60;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 32:
pTarget.trans.x:=pPlaceBase90.trans.x+nBoxL-20;
pTarget.trans.y:=pPlaceBase90.trans.y-40;
pTarget.trans.z:=pPlaceBase90.trans.z+8*nBoxH-80;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
DEFAULT:
TPErase;
TPWrite "The data 'nCount' is error,please check it !";
stop;
ENDTEST
Return pTarget;
ENDFUNC
PROC rPlaceRD()
TEST nPalletNo
CASE 1:
Incr nCount_L;
IF nCount_L>32 THEN
Set do02_PalletFull_L;
bPalletFull_L:=TRUE;
nCount_L:=1;
IF bPalletFull_L=TRUE AND di02_PalletInPos_L=1 THEN
Reset do02_PalletFull_L;
ENDIF
CASE 2:
Incr nCount_R;
IF nCount_R>15 THEN
Set do03_PalletFull_R;
bPalletFull_R:=TRUE;
nCount_R:=1;
ENDIF
DEFAULT:
TPERASE;
TPWRITE "The data 'nPalletNo' is error,please check it!";
Stop;
ENDTEST
ENDPROC
PROC rCheckHomePos()
VAR robtarget pActualPos;
IF NOT CurrentPos(pHome,tGripper) THEN
pActualpos:=CRobT(\Tool:=tGripper\WObj:=wobj0);
pActualpos.trans.z:=pHome.trans.z;
MoveL pActualpos,v500,z10,tGripper;
MoveJ pHome,v1000,fine,tGripper;
ENDIF
ENDPROC
FUNC bool CurrentPos(robtarget ComparePos,INOUT tooldata TCP)
VAR num Counter:=0;
VAR robtarget ActualPos;
ActualPos:=CRobT(\Tool:=TCP\WObj:=wobj0);
IF ActualPos.trans.x>ComparePos.trans.x-25 AND ActualPos.trans.x<ComparePos.trans.x+25 Counter:=Counter+1;
IF ActualPos.trans.y>ComparePos.trans.y-25 AND ActualPos.trans.y<ComparePos.trans.y+25 Counter:=Counter+1;
IF ActualPos.trans.z>ComparePos.trans.z-25 AND ActualPos.trans.z<ComparePos.trans.z+25 Counter:=Counter+1;
IF ActualPos.rot.q1>ComparePos.rot.q1-0.1 AND ActualPos.rot.q1<ComparePos.rot.q1+0.1 Counter:=Counter+1;
IF ActualPos.rot.q2>ComparePos.rot.q2-0.1 AND ActualPos.rot.q2<ComparePos.rot.q2+0.1 Counter:=Counter+1;
IF ActualPos.rot.q3>ComparePos.rot.q3-0.1 AND ActualPos.rot.q3<ComparePos.rot.q3+0.1 Counter:=Counter+1;
IF ActualPos.rot.q4>ComparePos.rot.q4-0.1 AND ActualPos.rot.q4<ComparePos.rot.q4+0.1 Counter:=Counter+1;
RETURN Counter=7;
ENDFUNC
TRAP tEjectPallet_L
Reset do02_PalletFull_L;
bPalletFull_L:=FALSE;
ENDTRAP
TRAP tEjectPallet_R
Reset do03_PalletFull_R;
bPalletFull_R:=FALSE;
ENDTRAP
PROC rMoveAbsj()
MoveAbsJ jposHome\NoEOffs, v100, fine, tGripper\WObj:=wobj0;
ENDPROC
PROC rModPos()
MoveL pHome,v100,fine,tGripper\WObj:=Wobj0;
MoveL pPick_L,v100,fine,tGripper\WObj:=Wobj0;
MoveL pPick_R,v100,fine,tGripper\WObj:=Wobj0;
MoveL pPlaceBase0_L,v100,fine,tGripper\WObj:=Wobj0;
MoveL pPlaceBase90_L,v100,fine,tGripper\WObj:=Wobj0;
MoveL pPlaceBase0_R,v100,fine,tGripper\WObj:=WobjPallet_R;
MoveL pPlaceBase90_R,v100,fine,tGripper\WObj:=WobjPallet_R;
ENDPROC
ENDMODULE