ABB左右碼垛詳細解說

1、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！定義左側碼盤工件坐標系WobjPallet_LPERS wobjdata WobjPallet_R:=FALSE,TRUE,"",-421.764,1102.39,-233.373,1,0,0,0,0,0,0,1,0,0,0; ！定義右側碼盤工件坐標系WobjPallet_RPERS tooldata tGripper:=TRUE,0,0,

2、527,1,0,0,0,20,0,0,150,1,0,0,0,0,0,0;！定義工具坐標系數(shù)據(jù)tGripperPERS loaddata LoadFull:=20,0,0,300,1,0,0,0,0,0,0.1;！定義有效載荷數(shù)據(jù)LoadFullPERS wobjdata CurWobj;！定義工件坐標系數(shù)據(jù)CurWobj，此工件坐標系作為當前使用坐標系。即當在左側碼垛時，將左側碼盤坐標系WobjPllaet_L賦值給該數(shù)據(jù)；當在右側碼垛時，則將WobjPllaet_R賦值給該數(shù)據(jù)PERS jointtarget jposHome:=0,0,0,0,0,0,9E+09,9E+09,9E+09,

3、9E+09,9E+09,9E+09;！定義關節(jié)目標點數(shù)據(jù)，各關節(jié)軸數(shù)值為0，用于手動將機器人運動至各關節(jié)軸機械零位CONST robtarget pPlaceBase0_L:=296.473529255,212.21064316,3.210904169,0,0.70711295,-0.707100612,0,-2,0,-3,0,9E9,9E9,9E9,9E9,9E9,9E9;！左側不旋轉放置基準位置CONST robtarget pPlaceBase90_L:=218.407102669,695.953395421,3.210997808,0,-0.000001669,1,0,-2,0,-2,

4、0,9E9,9E9,9E9,9E9,9E9,9E9;！左側旋轉90°放置基準位置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

5、,9E9,9E9,9E9,9E9,9E9;！右側旋轉90°放置基準位置CONST robtarget pPick_L:=1627.550991372,-426.974661352,-26.736921885,0,0.707109873,-0.707103689,0,-1,0,-2,0,9E9,9E9,9E9,9E9,9E9,9E9;！左側抓取位置CONST robtarget pPick_R:=1611.055992534,442.364097921,-26.736584068,0,0.707220363,-0.706993181,0,0,0,-1,0,9E9,9E9,9E9,9E9

6、,9E9,9E9;！右側抓取位置CONST robtarget pHome:=1505.00,-0.00,878.55,1.28548E-06,0.707107,-0.707107,-1.26441E-06,0,0,-2,0,9E+09,9E+09,9E+09,9E+09,9E+09,9E+09;！程序起始點，即Home點PERS robtarget pPlaceBase0; PERS robtarget pPlaceBase90; PERS robtarget pPick; PERS robtarget pPlace;！定義目標點數(shù)據(jù)，這些數(shù)據(jù)是機器人當前使用的目標點。當在左側、右側碼垛時，

7、將對應的左側、右側基準點賦值給這些數(shù)據(jù)PERS robtarget pPickSafe;！機器人將產(chǎn)品抓取后需提升至一定的安全高度，才能向碼垛位置移動，隨著擺放位置逐層加高，此數(shù)據(jù)在程序中會被賦予不同的數(shù)值，以防止機器人與碼放號的產(chǎn)品發(fā)生碰撞PERS num nCycleTime:=3.803; ！定義數(shù)字型數(shù)據(jù)，用于存儲單次節(jié)拍時間 PERS num nCount_L:=1;PERS num nCount_R:=1;！定義數(shù)字型數(shù)據(jù)，分別用于左側、右側碼垛計數(shù)，在計算位置子程序中根據(jù)該計數(shù)計算出相應的放置位置PERS num nPallet:=2;！定義數(shù)字型數(shù)據(jù)，利用TEST指令判斷此數(shù)值

