ABB机器人程序实例ROBOTstudio6.01(附带与工业相机网络通讯实例)(完整资料).doc

1、【最新整理，下载后即可编傅】ABB机器人（ROBOT studio6.01）程序实例MODULE MainModulePERS tooldatatGripper:=TRUE, 0.533078,1.51617,583.739, 1,0,0,0, 30, 0,0,50, 1, （）,0,0 ,（）,（）,（）；TASK PERS wobjdataVisionWobj:=FALSE,TRUE,n,0,0,0,1,0,0,0,-934.534,1807.34,-7 6.7707,0.400996,0.0128267,-0.0292473,-0.915523;TASK PERS wobjdataWob

2、 jCompressorl:=FALSE,TRUE；n',518.656,-1088.9,164.25,0,0,0 ,1,686.651,296.298,-588.529,0.917114,1.69419E-06,-7.35001E-05,- 0.398626;TASK PERS wobjdataWobjCompressor2:=FALSE,TRUE；n,518.656,-1088.9,164.25,0,0,0 ,1,-944.871,-657.402,-323.406,0.918098,-1.98999E-05,-6.49686E-06 ,0.396353;PERS wobjdata

3、 WobjComprcssor;VAR rob target pAcmalPos;VAR sockctdcv scncr_sockct;VAR sockctdcv clicnt_sockct;VAR string clicnt_ip;VAR string stRcccivcd;VAR num NumCharactcrs:=9;VAR bool b（）K;PERS num nXOffs;PERS num nYOffs;PERS num nAnglcOffs;VAR string XData:=un;VAR string YData:=,HI;VAR string AnglcData:=nM;VA

4、R num nPrcsenccC )rAbscncc;PERS num nPickH:=-400;PERS num nCountX;PERS num nCountY;PERS num nCountZ;PERS num nCount;VAR num nPlaceNo;PERS bool bSMPreOrAbs;PERS bool blnpos;VAR robtargct PVision;VAR robtargct Vision;VAR robtargct ppPick;VAR robtargct pPick;PERS robtargct Pick;PERS robtargct ErCiDingW

5、eiPlace;PERS robtargct ErCiDingWciPick;PERS robtargct pPlacc;PERS robtargct PlaceVision;PERS robtargct PZhanban;PERS robtargct PZhanbanUp;PERS robtargct PZhanbanDown;PERS robtargct PlaceZhanban;PERS robtargct Place;PERS bool bKindChoose;VAR num nAnglc;VAR num nX;VAR num nCamOut;VAR num nlnpos;VAR nu

6、m nTotalPallctHigh;VAR num nPallctHigh;PERS num nPallctHighUp;PERS num nPallctHighDown;VAR num Compensation 8,3 ;VAR num CompcnsationTwo8,3；PERS numCompcnsationErr8,3:=999999,999999,999999,999999,999999,999 999,999999,999999,999999,999999,999999,999999,999999,999999, 999999,999999,999999,999999,9999

7、99,999999,999999,999999,999 999,999999；PERS jointtargetjposHome:=0,0,0,0,0,0,9E+09,9E+09,9E+09,9E+09,9E+09,9E+09 ；CONST robtargetpHome:=621.23,-975.96,1166.44 ,0.00703884,-0.385671 ,-0.922573,-0 .00826231,-l,0,-l,0,9E+09,9E+09,9E+09,9E+09,9E+09,9E+09;CONST robtargetPZhanbanSafe:= -162.69,-1703.07,22

8、70.38 ,0.0109452,-0.955357,-0.2 95179,-0.00645602,-l,-l,-l,0,9E+09,9E4-09,9E+09,9E-l-09,9E+09,9 E+09;CONST robtargetVisionA:=-228.96,94.47,643.11,0.00434955,-0.699954,0.714102,-0.0101798,-2,-l,0,0,9E+09,9E+09,9E+09,9E+09,9E+09,9E+09;CONST robtargetPickA:=90.09,79.65,61.29,0.0102135,0.00299714,0.9999

