ABB左右碼垛詳細(xì)解說(shuō)

1、MODULE MainMoudlePERSwobjdataWobjPallet_L:=FALSE,TRUE,"",-456.216,-2058.49,-233.373,1,0,0,0,0,0,0, 1,0,0,0 ！定義左側(cè)碼盤(pán)工件坐標(biāo)系 WobjPallet_LPERSwobjdataWobjPallet_R:=FALSE,TRUE,"",-421.764,1102.39,-233.373,1,0,0,0,0,0,0,1 ,0,0,0;！定義右側(cè)碼盤(pán)工件坐標(biāo)系 WobjPallet_RPERStooldatatGripper:=TRUE,0,0,527

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

3、E+09,9E+09,9E+09; ！定義關(guān)節(jié)目標(biāo)點(diǎn)數(shù)據(jù)，各關(guān)節(jié)軸數(shù)值為 0 ，用于手動(dòng)將機(jī)器人運(yùn)動(dòng)至各關(guān)節(jié)軸機(jī)械零位CONSTrobtargetpPlaceBase0_L:=296.473529255,212.21064316,3.210904169,0,0.70711295,-0.707100 612,0,-2,0,-3,0,9E9,9E9,9E9,9E9,9E9,9E9; ！左側(cè)不旋轉(zhuǎn)放置基準(zhǔn)位置CONSTrobtargetpPlaceBase90_L:=218.407102669,695.953395421,3.210997808,0,-0.000001669,1,0, -2,0,-2

4、,0,9E9,9E9,9E9,9E9,9E9,9E9; ！左側(cè)旋轉(zhuǎn)90°放置基準(zhǔn)位置CONSTrobtargetpPlaceBase0_R:=296.473529255,212.21064316,3.210904169,0,0.707221603,-0.70699 194,0,1,0,0,0,9E9,9E9,9E9,9E9,9E9,9E9; ！右側(cè)旋轉(zhuǎn)不旋轉(zhuǎn)放置基準(zhǔn)位置CONSTrobtargetpPlaceBase90_R:=218.407102669,695.953395421,3.210997808,0,-0.00038594,0.9999 99926,0,1,0,1,0,9E

5、9,9E9,9E9,9E9,9E9,9E9; ！右側(cè)旋轉(zhuǎn)90°放置基準(zhǔn)位置CONSTrobtargetpPick_L:=1627.550991372,-426.974661352,-26.736921885,0,0.707109873,-0.707103 689,0,-1,0,-2,0,9E9,9E9,9E9,9E9,9E9,9E9; ！左側(cè)抓取位置CONSTrobtargetpPick_R:=1611.055992534,442.364097921,-26.736584068,0,0.707220363,-0.7069931 81,0,0,0,-1,0,9E9,9E9,9E9,9E

6、9,9E9,9E9;!右側(cè)抓取位置robtargetCONST 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;!程序起始點(diǎn)，即 Home嵐PERS robtarget pPlaceBase0;PERS robtarget pPlaceBase90;PERS robtarget pPick;PERS robtarget pPlace;！定義目標(biāo)點(diǎn)數(shù)據(jù)，這些數(shù)據(jù)是機(jī)器人當(dāng)前使用的目標(biāo)點(diǎn)。當(dāng)在左側(cè)、右側(cè)碼垛時(shí)，將

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

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

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

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

11、00;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ù)，分別對(duì)應(yīng)空載時(shí)高、中、低速，以及滿載時(shí)的高、中、低速，便于對(duì)機(jī)器人的各個(gè)動(dòng)作進(jìn)行速度控制PERSnumCompensation15

12、,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ù)組，用于各擺放位置的偏差調(diào)整； 15 組數(shù)據(jù)，對(duì)應(yīng)15 個(gè)擺放位置，每組數(shù)據(jù)3個(gè)數(shù)值，對(duì)應(yīng)X、 Y、 Z 的偏差值PROC main()！主程序rInitAll;！調(diào)用初始化程序，包括復(fù)位信號(hào)、復(fù)位程序數(shù)據(jù)、初始化中斷等WHILE TRUE DO!利用WHILE循環(huán)，將初始化程序隔離開(kāi)，即只在第一次運(yùn)行時(shí)需要執(zhí)行一次初始化程 序，之后循環(huán)執(zhí)行拾取放置動(dòng)作IF bReady THEN!利用IF條

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