8、，從而決定執(zhí)行哪側的碼垛任務，1為左側，2為右側PERS num nPalletNo:=1;！定義數(shù)字型數(shù)據(jù)，利用TEST指令判斷此數(shù)值，從而決定執(zhí)行哪垛計數(shù)累計，1為左側，2為右側 PERS num nPickH:=300;PERS num nPlaceH:=400;！定義數(shù)字型數(shù)據(jù)，分別對應的是抓取、放置時的一個高度。例如nPickH:=300,則表示機器人快速移動至抓取位置上放300mm處，然后慢速移動至抓取位置，接著慢速將產(chǎn)品提升至抓取位置上放300mm處，最后再快速移動至其他位置 PERS num nBoxL:=605; PERS num nBoxW:=405;PERS num nB

9、oxH:=300;！定義三個數(shù)字型數(shù)據(jù)，分別對應的是產(chǎn)品長、寬、高。在計算位置程序中，通過在放置基準點上面疊加長、寬、高數(shù)值計算處放置位置VAR clock Timer1;！定義時鐘數(shù)據(jù)，用于計時PERS bool bReady:=TRUE;！定義布爾量數(shù)據(jù)，作為主程序邏輯判斷條件，當左右兩側有任何一側滿足碼垛條件時，此布爾量均為TRUE,即機器人會執(zhí)行碼垛任務，否則該布爾量為FLASE，機器人會等待直至條件滿足 PERS bool bPalletFull_L:=FALSE; PERS bool bPalletFull_R:=FALSE; ！定義兩個布爾數(shù)據(jù)，當機器人在左側碼垛時，則bPall

10、et_L為TURE，bPallet_R為FALSE,當機器人在右側時，則相反PERS bool bGetPosition:=FALSE; ！定義兩個布爾量數(shù)據(jù)，判斷是否已計算出當前取放位置 VAR triggdata HookAct; VAR triggdata HookOff; ！定義兩個觸發(fā)數(shù)據(jù)，分別對應的是夾具上面鉤爪收緊及松開動作 VAR intnum iPallet_L;VAR intnum iPallet_R;！定義兩個中斷符，對應左側、右側碼盤更換時所需觸發(fā)的相應復位操作，如滿載信號復位等 PERS speeddata vMinEmpty:=2000,400,6000,1000;

11、 PERS speeddata vMidEmpty:=3000,400,6000,1000; PERS speeddata vMaxEmpty:=5000,500,6000,1000; PERS speeddata vMinLoad:=1000,200,6000,1000; PERS speeddata vMidLoad:=2500,500,6000,1000;PERS speeddata vMaxLoad:=4000,500,6000,1000;！定義多種速度數(shù)據(jù)，分別對應空載時高、中、低速，以及滿載時的高、中、低速，便于對機器人的各個動作進行速度控制PERS num Compensatio

12、n15,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;！定義二維數(shù)組，用于各擺放位置的偏差調整；15組數(shù)據(jù)，對應15個擺放位置，每組數(shù)據(jù)3個數(shù)值，對應X、Y、Z的偏差值PROC main()！主程序rInitAll;！調用初始化程序，包括復位信號、復位程序數(shù)據(jù)、初始化中斷等WHILE TRUE DO！利用WHILE循環(huán)，將初始化程序隔離開，即只在第一次運行時需要執(zhí)行一次初始化程序，之后循環(huán)執(zhí)行拾取放置動作 IF bReady THEN！利用IF條件判斷，當

13、左右兩側至少有一側滿足碼垛條件時，判斷條件bReady為TRUE,機器人則執(zhí)行碼垛任務 rPick;！調用抓取程序 rPlace;！調用放置程序ENDIFrCycleCheck;！調用循環(huán)檢測程序，里面包含寫屏顯示循環(huán)時間、碼垛個數(shù)、判斷當前左右兩側狀況等Wait Time 0.05！循環(huán)等待時間，防止不滿足機器人動作條件的情況下程序執(zhí)行進入無限循環(huán)狀態(tài)，造成機器人控制CPU過負荷ENDWHILEENDPROCPROC rInitAll()！初始化程序rCheckHomePos;！調用檢測Home點程序，若機器人在Home點，則直接執(zhí)行后面的指令，否則機器人先安全返回Home點，然后再執(zhí)行后面