9、35,-0.00417093 ,-2,-1,1,0 ,9E+09,9E+09,9E+09,9E+09,9E+09,9E+09;CONST robtargetPlaceA:= -644.46,-1 ()19.60,396.56,0.00841448,-0.692574,0.721298,0.000680825 ,-1，-1,1,0 ,9E+09,9E+09,9E+09,9E+09,9E+09,9E+09;CONST rob targetPZhanbanUpA:=549.64,541.39,821.21,().00()808()37,-().999985,-0.0() 498684,-0.0020

10、9158,-2,-l,-l,0,9E+09,9E+09,9E+09,9E+09,9E+09, 9E+09;CONST rob targetPZhanbanDownA:=548.40,560.53,179.18,0.00417276,0.999929,-0.() 0436864,0.0102239,-2,-l,-l,0,9E+09,9E+09,9E+09,9E+09,9E+09,9 E+09;CONST robtargetPlaceZhanbanA:=2456.73,-1154.76,205.32,0.0106126,-0.0247295,-0. 999603,0.00838785,1,1,0,

11、0,9E+09,9E+09,9E+09,9E+09,9E+09,9 E+09;PERS numCompensationA8,3:=0,0,0,210,136,0,430,264,99999999,645,403.0 ,855,533,99999999,99999999,670,99999999,99999999,8 0(),99999999,99999999,936,99999999;PERS numCompensationAl 8,3:=860,936,0,0,0,0,215,133,99999999,430,264.0 ,645,403,99999999,99999999,533,9999

12、9999,99999999,670.0 ,99999999,800,99999999;PERS numCompensationA2 8,3 :=860,0,0 ,0,133,0,215,264,99999999 ,430,4 03,99999999,645,533,99999999,99999999,670,99999999,99999999,800.0 ,99999999,936,99999999；CONST num nPalletHighUpA:=10();CONST num nPalletHighDownA:=200;CONST rob targetVisionB:=936.63,959

13、.75,378.90,0.00373262,0.105479,-0.994406,-0.() 0407233,卜 1,0,0,0,9E+09,9E+09,9E+09,9E+09,9E+09,9E+09;CONST rob targetPickB:=l 180.22,1009.95,213.74,0.00373443,0.1055,-0.994404,-0.004 06331, -l,0,0,0,9E+09,9E+09,9E+09,9E+09,9E+09,9E+09;CONST rob targetPlaceB:=-1194.30,-1552.83,582.17,0.0062614,079593

14、8,-0.605346,-0. 000999941,0,0,0,0,9E+09,9E+09,9E+09,9E+09,9E+09,9E+09;CONST robtargetPZhanbanUpB:=511.86,550.0&92.05,0.00172284,-0.00758955,-0.99 997,-0.000606164,-l,03,0,9E+09,9E+09,9E4-09,9E+09,9E+09,9E +09;CONST rob targetPZhan banDownB:二511.86,550.0&92.05,0.00172284,-0.00758955,-0. 99997

15、,0.000606164, -l,0,l,0,9E+09,9E+09,9E+09,9E+09,9E+09,9 E+09;CONST robtargetPlaccZhanbanB:=511.86,550.08,92.05,0.00172284,-0.()0758955,-0.99 997,-0.000606164,卜 1,0,1,0,9E+09,9E+09,9E+09,9E+09,9E+09,9E +09;PERS numCompensationB 8,3:=0,0,300,210,136,0,430,264,99999999,645,403.0 ,855,533,99999999,999999

16、99,670,99999999,99999999.800.0 ,99999999,936,99999999;PERS numCompcnsationBl 8,3:=860,936,300,0,0,0,215,133,99999999,430 ,264,99999999,645,403,99999999,99999999,533,99999999,9999999 9,67(),99999999,99999999,800,99999999;PERS numCompensationB28,3:=860,0,300,0,133,0,215,264,99999999,430 ,403,99999999,