14、再執(zhí)行后面的指令ConfLOFF;ConfJOFF;！關(guān)閉軸配置監(jiān)控nCount_L:=1;nCount_R:=1;！初始化左右兩側(cè)碼垛計(jì)數(shù)數(shù)據(jù)nPallet:=1;！初始化兩側(cè)碼垛任務(wù)標(biāo)識(shí)， 1 為左側(cè)， 2 為右側(cè)nPalletNo:=1;！初始化兩側(cè)碼垛計(jì)數(shù)累計(jì)標(biāo)識(shí)， 1 為左側(cè)， 2 為右側(cè)bPalletFull_L:=FALSE;bPalletFull_R:=FALSE;！初始化左右兩側(cè)碼垛滿載布爾量bGetPosition:=FALSE;!初始化計(jì)算位置標(biāo)識(shí)，F(xiàn)ALSE為未完成計(jì)算，TRU日已完成計(jì)算Reset do00_ClampAct;Reset do01_HookAct;！初

15、始化夾具，夾板張開(kāi)和鉤爪松開(kāi)ClkStop Timer1;！停止時(shí)鐘計(jì)時(shí)ClkReset Timer1;！復(fù)位時(shí)鐘TriggEquip HookAct,100,0.1DOp:=do01_HookAct,1;!定義觸發(fā)事件：鉤爪收緊。朝向指定目標(biāo)點(diǎn)運(yùn)動(dòng)時(shí)提前100mm攵緊鉤爪，即將產(chǎn)品鉤住，提前動(dòng)作時(shí)間為 0.1 秒TriggEquip HookOff,100Start,0.1DOp:=do01_HookAct,0;!定義觸發(fā)事件：鉤爪松開(kāi)。距離之后加上可選參變量Start ,則表示在離開(kāi)起點(diǎn)100mn#松開(kāi)鉤爪，提前動(dòng)作時(shí)間為 0.1 秒IDelete iPallet_L;CONNECT iP

16、allet_L WITH tEjectPallet_L;ISignalDI di02_PalletInPos_L,0,iPallet_L;！中斷初始化，當(dāng)左側(cè)滿載碼盤(pán)到位信號(hào)為 0 時(shí)，即表示滿載碼盤(pán)被取走，則觸發(fā)中斷程序 iPallet_L, 復(fù)位左側(cè)滿載信號(hào)、滿載布爾量IDelete iPallet_R;CONNECT iPallet_R WITH tEjectPallet_R;ISignalDI di03_PalletInPos_R,0,iPallet_R;！中斷初始化，當(dāng)右側(cè)滿載碼盤(pán)單位信號(hào)變?yōu)?0 時(shí)，即表示滿載碼盤(pán)被取走，則觸發(fā)中斷程序 iPallet_R ，復(fù)位右側(cè)滿載信號(hào)、滿載

17、布爾量等ENDPROCPROC rPick()！抓取程序ClkReset Timer1;！復(fù)位時(shí)鐘ClkStart Timer1;！開(kāi)始計(jì)時(shí) rCalPosition;！計(jì)算位置，包括抓取位置、抓取安全位置、放置位置等MoveJ Offs(pPick,0,0,nPickH),vMaxEmpty,z50,tGripperWObj:=wobj0;！利用 MoveJ 移動(dòng)至抓取位置正上方MoveL pPick,vMinLoad,fine,tGripperWObj:=wobj0;!利用MoveL移動(dòng)至抓取位置Set do00_ClampAct;！置位夾板信號(hào)，將夾板收緊，夾取產(chǎn)品Waittime 0.

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

19、MoveL移動(dòng)至抓取位置ENDPROCPROC rPlace()！放置程序MoveJ Offs(pPlace,0,0,nPlaceH),vMaxLoad,z50,tGripperWObj:=CurWobj;！利用 MoveJ 移動(dòng)至放置位置正上方TriggL pPlace,vMinLoad,HookOff,fine,tGripperWObj:=CurWobj;！ 利用 TriggL 移動(dòng)至放置位置， 并調(diào)用觸發(fā)事件HookOff, 即在離開(kāi)放置位置正上方點(diǎn)為 100mm后將鉤爪放開(kāi)Reset do00_ClampAct;！復(fù)位夾板信號(hào)，夾板松開(kāi)，放下產(chǎn)品Waittime 0.3;！預(yù)留夾具動(dòng)作