14、的指令 ConfLOFF;ConfJOFF;！關閉軸配置監(jiān)控nCount_L:=1;nCount_R:=1;！初始化左右兩側碼垛計數(shù)數(shù)據(jù)nPallet:=1;！初始化兩側碼垛任務標識，1為左側，2為右側nPalletNo:=1;！初始化兩側碼垛計數(shù)累計標識，1為左側，2為右側bPalletFull_L:=FALSE;bPalletFull_R:=FALSE;！初始化左右兩側碼垛滿載布爾量bGetPosition:=FALSE;！初始化計算位置標識，F(xiàn)ALSE為未完成計算，TRUE為已完成計算Reset do00_ClampAct;Reset do01_HookAct;！初始化夾具，夾板張開和鉤

15、爪松開ClkStop Timer1;！停止時鐘計時ClkReset Timer1;！復位時鐘TriggEquip HookAct,100,0.1DOp:=do01_HookAct,1;！定義觸發(fā)事件：鉤爪收緊。朝向指定目標點運動時提前100mm收緊鉤爪，即將產(chǎn)品鉤住，提前動作時間為0.1秒TriggEquip HookOff,100Start,0.1DOp:=do01_HookAct,0;！定義觸發(fā)事件：鉤爪松開。距離之后加上可選參變量Start，則表示在離開起點100mm處松開鉤爪，提前動作時間為0.1秒IDelete iPallet_L; CONNECT iPallet_L WITH tE

16、jectPallet_L;ISignalDI di02_PalletInPos_L,0,iPallet_L;！中斷初始化，當左側滿載碼盤到位信號為0時，即表示滿載碼盤被取走，則觸發(fā)中斷程序iPallet_L,復位左側滿載信號、滿載布爾量 IDelete iPallet_R; CONNECT iPallet_R WITH tEjectPallet_R;ISignalDI di03_PalletInPos_R,0,iPallet_R;！中斷初始化，當右側滿載碼盤單位信號變?yōu)?時，即表示滿載碼盤被取走，則觸發(fā)中斷程序iPallet_R，復位右側滿載信號、滿載布爾量等ENDPROCPROC rPick

17、()！抓取程序ClkReset Timer1;！復位時鐘ClkStart Timer1;！開始計時rCalPosition;！計算位置，包括抓取位置、抓取安全位置、放置位置等MoveJ Offs(pPick,0,0,nPickH),vMaxEmpty,z50,tGripperWObj:=wobj0;！利用MoveJ移動至抓取位置正上方MoveL pPick,vMinLoad,fine,tGripperWObj:=wobj0;！利用MoveL移動至抓取位置Set do00_ClampAct;！置位夾板信號，將夾板收緊，夾取產(chǎn)品Waittime 0.3;！預留夾具動作時間，以保證夾具已將產(chǎn)品收緊，

18、等待時間根據(jù)實際情況來調整其大??；若有夾緊反饋信號，則可利用WaitDI指令等待反饋信號變?yōu)?，從而替代固定的等待時間GripLoad LoadFull;！加載載荷數(shù)據(jù)TriggL Offs(pPick,0,0,nPickH),vMinLoad,HookAct,z50,tGripperWObj:=wobj0;！利用TriggL移動至抓取正上方，并調用觸發(fā)事件HookAct，即在距離到達點100mm處將鉤爪收緊，防止產(chǎn)品在快速移動中掉落MoveL pPickSafe,vMaxLoad,z100,tGripperWObj:=wobj0;！利用MoveL移動至抓取位置ENDPROCPROC rPla