17、645,533,99999999,99999999,670,99999999,99999999,800,0 ,99999999,936,99999999;CONST num nPallctHighUpB:=l()();CONST num nPalletHighDownB:=200;PERS speeddata vMinEmpty:= 1300,100,6000,1000;PERS speeddata vMidEmpty：=1400,100,6000,1 ()()();PERS speeddata vMaxEmpty：=l 500,10(),6000,1000;PERS speeddata vB

18、igMaxEmpty:=1500,100,6000,1000;PERS speeddata vMinLoad:=1200,100,6000,1000;PERS speeddata vMaxLoad:=1300,100,6000,1 ()()();PERS speeddata vBigMaxLoad:=1400,100,600(),1000;TASK PERS wobjdatawobj 1 :=FALSE,TRUE,"n,0,0,0 ,1,0,0,0 ,1984.06,-1180.48,453.803 ,0.661114,-0.659907,-0.252753,0.252124;PRO

19、C MainOrlnitAll;WHILE TRUE DOrPickCal; rPick;ENDWHILEWaitTimc ().3;ENDPROCPROC rlnitAHOAccSct 60,100;Compensation:=CompcnsationErr;CompcnsationTwo:=CompcnsationErr;rcgl :=();rcg2 :二()； nCountX:=l; nCountY:=l; nCount:=0;nCountZ:=l; nTotalPallctHigh:=(); nPalletHigh:=O; nlnpos:=(); rKindChoosc;rChcckH

20、omePos；Reset do()()Jaws()pen ;Reset doOl JawsClose ;Set do()()Jaws()pen ;Reset do()2_BigJawsl ;WaitTime ().3;Reset do()3_BigJaws2;Reset do()()Jaws()pen ;Reset dol3_Placing;Reset dol2_PlaceOI<ENDPROCPROC rPickCal 0Set do()()Jaws()pen ; bSMPre()rAbs:=TRUE;IF nCount 二()THENrComprcssorlnPos;ENDIFWHIL

21、E bSMPwOrAbs二TRUE DOIncr nCount;IF nCount =1 THENMovcL Offs(Vision /)/),CompensationnCountZ,3 +1 ()0) "Big卜laxEmpty ,z20 , tGrippcrWC)bj:=WobjCompressor ;MoveJ Offs(Vision /)/),Compensation nCountZ,3 ) ,vBigMaxEmpty7 fine ,tGrip perWObj:=WobjCompressor ;GOTO C;ENDIFIncr rcg2 ;rnXCal;pVision:=()

22、ffs(Vision >CompensationnCountX>l >CompcnsationnCountY,2 ， Compensation nCountZ );MovcLpVision.vMidEmpty ?finc,tGrippcrW()bj:=WobjComprcssor;c：nPrescncc()rAbscncc:=l;!rScncr;!socketscnd clicnt_socket Str:=MTln;!sockctRcccivc clicnt_socket Str:=stRcccived;!b ()K:=StrToVQl(srRucDivud,nPwscncD

23、()rAbsc：ricc:);!IF nPrescnceC)rAbsence =2 OR nPrescnce()rAbsence=3 THEN!bSMPre()rAbs:=FALSE;!ELSEIF nPresenceOrAbsence 二5 THEN!bSMPwOrAbs:二TRUE ;!IF di()2_PhotoelectricSensor二 1 THEN!TPErasc;!TPWrite nThc camera get picture there is aproblem, Please check it!(2) n;!SystemStopAction Hak ;!ELSEIF di()

24、2_PhotoelcctricScnsor =() THEN!rcg2 :=();!ENDIF!ELSEIF nPrescnccOrAbsence 二 1 ORnPrcscnccC )r Absence =4 TH ENnCountY:=l;nlnpos:=();IF reg2=0 THENrZhanban ;IF nCountZ=2 THEN rCompressor( )utPos; nCountZ:=l;ENDIFENDIFreg2:=0;ENDIF!DEFAULT:!TPErase;!TP Write " The PLC communication there is aprob