20、時(shí)間，以保證夾具已將產(chǎn)品完全放下，等待時(shí)間根據(jù)實(shí)際情況調(diào)整其大小GripLoad Load0;！加載載荷數(shù)據(jù)Load0MoveL Offs(pPlace,0,0,nPlaceH),vMinEmpty,z50,tGripperWObj:=CurWobj;!利用MoveL移動(dòng)至放置位置正上方 rPlaceRD;！調(diào)用放置計(jì)數(shù)程序，其中會(huì)執(zhí)行計(jì)數(shù)加 1 操作，并判斷當(dāng)前碼盤(pán)是否已滿載MoveJ pPickSafe,vMaxEmpty,z50,tGripperWObj:=wobj0;！利用 MoveJ 移動(dòng)至抓取安全位置，以等待執(zhí)行下一次循環(huán) ClkStop Timer1;停止計(jì)時(shí)nCycleTime

21、:=ClkRead(Timer1);讀取時(shí)鐘數(shù)值，并賦值給nCycletimeENDPROCPROC rCycleCheck()！周期循環(huán)檢查T(mén)PErase;TPWrite "The Robot is running!"！示教器清屏，并顯示當(dāng)前機(jī)器人運(yùn)行狀態(tài)TPWrite "Last cycle time is : "Num:=nCycleTime;！顯示上次循環(huán)運(yùn)行時(shí)間TPWrite "The number of the Boxes in the Left pallet is:"Num:=nCount_L-1;TPWrite &qu

22、ot;The number of the Boxes in the Right pallet is:"Num:=nCount_R-1;！顯示當(dāng)前左右碼盤(pán)上面已擺放產(chǎn)品個(gè)數(shù)。由于nCount_L 和 nCount_R 表示的是下輪循環(huán)將要擺放的第多少個(gè)產(chǎn)品，此處顯示的是碼盤(pán)上已擺放的產(chǎn)品數(shù)，所以在當(dāng)前計(jì)數(shù)數(shù)值上面減 去1IF (bPalletFull_L=FALSE AND di02_PalletInPos_L=1 AND di00_BoxInPos_L=1) OR (bPalletFull_R=FALSE AND di03_PalletInPos_R=1 AND di01_BoxIn

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

24、標(biāo)識(shí)的數(shù)值，1為左側(cè)，2為右側(cè)！若為 1，則執(zhí)行左側(cè)檢測(cè)IF bPalletFull_L=FALSE AND di02_PalletInPos_L=1 AND di00_BoxInPos_L=1 THEN！判斷左側(cè)是否滿足碼垛條件，若條件滿足則將左側(cè)的基準(zhǔn)位置數(shù)值賦值給當(dāng)前執(zhí)行位置數(shù)據(jù)pPick:=pPick_L;！將左側(cè)抓取目標(biāo)點(diǎn)數(shù)據(jù)賦值給當(dāng)前抓取目標(biāo)點(diǎn)pPlaceBase0:=pPlaceBase0_L;pPlaceBase90:=pPlaceBase90_L;！將左側(cè)放置位置基準(zhǔn)目標(biāo)點(diǎn)數(shù)據(jù)賦值給當(dāng)前放置位置基準(zhǔn)點(diǎn)CurWobj:=WobjPallet_L;！將左側(cè)碼盤(pán)工件坐標(biāo)系數(shù)據(jù)賦值給

