MoveL Offs(pPlace,0,0,nPlaceH),vMinEmpty,z50,tGripper\\WObj:=CurWobj; !利用MoveL移动至放置位置正上方 rPlaceRD;
！调用放置计数程序，其中会执行计数加1操作，并判断当前码盘是否已满载 MoveJ pPickSafe,vMaxEmpty,z50,tGripper\\WObj:=wobj0; ！利用MoveJ移动至抓取安全位置，以等待执行下一次循环 ClkStop Timer1; 停止计时
nCycleTime:=ClkRead(Timer1); 读取时钟数值，并赋值给nCycletime ENDPROC
PROC rCycleCheck() ！周期循环检查 TPErase;
TPWrite \ ！示教器清屏，并显示当前机器人运行状态
TPWrite \ ！显示上次循环运行时间
TPWrite \ TPWrite \ ！显示当前左右码盘上面已摆放产品个数。由于nCount_L和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; ！判断当前工作站状况，只要左右两侧有任何一侧满足码垛条件，则布尔量bReady为TRUE，机器人继续执行码垛任务；否则布尔量bReady为FALSE，机器人则等待码垛条件的满足 WaitTime 0.1; ENDIF ENDPROC
PROC rCalPosition() ！计算位置程序
bGetPosition:=FALSE; ！复位完成计算位置标识
WHILE bGetPosition=FALSE DO
！若未完成计算位置，则重复执行WHILE循环 TEST nPallet
！利用TEST判断执行码垛检测标识的数值，1为左侧，2为右侧 CASE 1:
！若为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:=WobjPallet_L; ！将左侧码盘工件坐标系数据赋值给当前工件坐标系
pPlace:=pPattern(nCount_L);
！调用计算放置位置功能程序，同时写入左侧计数参数，从而计算出当前需要摆放的位置数据，并赋值给当前放置目标点
bGetPosition:=TRUE;
！已完成计算位置，则将完成计算位置标识为TURE nPalletNo:=1;
！将码垛计数标识置为1，则后续会执行左侧码垛计算累计 ELSE
bGetPosition:=FALSE;
！若左侧不满足码垛任务，则完成计算位置位置标识置为FLASE，则程序会再次执行WHILE循环
ENDIF
nPallet:=2;
！将码垛检测标识置为2，则下次执行WHILE循环时检测右侧是否满足码垛条件 CASE 2:
！若为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;
！已完成计算位置，则将完成计算位置标识为TRUE
nPalletNo:=2;
！将码垛计数标识置为2，则后续会执行右侧码垛计算累计 ELSE
bGetPosition:=FALSE;
！若右侧不满足码垛任务，则将完成计算位置标识置为FLASE，则程序会再次执行WHILE循环
ENDIF
nPallet:=1;
！将码垛检测标识置为1，则下次执行WHILE循环时检测左侧是否满足码垛条件 DEFAULT: TPERASE;
TPWRITE \ Stop;
！数据nPallet数值出错处理，提示操作员检查并停止运行 ENDTEST ENDWHILE
！此种程序结构便于程序的扩展，假设在此两进两出的基础上改为四进四出，则可并列写入CASE3和CASE4。在CASE中切换nPallet的数值，是为了将各线体作为并列处理，则执行完成左侧后，下次优先检测右侧，之后下次再优先检测左侧 ENDPROC
FUNC robtarget pPattern(num nCount)
！计算摆放位置功能程序，调用时需写入计数参数，以区别计算左侧或右侧的摆放位置 VAR robtarget pTarget;
！定义一个目标点数据，用于返回摆放目标点数据 IF nCount>=1 AND nCount<=5 THEN
pPickSafe:=Offs(pPick,0,0,400); ELSEIF nCount>=6 AND nCount<=10 THEN pPickSafe:=Offs(pPick,0,0,600);
ELSEIF nCount>=11 AND nCount<=15 THEN pPickSafe:=Offs(pPick,0,0,800); ENDIF
！利用IF判断当前码垛是第几层（本案例中每层堆放5个产品），根据判断结果来设置抓取安全位置，以保证机器人不会与已码垛产品发生碰撞，抓取安全高度设置由现场实际情况来调整。此案例中的安全位置是以抓取点为基准偏移出来的，在实际中也可单独去示教一个抓取点后的安全目标点，同样也是根据码垛层数的增加而改变该安全目标点的位置 TEST nCount
！判定计数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});
！若为1，则放置在第一个摆放位置，以摆放基准点为基准，分别在X、Y、Z方向做相应的偏移，同时指定TCP姿态数据、轴配置参数等。为方便对各个摆放位置进行微调，利用Offs功能在已计算好的摆放位置基础上沿着X、Y、Z再进行微调，其中调用的是已创建的数组Compensation,例如摆放第一个位置时nCount1,则pTarget:=Offs(pTarget,Compensation｛1,1｝，Compensation｛1，2｝，Compensation｛1，3｝)；如果发现第一个摆放位置向X负方向偏了5mm，则只需在程序数据数组Compensation中将第一组数中的第一个数设为5，即可对其X方向摆放位置进行微调。 CASE 2:
pTarget.trans.x:=pPlaceBase0.trans.x+nBoxL; 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 3:
pTarget.trans.x:=pPlaceBase90.trans.x; pTarget.trans.y:=pPlaceBase90.trans.y; pTarget.trans.z:=pPlaceBase90.trans.z; pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3}); CASE 4:
pTarget.trans.x:=pPlaceBase90.trans.x+nBoxW; pTarget.trans.y:=pPlaceBase90.trans.y; pTarget.trans.z:=pPlaceBase90.trans.z; 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:=pPlaceBase90.trans.x+2*nBoxW; pTarget.trans.y:=pPlaceBase90.trans.y; pTarget.trans.z:=pPlaceBase90.trans.z; pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3}); CASE 6:
pTarget.trans.x:=pPlaceBase0.trans.x;
pTarget.trans.y:=pPlaceBase0.trans.y+nBoxL; 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 7:
pTarget.trans.x:=pPlaceBase0.trans.x+nBoxL; pTarget.trans.y:=pPlaceBase0.trans.y+nBoxL; 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 8:
pTarget.trans.x:=pPlaceBase90.trans.x;
pTarget.trans.y:=pPlaceBase90.trans.y-nBoxW; pTarget.trans.z:=pPlaceBase90.trans.z+nBoxH; 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:=pPlaceBase90.trans.x+nBoxW; pTarget.trans.y:=pPlaceBase90.trans.y-nBoxW; pTarget.trans.z:=pPlaceBase90.trans.z+nBoxH; pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3}); CASE 10:
pTarget.trans.x:=pPlaceBase90.trans.x+2*nBoxW; pTarget.trans.y:=pPlaceBase90.trans.y-nBoxW; pTarget.trans.z:=pPlaceBase90.trans.z+nBoxH; pTarget.rot:=pPlaceBase90.rot;