25、lcm ,Please check it!ENDTESTENDWHILEENDPROCPROC rPick 0!TESTdil6_giBCD!CASE 1:IF nCountXl THEN CompcnsationTwo:=CompensationAl;ELSECompcnsationTwo:=CompensationA2;ENDIFpPick:=()ffs(Pick,CompcnsationTwo nCountX,l +nX ,CompcnsationTwo nCo untY,2 ,CompcnsationTwo nCountZ,3)；CASE 2:!pPick:=()ffs(Pick.Co

26、mpcnsationTwo nCountXJ +nX ,CompcnsationTwonCo untY,2 CompcnsationTwo nCountZ,3);!DEFAULT:!TPErase;!TP Write M The PLC communication there is aproblem Please check it n;!ENDTESTWaitTimc 0.2;nCam()ut:=l;!rSener;!socketscnd client_socket Str:=nT2n;!sockctReceivc clicnt_socket Str:=stReceivcd;!XData:=

27、StrPart(stReccivcd, 3, NumCharactcrs);!YData:= StrPart(stRcccived, NumCharactcrs,NumCharactcrs);!AnglcData:33 StrParstRcccivcd, 2*NumCharactcrs,N umCharac ters);!StrToVal(XData,nX()ffs);!StrToVal(YData,nY()ffs);!b()I<:=StrToVal (AnglcD ata,nAngle() ffs);r Angle;!ppPick :=RelTool (pPick,0,0,0Rz: =

28、 -nAnglcOffs+nAnglc);ppPick :=Rd1Tq()1 (pPick,(),(),()Rz:= nAngle);ContL()ff;!MoveL Offs (ppPick,nX()ffs ,nY()ffs,120), vMinEmpty, fine , tGrippcrW()bj:=WobjComprcssor;!MoveL Offs (ppPick,nX()ffs ,nY()ffs,0), vl()(), fine ,tGripperW()bj:=WobjCompressor;MoveL Offs (ppPick,() ,0,200), vMinEmpty, fine

29、, tGrippcrW()bj:=WobjComprcssor;MoveL Offs (ppPick,0 ,0,0), vl 0(), fine , tGrippcrWC)bj:=WobjComprcssor;Reset do()()_JawsOpen ;Set do()lawsClosc ;WaitDI di00_JawsInClose,l;!MoveL Offs(ppPick,nXOffs,nYOffs,80),vMinLoad ,210 ,tGripperWObj:=Wobj Compressor ;!MoveL Offs(ppPick,nX()ffs,nY()ffs,400),vMax

30、Load fine ,tGrippcrW()bj:=Wo bjComprcssor ;MoveL Offs(ppPick,0,0,80),vMinLoad ,zl 0 ,tGrippcr)bj: =WobjCompressorMoveL Offs(ppPick,0,0,400),vMaxLoad ,fine ,tGripperWObj:=WobjCompressoConfL()n;rPlacc;TEST di!6_giBCDCASE 1:IF nCount 二4() THENnCount :=();nCountX:=l;nCountY:=l;reg2:=0;nlnpos:=0;rZhanban

31、 ;IF nCountZ=2 THENrComprcssorC )utPos; nCountZ:=l;ENDIFENDIF!DEFAULT:!TPErase;!TPWrite " The PLC communication there is aproblem ,Please check it ”；!ENDTESTENDPROCPROC rPlace 0MovcJ Offs (Place,0,0,200) ,vMaxLoad ,z40 , tGrippcrW()bj:= WobjCompressor ;Set dol3_Placing;Waitdi di()6_AssemblyLine