25、當(dāng)前工件坐標(biāo)系pPlace:=pPattern(nCount_L);！調(diào)用計(jì)算放置位置功能程序，同時(shí)寫(xiě)入左側(cè)計(jì)數(shù)參數(shù)，從而計(jì)算出當(dāng)前需要擺放的位置數(shù)據(jù)，并賦值給當(dāng)前放置目標(biāo)點(diǎn)bGetPosition:=TRUE;！已完成計(jì)算位置，則將完成計(jì)算位置標(biāo)識(shí)為 TUREnPalletNo:=1;！將碼垛計(jì)數(shù)標(biāo)識(shí)置為1，則后續(xù)會(huì)執(zhí)行左側(cè)碼垛計(jì)算累計(jì)ELSEbGetPosition:=FALSE;WHILE!若左側(cè)不滿足碼垛任務(wù)，則完成計(jì)算位置位置標(biāo)識(shí)置為FLASE則程序會(huì)再次執(zhí)行循環(huán)ENDIFnPallet:=2;!將碼垛檢測(cè)標(biāo)識(shí)置為2,則下次執(zhí)行 WHILE循環(huán)時(shí)檢測(cè)右側(cè)是否滿足碼垛條件CASE 2:

26、！若為2，則執(zhí)行右側(cè)檢測(cè)IF bPalletFull_R=FALSE AND di03_PalletInPos_R=1 AND di01_BoxInPos_R=1 THEN！判斷右側(cè)是否滿足碼垛條件，若條件滿足，則將右側(cè)的基準(zhǔn)位置數(shù)值賦值給當(dāng)前執(zhí)行位置數(shù)據(jù)pPick:=pPick_R;！將右側(cè)抓取目標(biāo)點(diǎn)數(shù)據(jù)賦值給當(dāng)前抓取目標(biāo)點(diǎn)pPlaceBase0:=pPlaceBase0_R;pPlaceBase90:=pPlaceBase90_R;！將右側(cè)放置位置基準(zhǔn)目標(biāo)點(diǎn)數(shù)據(jù)賦值給當(dāng)前放置位置基準(zhǔn)點(diǎn)CurWobj:=WobjPallet_R;！將右側(cè)碼盤(pán)工件坐標(biāo)系數(shù)據(jù)賦值給當(dāng)前工件坐標(biāo)系pPlace:=

27、pPattern(nCount_R);！調(diào)用計(jì)算放置位置功能程序，同時(shí)寫(xiě)入右側(cè)計(jì)數(shù)參數(shù)，從而計(jì)算出當(dāng)前需要擺放的位置數(shù)據(jù)，并賦值給當(dāng)前放置目標(biāo)點(diǎn)bGetPosition:=TRUE;!已完成計(jì)算位置，則將完成計(jì)算位置標(biāo)識(shí)為T(mén)RUEnPalletNo:=2;！將碼垛計(jì)數(shù)標(biāo)識(shí)置為2，則后續(xù)會(huì)執(zhí)行右側(cè)碼垛計(jì)算累計(jì)ELSEbGetPosition:=FALSE;!若右側(cè)不滿足碼垛任務(wù)，則將完成計(jì)算位置標(biāo)識(shí)置為FLASE則程序會(huì)再次執(zhí)行WHILE循環(huán)ENDIFnPallet:=1;!將碼垛檢測(cè)標(biāo)識(shí)置為1,則下次執(zhí)行 WHILE循環(huán)時(shí)檢測(cè)左側(cè)是否滿足碼垛條件DEFAULT:TPERASE;TPWRITE

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

29、側(cè)的擺放位置VAR robtarget pTarget;！定義一個(gè)目標(biāo)點(diǎn)數(shù)據(jù)，用于返回?cái)[放目標(biāo)點(diǎn)數(shù)據(jù)IF nCount>=1 AND nCount<=5 THENpPickSafe:=Offs(pPick,0,0,400);ELSEIF nCount>=6 AND nCount<=10 THENpPickSafe:=Offs(pPick,0,0,600);ELSEIF nCount>=11 AND nCount<=15 THENpPickSafe:=Offs(pPick,0,0,800);ENDIF！利用 IF 判斷當(dāng)前碼垛是第幾層(本案例中每層堆放5 個(gè)產(chǎn)

30、品) ，根據(jù)判斷結(jié)果來(lái)設(shè)置抓取安全位置，以保證機(jī)器人不會(huì)與已碼垛產(chǎn)品發(fā)生碰撞，抓取安全高度設(shè)置由現(xiàn)場(chǎng)實(shí)際情況來(lái)調(diào)整。此案例中的安全位置是以抓取點(diǎn)為基準(zhǔn)偏移出來(lái)的，在實(shí)際中也可單獨(dú)去示教一個(gè)抓取點(diǎn)后的安全目標(biāo)點(diǎn)，同樣也是根據(jù)碼垛層數(shù)的增加而改變?cè)摪踩繕?biāo)點(diǎn)的位置TEST nCount！判定計(jì)數(shù)nCount 的數(shù)值，根據(jù)此數(shù)據(jù)的不同數(shù)值計(jì)算出不同擺放位置的目標(biāo)點(diǎn)數(shù)據(jù)CASE 1:pTarget.trans.x:=pPlaceBase0.trans.x;pTarget.trans.y:=pPlaceBase0.trans.y;pTarget.trans.z:=pPlaceBase0.trans.z;

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