19、ce()！放置程序MoveJ Offs(pPlace,0,0,nPlaceH),vMaxLoad,z50,tGripperWObj:=CurWobj;！利用MoveJ移動至放置位置正上方TriggL pPlace,vMinLoad,HookOff,fine,tGripperWObj:=CurWobj;！利用TriggL移動至放置位置，并調用觸發(fā)事件HookOff,即在離開放置位置正上方點為100mm后將鉤爪放開Reset do00_ClampAct;！復位夾板信號，夾板松開，放下產(chǎn)品Waittime 0.3;！預留夾具動作時間，以保證夾具已將產(chǎn)品完全放下，等待時間根據(jù)實際情況調整其大小Grip

20、Load Load0;！加載載荷數(shù)據(jù)Load0MoveL Offs(pPlace,0,0,nPlaceH),vMinEmpty,z50,tGripperWObj:=CurWobj;!利用MoveL移動至放置位置正上方rPlaceRD;！調用放置計數(shù)程序，其中會執(zhí)行計數(shù)加1操作，并判斷當前碼盤是否已滿載MoveJ pPickSafe,vMaxEmpty,z50,tGripperWObj:=wobj0;！利用MoveJ移動至抓取安全位置，以等待執(zhí)行下一次循環(huán)ClkStop Timer1;停止計時nCycleTime:=ClkRead(Timer1);讀取時鐘數(shù)值，并賦值給nCycletimeEND

21、PROCPROC rCycleCheck()！周期循環(huán)檢查 TPErase;TPWrite "The Robot is running!"！示教器清屏，并顯示當前機器人運行狀態(tài)TPWrite "Last cycle time is : "Num:=nCycleTime;！顯示上次循環(huán)運行時間TPWrite "The number of the Boxes in the Left pallet is:"Num:=nCount_L-1;TPWrite "The number of the Boxes in the Right p

22、allet is:"Num:=nCount_R-1;！顯示當前左右碼盤上面已擺放產(chǎn)品個數(shù)。由于nCount_L和nCount_R表示的是下輪循環(huán)將要擺放的第多少個產(chǎn)品，此處顯示的是碼盤上已擺放的產(chǎn)品數(shù)，所以在當前計數(shù)數(shù)值上面減去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:=FAL

23、SE;！判斷當前工作站狀況，只要左右兩側有任何一側滿足碼垛條件，則布爾量bReady為TRUE，機器人繼續(xù)執(zhí)行碼垛任務；否則布爾量bReady為FALSE，機器人則等待碼垛條件的滿足 WaitTime 0.1;ENDIFENDPROCPROC rCalPosition()！計算位置程序bGetPosition:=FALSE;！復位完成計算位置標識WHILE bGetPosition=FALSE DO！若未完成計算位置，則重復執(zhí)行WHILE循環(huán) TEST nPallet！利用TEST判斷執(zhí)行碼垛檢測標識的數(shù)值，1為左側，2為右側CASE 1:！若為1，則執(zhí)行左側檢測 IF bPalletFull

24、_L=FALSE AND di02_PalletInPos_L=1 AND di00_BoxInPos_L=1 THEN！判斷左側是否滿足碼垛條件，若條件滿足則將左側的基準位置數(shù)值賦值給當前執(zhí)行位置數(shù)據(jù) pPick:=pPick_L;！將左側抓取目標點數(shù)據(jù)賦值給當前抓取目標點 pPlaceBase0:=pPlaceBase0_L; pPlaceBase90:=pPlaceBase90_L;！將左側放置位置基準目標點數(shù)據(jù)賦值給當前放置位置基準點 CurWobj:=WobjPallet_L;！將左側碼盤工件坐標系數(shù)據(jù)賦值給當前工件坐標系 pPlace:=pPattern(nCount_L);！調用