32、()I<, 0;MovcL Offs (Place,0,(),50) ,vMinLoad ,z5 , tGrippcrW()bj:= Wobj Compressor;MovcL Place ,vl()() ,finc , tGrippcr)bj:=Wobj Compressor;Reset do()l_JawsClosc ;Set do()()Jaws()pcn ;WaitDI di()lawsInOpcn ,1;MovcL Offs (Place,0,(),300) ,vMinEmpty ,finc , tGrippcrW()bj:= Wobj Compressor;Reset dol

33、 3_Placing;WaitTime 0.4;Reset do()()J awsOpen ;ENDPROCPROC rnXCalO!TEST dil6_giBCD!CASE 1:TEST nCountCASE1,2,3,4,5,11,12,13,14,15,21,22,23,24,25,31,32,33,34,35 : nX:=();CASE 6,7,8,9,10,16,17,18,19,20,26,27,2&29,3(),36,37,3&39,4(): nX:=65;ENDTEST!ENDTESTENDPROCPROC rAngle0!TEST dil6_giBCD! CA

34、SE 1:TEST nCountCASE1,2,3,4,5,11,12,13,14,15,21,22,23,24,25,31,32,33,34,35 : nAnglc:=();CASE 6,7,&9,10,16,17,18,19,20,26,27,2&29,30,36,37,38,39,40 :nAngle:=180;ENDTESTENDTESTENDPROCPROC rZhanban 0 nTotalPallctHigh:=nTotalPallctHigh+nPallctHigh;TEST nCountZCASE 1:PZhan ban:二 PZhanbanUp; nPall

35、ecHigh:=nPallctHighUp;CASE 2:PZhanban:=PZhanbanDown; nPalletHigh:=nPalletHighDown;DEFAULT:TPErasc ;TPWritc " The 'nCountZ' have a trouble ,Please checkENDTESTMoveJ Offs(PZhanban,(),(),30() ,vBigMaxEmpty ,finc , tGripper W()bj:= WobjCompressor ;Set do()2_BigJawsl ;WaitTimc ().6;Set do()3

36、_BigJaws2;WaitDI di()8_BigJawsIn()pcn, 1;MovcL PZhanban ,vMinEmpty ,finc , tGrippcrW()bj:二WobjCompressor ;WaitTimc ().5;Reset do()2_BigJawsl ;Reset do()3_BigJaws2 ;WaitTimc ().3;WaitDI di()8_BigJawsIn()pcn,l;pActualpos:=CRo bT(Toc)l:=rGrippur'W()bj:=:WQbjCornpwssQrl); pActualpos.trans.z:=2000;Mo

37、vcLpActualpos,vMinEmpty,220,tGripperWObj:=WobjCompressorl ;!MoveL Offs ( PZhonban,(),(),400) ,vMinEmpty ,z20 ,tGripperW()bj:= WobjCompressor ;WaitDI di()5_PallctInPos ,1;IF nlnpos 二2 THENMove PZhanbanSafe ,vMinEmpty ,z20 ,tGrippcrW()bj:= WobjCompressor ;ENDIFpActualpos:=PlaccZhan ban;pActualpos.tran

38、s.z:=2000;MovcJpActualpos,vMinLoad,z20,tGripperW()bj:=WobjComprcssorl ;!MoveJ Offs(PlaceZhanban ,0,0,nTotalPalletHigh+700),v5()() ,z5 ,tGripperW()bj:= WobjCompressor ;MoveL Offs(PlaceZhanban ,0,(),nTotalPallctHigh) ,vMinLoad ,finc , tGrippcrW()bj:= WobjCompressor ;WaitTimc ().5;Set do()2_BigJawsl ;S