32、擺放第一個(gè)位置時(shí)nCount1, 則 pTarget:=Offs(pTarget,Compensation1,1 , Compensation 1, 2, Compensation 1, 3);如果發(fā)現(xiàn)第一個(gè)擺放位置向 X負(fù) 方向偏了 5mm則只需在程序數(shù)據(jù)數(shù)組 Compensation中將第一組數(shù)中的第一個(gè)數(shù)設(shè)為5,即可對(duì)其 X 方向擺放位置進(jìn)行微調(diào)。CASE 2:pTarget.trans.x:=pPlaceBase0.trans.x+nBoxL;pTarget.trans.y:=pPlaceBase0.trans.y;pTarget.trans.z:=pPlaceBase0.trans.

33、z;pTarget.rot:=pPlaceBase0.rot;pTarget.robconf:=pPlaceBase0.robconf;pTarget:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,Compensa tionnCount,3);CASE 3:pTarget.trans.x:=pPlaceBase90.trans.x;pTarget.trans.y:=pPlaceBase90.trans.y;pTarget.trans.z:=pPlaceBase90.trans.z;pTarget.rot:=pPlaceBase9

34、0.rot;pTarget.robconf:=pPlaceBase90.robconf;pTarget:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,Compensa tionnCount,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.robc

35、onf:=pPlaceBase90.robconf;pTarget:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,Compensa tionnCount,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:=pPlaceBase9

36、0.robconf;pTarget:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,Compensa tionnCount,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

37、:=Offs(pTarget,CompensationnCount,1,CompensationnCount,2,Compensa tionnCount,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(pTar

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

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

40、ount,1,CompensationnCount,2,Compensa tionnCount,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,CompensationnCoun

41、t,1,CompensationnCount,2,Compensa tionnCount,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,CompensationnCou

42、nt,2,Compensa tionnCount,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,Compensa

43、tionnCount,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,Compensa tionnCount,3);C

44、ASE 14: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,Compensa tionnCount,3);CASE 15:pT

45、arget.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,Compensa tionnCount,3);DEFAULT:TPErase;T

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

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

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

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

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

51、DIFENDPROCFUNC bool CurrentPos(robtarget ComparePos,INOUT tooldata TCP)！檢測(cè)目標(biāo)點(diǎn)功能程序，帶有兩個(gè)參數(shù)，比較目標(biāo)點(diǎn)和所使用的工具數(shù)據(jù)！比較機(jī)器人當(dāng)前位置是否在給定目標(biāo)點(diǎn)偏差范圍之內(nèi)VAR num Counter:=0;！定義數(shù)字型數(shù)據(jù)CounterVAR robtarget ActualPos;！定義目標(biāo)點(diǎn)數(shù)據(jù)ActualPosActualPos:=CRobT(Tool:=TCPWObj:=wobj0);!利用CRob礎(chǔ)能讀取當(dāng)前機(jī)器人目標(biāo)位置并賦值給ActualPosIFActualPos.trans.x>ComparePos.trans.x-25ANDActualPos.trans.x<ComparePos.trans.x+25 Counter:=Counter+1;IFActualPos.trans.y>ComparePos.trans.y-25ANDActualPos.trans.y<ComparePos.trans.y+25 Counter:=Counter+1;IFActualPos.trans.z>ComparePos.trans.z-25ANDActualPos.trans.z<ComparePos.