25、計算放置位置功能程序，同時寫入左側計數(shù)參數(shù)，從而計算出當前需要擺放的位置數(shù)據(jù)，并賦值給當前放置目標點bGetPosition:=TRUE;！已完成計算位置，則將完成計算位置標識為TUREnPalletNo:=1;！將碼垛計數(shù)標識置為1，則后續(xù)會執(zhí)行左側碼垛計算累計 ELSEbGetPosition:=FALSE;！若左側不滿足碼垛任務，則完成計算位置位置標識置為FLASE，則程序會再次執(zhí)行WHILE循環(huán) ENDIF nPallet:=2;！將碼垛檢測標識置為2，則下次執(zhí)行WHILE循環(huán)時檢測右側是否滿足碼垛條件CASE 2:！若為2，則執(zhí)行右側檢測 IF bPalletFull_R=FALSE

26、 AND di03_PalletInPos_R=1 AND di01_BoxInPos_R=1 THEN！判斷右側是否滿足碼垛條件，若條件滿足，則將右側的基準位置數(shù)值賦值給當前執(zhí)行位置數(shù)據(jù) pPick:=pPick_R;！將右側抓取目標點數(shù)據(jù)賦值給當前抓取目標點 pPlaceBase0:=pPlaceBase0_R; pPlaceBase90:=pPlaceBase90_R;！將右側放置位置基準目標點數(shù)據(jù)賦值給當前放置位置基準點 CurWobj:=WobjPallet_R;！將右側碼盤工件坐標系數(shù)據(jù)賦值給當前工件坐標系 pPlace:=pPattern(nCount_R);！調用計算放置位置功

27、能程序，同時寫入右側計數(shù)參數(shù)，從而計算出當前需要擺放的位置數(shù)據(jù)，并賦值給當前放置目標點bGetPosition:=TRUE;！已完成計算位置，則將完成計算位置標識為TRUEnPalletNo:=2;！將碼垛計數(shù)標識置為2，則后續(xù)會執(zhí)行右側碼垛計算累計 ELSEbGetPosition:=FALSE;！若右側不滿足碼垛任務，則將完成計算位置標識置為FLASE，則程序會再次執(zhí)行WHILE循環(huán) ENDIF nPallet:=1;！將碼垛檢測標識置為1，則下次執(zhí)行WHILE循環(huán)時檢測左側是否滿足碼垛條件DEFAULT: TPERASE;TPWRITE "The data 'nPall

28、et' is error,please check it!" Stop;！數(shù)據(jù)nPallet數(shù)值出錯處理，提示操作員檢查并停止運行ENDTESTENDWHILE！此種程序結構便于程序的擴展，假設在此兩進兩出的基礎上改為四進四出，則可并列寫入CASE3和CASE4。在CASE中切換nPallet的數(shù)值，是為了將各線體作為并列處理，則執(zhí)行完成左側后，下次優(yōu)先檢測右側，之后下次再優(yōu)先檢測左側ENDPROCFUNC robtarget pPattern(num nCount)！計算擺放位置功能程序，調用時需寫入計數(shù)參數(shù)，以區(qū)別計算左側或右側的擺放位置VAR robtarget pTa

29、rget;！定義一個目標點數(shù)據(jù)，用于返回擺放目標點數(shù)據(jù)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個產(chǎn)品），根據(jù)判斷結果來設置抓取安全位置，以保證機

30、器人不會與已碼垛產(chǎn)品發(fā)生碰撞，抓取安全高度設置由現(xiàn)場實際情況來調整。此案例中的安全位置是以抓取點為基準偏移出來的，在實際中也可單獨去示教一個抓取點后的安全目標點，同樣也是根據(jù)碼垛層數(shù)的增加而改變該安全目標點的位置TEST nCount！判定計數(shù)nCount的數(shù)值，根據(jù)此數(shù)據(jù)的不同數(shù)值計算出不同擺放位置的目標點數(shù)據(jù) CASE 1: pTarget.trans.x:=pPlaceBase0.trans.x; pTarget.trans.y:=pPlaceBase0.trans.y; pTarget.trans.z:=pPlaceBase0.trans.z;pTarget.rot:=pPlaceBa