39、et do()3_BigJaws2;WaitDI di()8_BigJawsIn()pcn,l;MoveL Offs(PlaceZhanban ,0,(),nTotalPallctHigh+70() ,vMaxEmpty ,finc , tGripperW()bj:= WobjCompressor ;Reset do()3_BigJaws2;WaitTimc ().6;Reset do()2_BigJawsl;WaitDI di()7_BigJawsInClose, 1;rComprcssor( )utPos;IF nTotalPalletHigh>120() THENset do06_

40、PalletEmpty;waittimc 1;reset do()6_PallctEmpty;nTotalPalletHigh:=0; nPallctHigh:=();ENDIFENDPROCPROC rComprcssorlnPosOblnpos:二 TRUE;WHILE blnpos二TRUE D()IF di()3_Comprcssorl InPos =1 THEN WobjCompressor:=WobjComprcssorl; blnpos:= FALSE;nlnpos:=2;ELSEIF di()4_Comprcssor2InPos =1 THEN WobjComprcssor:=

41、WobjCompressor2; bln pos:=FALSE;nlnpos:=3;ELSEblnpos:二 TRUE;ENDIFWaitTime ().3;ENDWHILEENDPROCPROC rComprcssorOutPosOIF WobjCompressor=WobjCompressor! THENSet do()4_Compressorl Empty ;WaitTime 1;Reset do()4_Comprcssorl Empty;ELSEIF WobjComprcssor=WobjCompressorl THENSet do()5_Compressor2Empty ;WaitT

42、ime 1;Reset do()5_Comprcssor2Empty;ENDIFENDPROCFUNC bool CurrcntPos(robtarget ComparePosJNOUT tooldataTCP)VAR num Counter:=0;VAR rob target ActualPos;ActualPos:=CRobT(Tool:=TCPWObj:=wobjO);IF ActualPos.trans.x>ComparcPos.trans.x-25 ANDActualPos.trans.x<ComparcPos.trans.x+25 Counter:=Countcr+1;

43、IF ActualPos.trans.y>ComparcPos.trans.y-25 ANDActualPos.trans.y<ComparcPos.trans.y+25 Counter:=Countcr+1;IF AcrualPos.trans.z>ComparcPos.trans.z-25 ANDActualPos.trans.z<ComparePos.trans.z+25 Countcr:=Countcr+l;IF AcmalPos.rot.ql >ComparcPos.rcn.qANDActualPos.rot.ql <ComparcPos.r o

44、匸 ql +0.1 Countcr:=Countcr+1;IF AcmalPos.rot.q2>ComparePos.rot.q2-0.1 ANDActualPos.ro t.q 2<ComparcPos.r o 匸q 2+0.1 Countcr:=Counter+1;IF AccualPos.rot.q3>ComparcPos.rot.q3-0.1 ANDActualPos.ro匸q3<ComparcPos.rot.q3+().l Countcr:=Countcr+1;IF ActualPos.rot.q4>ComparcPos.rot.q4-0.1 ANDAc

45、tualPos.ro t.q4 < ComparePos.rotq4+0.1 Counter:=Coun ter +1;RETURN Countcr=7;ENDFUNCPROC rCheckHomcPosOIF NOT CurrcntPos(pHome,tGrippcr) THENpActualpos:=CRobT(Tool:=tGripperWObj:=wobjO);pActualpos.trans.zipHome.trans.z;MovcL pActualpos,v3()(),z3()jGrippcr;Move) pHome,v5()(),fine .tGripper;ENDIFEN

46、DPROCPROC rKindChooseOIF 6_giBCD=l THENCompcnsation:=CompcnsationA; nPallctHighUp:=nPallctHighUpA; nPallctHighDown:=nPallctHighDownA; Pick:= PickA;Vision :=VisionA;Place:=PlaccA;PZhanbanUp:=PZhanbanUpA; PZhanbanDown:=PZhanbanDownA;PlaceZhanban:=PlaccZhanbanA;ELSEIF chi6_giBCD=2 THENCompcnsation:=CompcnsationB;nPallctHighDown:=nPallctHighDownB;Pick:= PickB;Vision :=VisionB;!Place:=PlaccB;!PZhanbanUp:=PZhanbanUpB;!PZhanbanDown:=PZ