ABB机器人码垛程序.doc

MODULE MnMouePROMain(）PErase;TPRadNum nCount1， Qing Sh Ru i aBaoShu!；TPRedum nJob， QShuRuMo Shi:50K:=1,5KG:!;Iill；WHIL TRUE DORedType;ck2re;Pick;loc2re;PlceOF；ENDWHILEENDPOCPRC lock2e(）！epnt:=movtat； F DI0_1Prod0K 0 or DI1_2TuoPan = 0 ThenIF temint0、25 thenClktat C2;ELSlkStop LK2；ENDIFlskStop CK2;ndIFEpocROC IniAll()eHome;Ret O101Jiua;Rest D10_2aBan;eset O10_3ZhK;Rese DO0_4MaO；Wittie0、;bPickPat：=FLSE;lkRest CK1;ClkStart CK1;lRese CLK2;oe Of（Pick，0,0,40), vFast, z10, tGripeWObj:wobj0；NDROPRC PickIF(）IF bPickr = FAS ND nJo 0 NDDI10_Produt0K= TEN!Calclatick;ovJOffs(Pick,0,400), vat, 200,tGripeOj:woj0;!oveLDOfs(pPik,0,0,100), s, 0, ol0Wbj:=woj0, D10_1Jahu， 1;veL pPick, vFast，fi, triprOb:=wobj0;lo1;GrpLod Loadull；Acset0，50；adsakIF;Pickat:=TRUE;ConLff；oveJ pfterik, vas,z60, tGrierbj:=wbj0;EIENDPROCPOCPlaceOF(）F I1_2TuonOK = 1 N bPicPr = TRUETHECalculateace;ConfLf;！MoveL fs（pOver,0,0，OffsZ), vFast， z00, tGripper;IF bTagtue thencset70, 0;MovJ pAbovePlae， vst, z10, tGripprWObj:=wob0;eseAccset 90,0; MoJ AbovePace， st, z10, Griprbj:=0;EndAcc 10,10;！lace frst boConLff;!MoveL fs(pPlace1，nOfs1,OfsY1,fsZ1），vTurn,z10,tGrpperWOj:=wj0；ve pPlace1， Middle,fine， irWObj:=wobj;Open1;ofLff；Mve f（Pce1,0,0,nOfsZ1),vMde，z，tripperWObj:=bj0;GripLoa LoadEmpty;cs 100，10；bckPart：=FALS;Ir nCount1；Hdshae;MovJ AbovePk, vFat， z200， tGriprj:=ob;M Os(pik,0，0，40), vast,z0, tGripeObj:=wb0;！oveJ pome, Fas, z20, trippe;DIFDPOCPCHanhkeIF(）IF nJb1 THN！Set O10_3ZhaOK;NDIFI Jo2 THN！Se O10_3ZhuaOK;ENDIFDPROPO HandshaeOF()IF (nCo=4 and ob =1） or （Cou180 an nJb =2） HE10_4MaDoK;WaitDID0_2oanOK, ；etDOSDelay:=0、5,D10MauO,0;ot:=;lkStop CLK1;CkSp CK2;PEras；CyleCont:ClkRead（L1)；Witingtim：=ClkRd(CK2)；Prite ce time： Nm:=nCcCoun;TPritaiig ti in his Cyl： Num:=Witgtie;Cleet K;ClkRset CK1;Cktart CLK;NDFEDPROCPCovHome()emp1:=CRobT(To：=ol0 b:=j0);empP、trans、z：=500; MoveJ Tmp1, v100， z20, tGrp;MeJ pHome,10, 2, triper；ENDPROPROC aType()IF nPrority =1 AND D_1rduct0K =1 THE!nJob:=;! Incr Priority;LEIF nPriority 2 AND D01oduct0K= THEN!nJob：=2;!cr nPriority;ELSE!nob:=0；!Inr nPrioiy;!F niorit2 Prioity:=1;NDIFENDPROCPROpe1(）R DO0_2Yaan;Wtme 0、0;set DO1_1JiZh;waittim 0、；!Witte nTimeriper;NDPRCROC Cls1(）Se D10_1JiaZ;Waittime 0、0；aitD I14ha1End， 1；Set D12Yan；Watim 、0;!atim nTimeGripp;WtDI I1_4Zha1d, ;！WitDI DI10_6Zhua2En， 1;NDPROCPROC Cacatck（）IF nJo = THEpPick:=Pick;EEF nJob THpPick:=pPick;NDIF！pAboePck:pPick;!Afterck:ick;！Ptr80;!povPick、tran、z:pAfrPck、trs、z;!pAvPck、tns、：= pAferck、ans、y-50;！pAtePic、tns、y：= AfterPik、rans、y-500;NPCPROCCalclatePlace()I ob =1 THEnBgL:=nBag5L;nBaW：Bag50W；nag:=Ba50;Pattr8;ELSEIF nJb = THENnagL:=nBag25L;nBagW：nBag25W;nBaH:nB25H;Ptrn100;EDF！Aovec：=Ple1；pAftePlace：=pAbvePace；NDPROCROC Pattrn80()bTag:FALSE;TESnCout1+1AE 1:pPlae1:pBae10;pPe、ans、x:plce1、rans、x；pPlc1、tras、y:=pPlce1、trans、;pPce、an、z:a1、rans、z；nffsX1:=；nOffs:=；OfsZ1:500;nOfZ2:=-5;Lvel：=1;bTwice:=FALSE;pfteick、trn、z：pPik、ras、400;AboPlace：=plac1;pAvePac、trans、z:=pPck、trans、z+4;bTa:Tre;CAS2:plce1:=pase；pace1、trans、x:=pPlace1、tra、x;Pce1、rns、y：=pPac1、tran、y；pPla、tn、z:pace1、rans、;nOfsX1:=0;nOffsY1：=;ffsZ:=nBaH*、2;OfsZ2：=;nLeve:=1；bTice:=LSE;pAftePick、ans、：=pPick、trns、z400;pAboePae：=Ple1；pAovPlac、trans、:ick、trans、z00;ASE3:lace1:=Bse90;pPlace1、tns、x:=pPace1、ts、x；pPla、tras、y：=plae、tras、y+5;pPlace、trans、:=pc1、trans、;OfX1:=0；nsY1：=0;nOffs1:nBagH、2;nOfsZ2:-50;nLeve:=1;bTwce：=FLSE;fePck、rn、:=Pick、trans、z+400;bovelace:=pPlace1;pAbovPlace、rs、z：ic、tran、z+400;CASE4:lace1：=pase90；Pla1、trans、:=pPlc1、trans、x+2nBg;pPlac1、tns、：=pe1、trans、y2;place1、tra、z:=pPlce1、tran、z;nOfsX1:0;OfsY1:=0;nOffsZ1:=aH*1、2;nOfsZ2:-500；Lvel:=1；bTice：=FASE;pAtPik、rans、：=Pik、ran、z+400；pAbovePlace:pPlac1;pAbovelace、trs、z:=pPick、trns、z+400;CASE 5:lac1:=a0;pPlace、tns、x：=Plc、n、xnaW;pPlace1、trans、y:=pPace1、trans、y+25;pPlace1、trns、z：=pPlace、trans、+2；nOffsX:=0；nOs1:=;nOffsZ：nBaH、2;Offs2:=-50;nevel:=1;bTwice:=ALSE;pfterPic、tra、z:=pick、rans、z400;pbvelace:=pPlac1；pbovelace、trans、z:pPick、s、z+40;!Ler2 CS：pPlace：=as180；pPlc1、trans、x:=Plc1、ran、x;plce1、trans、y:=pPlae1、rns、+nBag-5;pPace1、ans、z:=pPlae1、tas、+nBagH;nffsX1:=0；nOfsY：=0;nOffsZ:=nagH、2;nOffs2:-00;Level:=1;wie:=FALSE;pAterPik、tas、z:pPik、tns、z+400;pAbovece：=pPlac1;pAovela、tas、z:=ick、trans、z+400;CASE :pPlace:=pe0;pPlac1、trns、x：Pac1、rans、x;place、trans、y:=plae1、trs、nBgL5;Place1、trans、z：=pPlae1、tans、+BagH;nffsX1:0；OfsY：;OfZ1:=naH*1、2;OffsZ2:=50;nLvl:=;Twic:=FALSE;AfterPic、trans、z:=Pick、tas、+40；pAbovPae:pPae1;pbovePlce、rns、z:=pPick、ras、z+400；CASE 8:pPlace1:=Base20;Pace、trns、x：=lace、trans、x;Plae1、trs、y:pPlace1、rans、y；lae1、trans、z:=pPlace1、trns、z;nOffX1:=；Offs1：0;OfsZ：=nBaH*1、2；nffZ2:=-50;Lve：=1;bTc:=ALE;pftric、tras、z:=pPick、ts、z+400;Aboveace：=place;pAboePae、tans、z：=Pik、tans、+400；CS 9:Place1:pBae270；lace1、rans、x:=pPlace1、trans、+2*nagW25；pace1、ts、y:=pace1、trn、y;plac1、rans、:=pPlce、tran、z;nOfsX1:=0;OY1：0;nOffZ1：=nBagH*、2;nff2:=-500;Leel：2;bTice:=FALE；pAfterPick、tns、z:=Pick、tran、z+00;pAbovePlae:=pPlce1；pvePl、tran、z:=pPick、tran、z+400;CAE 10：plac1：=pBae70;Plac1、tas、x:lace1、tras、xnBgW;plc1、trans、y:=plae1、tans、y;Place1、rns、z:=pPl1、trs、z;nOffsX1:=;nOffsY1:=0；nOffs1:=Ba*1、;nOffsZ2：=5;nLevel:=；bTwe:=FLSE;fterPick、ra、z:pPck、rans、z+00;pAboece:=pace1;pAvelac、trans、z:=pik、rans、z400；！ayerCAS 1：ace1:=pBas180;Pae1、tran、x：=place1、tans、x;Plc1、tans、y:=pPlace1、trans、；Pla、tr、z：=Place1、trans、z+2*nBagH；nOfs1:=0;fsY:=0;nfs1:nBaH*1、2；Off2:=-500;nLve：=1;bwice:=ALSE;pAtrPick、ran、z:pPck、trans、z+400;bovePlace:=pPae1;pAbovePace、rns、z:pPick、tn、400；bTag:=rue;E ：pac1：pas;pPlce1、trans、x：=Plac1、tra、x；pPlc1、trans、y:=Place1、trans、y;plac1、trans、z:=pPlace、trans、z+2*nBagH；nOfsX1:=0；nOffsY：=0；nOffsZ：=nBH*1、2；nOffsZ:=-500;nLeve:=1;wice：=FASE;AftPick、ran、z：=Pick、tran、z+4;pAbvePlce:plc1；povePlace、trns、z:ik、tras、z00;CASE 13：pPce1：=pBa;pPlace1、tans、x：=pPlace1、trans、x;pPlace1、ta、:=pPlace、trans、y25;plce、tn、z:pPlace、trns、z+2BagH;n:0;nOfsY1:=;nOffZ1:=a1、;nfsZ2:=-50；nLel:=1;bTwic：FASE;pAftrPik、an、z:=pick、tras、z400;pAbvePlce:=pPlace;oePlace、rans、z:=pPck、trans、z+40;SE:pace:=pBase90；pPlae1、tran、x：=pce1、tran、x+*agW;Pace1、tras、y：=pPlace1、tr、y+25;Pac1、rn、：=plae1、trans、+2*nag;nOffs1:=0;OfY1：=0;O1:BaH*1、;nOffsZ2:=-50;nLevl：=1;bTwice:FLSE；pAfePk、trans、:=pick、tans、400;pAbovPlce:=pPlace；pAoPlc、tran、z：=pPick、trns、z+400;ASE15：pPace:=pBase90;plae1、tran、x:=pPlc1、trans、x+nBagW;pPlc1、tas、y:=pPlce1、trans、y+;plce1、ra、z:=pPlce1、trns、2nBagH;nOf1：0；nOffs1：=0;nOfsZ1:nBagH*1、2;OfsZ2:=-00;nLeel:=;bTwic:FALS;pAfteik、ras、z:=pPick、ras、z+00；pAbovePle:pPace1；pAbovePlace、tra、z:=pPk、trans、+400；！ayer CAE 6：pPace1:=pase180;Place1、tras、x：pPace1、rs、x；lace1、rans、：=place1、tans、+nBagL5；pPae、tra、z:=pPace1、rs、+3*BagH+80；Ofs1:=0;nffsY1:=;nOff1：=agH*1、2;nOfsZ2:=0;nLvel:=1;Twi:=FLSE;Afteri、trs、z:=Pik、trans、z+40;pAbovPlac：=pPlace1;pAovelce、tans、z：pick、tns、z+0;CSE 1:plae1:=pBase0；pPace、tran、x:=place1、trn、x;pPlc、tans、:=pPace1、n、ynBagL-；l1、tn、z:=pPce1、rs、z+3*gH;nffsX1:;nOfsY1:=0;nffs:=nBagH*1、;nfsZ2:=300;nLvl:；bTwce:=FAL;Aterick、trns、:pPck、rans、z+40;pbovePlace：pPlae1；pAbvePlce、trans、z：=pPic、ans、z+40;CASE 18:pPlae:=pBase20;Plac、tans、x:=Pe1、trans、x;plae、rans、y:=pPlace1、tns、;pPla、tra、z:=pPla、trans、z+BagH;nOffsX1:=0;nOffsY1:=0;nOs1:nBagH1、;nOffsZ：30;neve:1；Twie:FAL；Afterik、ts、z:=pPick、ta、z400；AbovePl:=pPlace1;pbPace、tra、z:=pPick、tan、z+400;CAE 19：plac1:=pa20;Place1、ts、x:pPce1、ts、x+nBgW-25;pPce1、rans、y:=pPlace1、tan、;pPc1、trans、z:=pPlace1、tran、+2agH;nff:=0;nOffsY1：;nffsZ1：=nBaH1、2;nOffs2:-30;nLvl:=;bTwice:ALE;AferPck、trans、:=pPik、ras、z+400;Aovelce:=pac1;pAbovelce、rans、:=pPick、trans、z+00；ASE 2:le1:=pBase2；pPlace1、trans、x:=pPlace1、trans、x+nBaW;place1、trans、y:=pPlae1、rans、;pPlace、trns、z:plce1、tran、z+agH;nOfsX：=0；nOffsY1：=0;nOffsZ1:BagH*、;OfsZ:=300；neel：2;bTwice：=FALSE;ferPk、trans、z:pPick、trns、+400;pAbvePae:pPlce1；pAboePlce、an、z:=pck、trans、z400；!laer5ASE 21：place1:=pBase180；pPae1、trans、x:=Place1、trn、;pPace1、trns、y：=pPlace1、tran、;pPlace、tras、z:=pPlace1、trans、z+4BagH;nffsX:=0;Ofs1:=;OffsZ1:=Bag*1、2;nOffsZ2:=00;Level:=1；bTwice：=ALSE；AferPck、rans、z:=pPic、trns、z+500；pAbovPac：=pPlace;pAbovePlce、tras、z:=pik、ta、z+50；Tag:=Tue;CASE 2:pPlace:=pBase0；Pac1、trans、x:=Place、trns、x；pPlac1、r、y:=pPlace1、tas、y;place、n、z：pPlac1、tan、znBH；nfsX1：=0;nffY:=0；nOfsZ1:=ngH1、2;nOffsZ:=-30;Lvel:=;bTwce:=FLSE;terPk、trans、z:=p、trans、+50；pAbovlace:plce;AbvePace、ra、z:pPi、trns、z+00；CASE 23:pla:=pBase0;pPace、tras、x:=Plac1、trans、x;pP、rans、y:=pPace1、rans、y+25;pPace1、trans、z:Place1、ran、z+*agH;Offs1:=0;nOffsY1:0;nfs:=Bag1、2;nfsZ2:=-0;nLeve:=1；bTice：=FLSE；pAfterPick、trs、:=pik、trans、+00;pAbovePlae：=Place1;pbovePlac、ts、z：=pPc、tan、z+500;CAE 2:pPlace1:=se90；place1、trans、:lae、tans、x+nBagW;pPlace1、tran、y:=place1、trans、y+25；pPle、tns、z：=Pae、trs、z4*nBagH;nOffsX：=0;nOsY:=;nffsZ:=nBgH*1、2;nOffZ:=300;nLevel:=1;bTwce:=FALSE;AtePick、trns、：=pick、rans、z0；boePae:=place1;povPlce、trans、z:=pick、tans、z+500;AE 5:pPlce1：=ase90；pPlace1、rns、x：=pPlac1、ta、xng;pPlae、tran、y：Pc、trans、y+25;plac、tans、z:=pPle1、tra、z+4*nBag;nOff1:0;nOffsY1:0;OffZ1:=BagH、；nffsZ:=-300;nLeve：=1;bTwice:=ALSE;pAftPick、tans、z：pPic、tran、z+500;AovePlace:=pPlc;pAovePac、ans、：pPi、tran、z+500；!laye6CAE 26:pPace1:=as180；pPlace1、tras、x:=pPlace1、tans、x;pla1、rn、y:=pPace、rans、y+nBag-;pPle1、tras、z:=Place1、tran、z+*nBag-5；nOfsX1:=0;nfsY1:=；nOfsZ1:=Bg*1、2;nOffs2:=-100;Level:=1;Twice：=FAE；pAterPck、tan、z:=Pck、trans、+450+nBH;AbovePla:=plac1;pAbovPla、trans、z:=k、trns、z450+nagH；CASE 7:Pl:=pBa;plac1、trans、x：=plac1、tras、;pPlce1、ns、:pPlac1、tas、yBagL-5;Place、tra、z:pPlace、trans、z+ngH-50；nOs:0;nfsY1:=0;nOffsZ1:=nBagH*1、;nOZ2:=-00;nevel:;bwice:FALSE;pAftP、tran、z:=pPick、tns、z+50nBag;pbvePlac:pPlace1;pboelac、tran、:pPck、trans、+nBaH;CA28:Plce:pBae270；pPlac1、tas、x:=pPlac、rans、x；pPae1、trans、：=pPlace、trans、;pPlace、tran、z:=place、rs、z4*nBa-4;nOffX1:=0;nOffsY:=0;nOfZ:=BagH*、2;nffsZ:=100;nLevel:=;bTe:=FLSE;Afeick、rans、:pPc、tans、z+5+ngH;pbovePac：=pPlce；AboePla、tran、：=pck、trns、z+450+Bag；ASE 29：place:=pBase2；pPlace1、tans、x:=e1、tran、x*nag5;place1、ans、y：=pPae1、tr、y;lace1、rans、z：=Place、tra、z+4*nag-45;nOfsX1：=0;nOfs1：=;nOfsZ1:=BgH*1、;nOs2：=-100;nLevel:=;bTwie:=FALSE;Aterick、tan、z:pPick、tas、z+50+nag；AbovePlace：=pce；pbovPce、rans、z：=pPick、tras、z+50+nBa;ASE 0:plce1:=Bas20;plce1、trns、x:=Pae1、trans、x+naW；Plac1、tans、：=pPae1、trns、y;Pace、trns、z:=pPlce1、tras、+*nBH-5;OffX1：=0；nOffsY1:=0;nffsZ1:=naH*1、;nOffs：=-100;nLevl:=2；bTwic：=ALE;AftrPc、an、z:p、ans、+50+agH;pAoePlae:Pce;AbvePlce、tran、z:=pPc、trans、z+500+ngH;!layer7CASE 31：pPe1:Base10;plae、tra、x：=pPlace、trans、x;pPlace1、tns、y:=plce、rans、y;pPlace1、trans、：pPlace1、tan、+6nBa;nffs1:=0;nsY：=;nfsZ1:nagH*1、2;nOffsZ2：-100;nee:1;bTwice:=FALSE；pAferPick、tras、z:=pPik、as、z+40+2nBgH;pAoePlace:=ae1;pAbvePlae、tr、z:=pPick、rans、z+502naH;bTag:Te;CAE :place1:=pBse0;lace1、tans、x:=pPlae1、rns、x；Plac1、tans、:pPace1、tans、y;pPlce1、trn、：=c1、ans、z*nBagH-40;nOfX1：0;nOffY1:=0;nOfsZ1:=nBag1、2；nfsZ2:00;evel:=1;Twie:FALSE;pAfterPik、as、z：pPic、rans、z+450+*ngH;AbePla