31、se0.rot;pTarget.robconf:=pPlaceBase0.robconf; pTarget:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,CompensationnCount,3);！若為1，則放置在第一個擺放位置，以擺放基準點為基準，分別在X、Y、Z方向做相應的偏移，同時指定TCP姿態(tài)數(shù)據(jù)、軸配置參數(shù)等。為方便對各個擺放位置進行微調，利用Offs功能在已計算好的擺放位置基礎上沿著X、Y、Z再進行微調，其中調用的是已創(chuàng)建的數(shù)組Compensation,例如擺放第一個位置時nCount1,則pTarget:=Off

32、s(pTarget,Compensation1,1，Compensation1，2，Compensation1，3)；如果發(fā)現(xiàn)第一個擺放位置向X負方向偏了5mm，則只需在程序數(shù)據(jù)數(shù)組Compensation中將第一組數(shù)中的第一個數(shù)設為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.

33、robconf:=pPlaceBase0.robconf;pTarget:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,CompensationnCount,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.

34、robconf;pTarget:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,CompensationnCount,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

35、:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,CompensationnCount,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(pTarge

36、t,CompensationnCount,1,CompensationnCount,2,CompensationnCount,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

37、nCount,1,CompensationnCount,2,CompensationnCount,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,CompensationnCount,1

38、,CompensationnCount,2,CompensationnCount,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,CompensationnCount,1,Compensa

39、tionnCount,2,CompensationnCount,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,CompensationnCount,1,Compensatio

40、nnCount,2,CompensationnCount,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;pTarget.robconf:=pPlaceBase90.robconf;pTarget:=Offs(pTarget,CompensationnCount,1,Compensatio

41、nnCount,2,CompensationnCount,3); CASE 11: pTarget.trans.x:=pPlaceBase0.trans.x; pTarget.trans.y:=pPlaceBase0.trans.y; pTarget.trans.z:=pPlaceBase0.trans.z+2*nBoxH;pTarget.rot:=pPlaceBase0.rot;pTarget.robconf:=pPlaceBase0.robconf;pTarget:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,Compens

42、ationnCount,3); CASE 12: pTarget.trans.x:=pPlaceBase0.trans.x+nBoxL; pTarget.trans.y:=pPlaceBase0.trans.y; pTarget.trans.z:=pPlaceBase0.trans.z+2*nBoxH;pTarget.rot:=pPlaceBase0.rot;pTarget.robconf:=pPlaceBase0.robconf;pTarget:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,CompensationnCount

43、,3); CASE 13: pTarget.trans.x:=pPlaceBase90.trans.x; pTarget.trans.y:=pPlaceBase90.trans.y; pTarget.trans.z:=pPlaceBase90.trans.z+2*nBoxH;pTarget.rot:=pPlaceBase90.rot;pTarget.robconf:=pPlaceBase90.robconf;pTarget:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,CompensationnCount,3); CASE 14

44、: pTarget.trans.x:=pPlaceBase90.trans.x+nBoxW; pTarget.trans.y:=pPlaceBase90.trans.y; pTarget.trans.z:=pPlaceBase90.trans.z+2*nBoxH;pTarget.rot:=pPlaceBase90.rot;pTarget.robconf:=pPlaceBase90.robconf;pTarget:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,CompensationnCount,3); CASE 15: pTar

45、get.trans.x:=pPlaceBase90.trans.x+2*nBoxW; pTarget.trans.y:=pPlaceBase90.trans.y; pTarget.trans.z:=pPlaceBase90.trans.z+2*nBoxH;pTarget.rot:=pPlaceBase90.rot;pTarget.robconf:=pPlaceBase90.robconf;pTarget:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,CompensationnCount,3); DEFAULT: TPErase;

46、 TPWrite "The data 'nCount' is error,please check it !" stop;！若當前nCount數(shù)值均非所列CASE中的數(shù)值，則視為計數(shù)出錯，寫屏顯示信息，并停止程序運行ENDTESTReturn pTarget;！計算出放置位置后，將此位置數(shù)據(jù)返回，在其他程序中調用此功能后則算出當前所需的擺放位置數(shù)據(jù)ENDFUNCPROC rPlaceRD()！碼垛計數(shù)程序TEST nPalletNo！利用TEST判斷執(zhí)行哪側碼垛計數(shù)CASE 1:！若為1，則執(zhí)行左側碼垛計數(shù) Incr nCount_L;！左側計數(shù)nCount

47、_L加1，其等同于：nCount_L：=nCount_L+1 IF nCount_L>15 THEN Set do02_PalletFull_L; bPalletFull_L:=TRUE;nCount_L:=1; ENDIF！判斷左側碼盤是否已滿載，本案例中碼盤上面只擺放15個產(chǎn)品，則當計數(shù)數(shù)值大于15，則視為滿載，輸出左側碼盤滿載信號，將左側滿載布爾量置為TRUE，并復位計數(shù)數(shù)據(jù)nCount_LCASE 2:！若為2，則執(zhí)行右側碼垛計數(shù)Incr nCount_R;！右側計數(shù)nCount_R加1IF nCount_R>15 THENSet do03_PalletFull_R; bP

48、alletFull_R:=TRUE;nCount_R:=1; ENDIF！判斷右側碼盤是否已滿載，本案例中碼盤上面只擺放15個產(chǎn)品，則當計數(shù)數(shù)值大于15，則視為滿載，輸出右側碼盤滿載信號，將右側滿載布爾量置為TRUE，并復位計數(shù)數(shù)據(jù)nCount_RDEFAULT: TPERASE; TPWRITE "The data 'nPalletNo' is error,please check it!" Stop;！數(shù)據(jù)nPalletNo 數(shù)值出錯處理，提示操作員檢查并停止運行ENDTESTENDPROCPROC rCheckHomePos()！檢測機器人是否在Hom

49、e點程序VAR robtarget pActualPos;！定義一個目標點數(shù)據(jù)pActualPos IF NOT CurrentPos(pHome,tGripper)THEN ！調用功能程序CurrentPos。此為一個布爾量型的功能程序，括號里面的參數(shù)分別指的是所要比較的目標點以及使用的工具數(shù)據(jù)。這里寫入的是Phome,是將當前機器人位置與Phome點進行比較，若在Phome點，則此布爾量為True；若不在Phome點，則為False。在此功能程序的前面加上一個NOT，則表示當機器人不在Home點時才會執(zhí)行IF判斷中機器人返回Home點的動作指令 pActualpos:=CRobT(Tool

50、:=tGripperWObj:=wobj0);！利用CRobT功能讀取當前機器人目標位置并賦值給目標點數(shù)據(jù)pActualpos pActualpos.trans.z:=pHome.trans.z;！將pHome點的Z值賦給pActualpos 點的Z值 MoveL pActualpos,v500,z10,tGripper;！移至已被賦值后的pActualpos 點 MoveJ pHome,v1000,fine,tGripper; ！移至pHome點，上述指令的目的是需要先將機器人提升至與pHome點一樣的高度，之后再平移至pHome點，這樣可以簡單地規(guī)劃一條安全回Home點的軌跡 ENDIFE

51、NDPROCFUNC bool CurrentPos(robtarget ComparePos,INOUT tooldata TCP)！檢測目標點功能程序，帶有兩個參數(shù)，比較目標點和所使用的工具數(shù)據(jù)！比較機器人當前位置是否在給定目標點偏差范圍之內VAR num Counter:=0;！定義數(shù)字型數(shù)據(jù)Counter VAR robtarget ActualPos;！定義目標點數(shù)據(jù)ActualPos ActualPos:=CRobT(Tool:=TCPWObj:=wobj0);！利用CRobT功能讀取當前機器人目標位置并賦值給ActualPos 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&