ABB机器人码垛程序.docx

《ABB机器人码垛程序.docx》由会员分享，可在线阅读，更多相关《ABB机器人码垛程序.docx（40页珍藏版）》请在得力文库 - 分享文档赚钱的网站上搜索。

1、MODULE MainModulePROC Main()TPErase;TPReadNum nCount1, Qing Shu Ru Yi Ma Bao Shu! ;TPReadNum nJob, Qing Shu Ru Mo Shi:50KG:=1,25KG:=2!;InitAll;WHILE TRUE DOReadType;clock2re;PickIF;clock2re;PlaceOF;ENDWHILEENDPROCPROC clock2re()!tempint:=movstat; IF DI10_1Product0K = 0 or DI10_2TuoPanOK = 0 ThenIF t

2、empint0.25 thenClkStart CLK2;ELSEClkStop CLK2;ENDIFElseClkStop CLK2;EndIFEndprocPROC InitAll()MoveHome;Reset DO10_1JiaZhua;Reset DO10_2YaBan;Reset DO10_3ZhuaOK;Reset DO10_4MaDuoOK;Waittime 0.5;bPickPart:=FALSE;ClkReset CLK1;ClkStart CLK1;ClkReset CLK2;MoveL Offs(pPick,0,0,400), vFast, z10, tGripperW

3、Obj:=wobj0;ENDPROCPROC PickIF()IF bPickPart = FALSE AND nJob 0 AND DI10_1Product0K = 1 THEN!CalculatePick;MoveJ Offs(pPick,0,0,400), vFast, z200, tGripperWObj:=wobj0;!MoveLDO Offs(pPick,0,0,100), vFast, z20, tool0WObj:=wobj0, DO10_1JiaZhua, 1;MoveL pPick, vFast, fine, tGripperWObj:=wobj0;Close1;Grip

4、Load LoadFull;Accset 50,50;HandshakeIF;bPickPart:=TRUE;ConfLOff;MoveJ pAfterPick, vFast, z60, tGripperWObj:=wobj0;ENDIFENDPROCPROC PlaceOF()IF DI10_2TuoPanOK = 1 AND bPickPart = TRUE THENCalculatePlace;ConfLOff;!MoveL Offs(pOver,0,0,nOffsZ2), vFast, z200, tGripper;IF bTag=true thenAccset 70, 70;Move

5、J pAbovePlace, vFast, z10, tGripperWObj:=wobj0;elseAccset 90, 90; MoveJ pAbovePlace, vFast, z10, tGripperWObj:=wobj0; EndifAccset 100,100;! place first boxConfLOff;!MoveL Offs(pPlace1,nOffsX1,nOffsY1,nOffsZ1),vTurn,z10,tGripperWObj:=wobj0;MoveL pPlace1, vMiddle, fine, tGripperWObj:=wobj0;Open1;ConfL

6、Off;MoveL Offs(pPlace1,0,0,nOffsZ1),vMiddle,z5,tGripperWObj:=wobj0;GripLoad LoadEmpty;Accset 100,100;bPickPart:=FALSE;Incr nCount1;HandshakeOF;MoveJ pAbovePick, vFast, z200, tGripperWObj:=wobj0;MoveL Offs(pPick,0,0,400), vFast, z10, tGripperWObj:=wobj0;!MoveJ pHome, vFast, z200, tGripper;ENDIFENDPRO

7、CPROC HandshakeIF()IF nJob=1 THEN!Set DO10_3ZhuaOK;ENDIFIF nJob=2 THEN!Set DO10_3ZhuaOK;ENDIFENDPROCPROC HandshakeOF()IF (nCount1=40 and nJob =1) or (nCount1=80 and nJob =2) THENSet DO10_4MaDuoOK;WaitDI DI10_2TuoPanOK, 0;SetDOSDelay:=0.5,DO10_4MaDuoOK,0;nCount1:=0;ClkStop CLK1;ClkStop CLK2;TPErase;n

8、CycleCount:=ClkRead(CLK1);Waitingtime:=ClkRead(CLK2);TPWrite Cycle time: Num:=nCycleCount;TPWrite Waiting time in this Cycle: Num:=Waitingtime;ClkReset CLK2;ClkReset CLK1;ClkStart CLK1;ENDIFENDPROCPROC MoveHome()TempP1:=CRobT(Tool:=tool0 WObj:=wobj0);TempP1.trans.z:=500 ; MoveJ TempP1, v1000, z20, t

9、Gripper;MoveJ pHome, v1000, z20, tGripper;ENDPROCPROC ReadType()IF nPriority = 1 AND DI10_1Product0K = 1 THEN!nJob:=1;! Incr nPriority;ELSEIF nPriority = 2 AND DI10_1Product0K = 1 THEN!nJob:=2;!Incr nPriority;ELSE!nJob:=0;!Incr nPriority;!IF nPriority2 nPriority:=1;ENDIFENDPROCPROC Open1()Reset DO10

10、_2YaBan;Waittime 0.0;Reset DO10_1JiaZhua;waittime 0.1;!Waittime nTimeGripper;ENDPROCPROC Close1()Set DO10_1JiaZhua;Waittime 0.0;WaitDI DI10_4Zhua1End, 1;Set DO10_2YaBan;Waittime 0.0;!Waittime nTimeGripper;WaitDI DI10_4Zhua1End, 1;!WaitDI DI10_6Zhua2End, 1;ENDPROCPROC CalculatePick()IF nJob = 1 THENp

11、Pick:=pPick1;ELSEIF nJob = 2 THENpPick:=pPick1;ENDIF!pAbovePick:=pPick;!pAfterPick:=pPick;!Pattern80;!pAbovePick.trans.z:= pAfterPick.trans.z;!pAbovePick.trans.y:= pAfterPick.trans.y-500;!pAfterPick.trans.y:= pAfterPick.trans.y-500;ENDPROCPROC CalculatePlace()IF nJob = 1 THENnBagL:=nBag50L;nBagW:=nB

12、ag50W;nBagH:=nBag50H;Pattern80;ELSEIF nJob = 2 THENnBagL:=nBag25L;nBagW:=nBag25W;nBagH:=nBag25H;Pattern100;ENDIF!pAbovePlace:=pPlace1;pAfterPlace:=pAbovePlace;ENDPROCPROC Pattern80()bTag:=FALSE;TEST nCount1+1CASE 1:pPlace1:=pBase180;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y;p

13、Place1.trans.z:=pPlace1.trans.z;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=500;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;bTag:=True;CASE 2:pPlace1:=pBase0;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans

14、.y;pPlace1.trans.z:=pPlace1.trans.z;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 3:pPlace1:=pBase90;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans

15、.y+25;pPlace1.trans.z:=pPlace1.trans.z;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 4:pPlace1:=pBase90;pPlace1.trans.x:=pPlace1.trans.x+2*nBagW;pPlace1.trans.y:=pP

16、lace1.trans.y+25;pPlace1.trans.z:=pPlace1.trans.z;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 5:pPlace1:=pBase90;pPlace1.trans.x:=pPlace1.trans.x+nBagW;pPlace1.tr

17、ans.y:=pPlace1.trans.y+25;pPlace1.trans.z:=pPlace1.trans.z+20;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;!Layer2 CASE 6:pPlace1:=pBase180;pPlace1.trans.x:=pPlace1.tra

18、ns.x;pPlace1.trans.y:=pPlace1.trans.y+nBagL-5;pPlace1.trans.z:=pPlace1.trans.z+nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 7:pPlace1:=pBase0;pPlace1.trans.x

19、:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y+nBagL-5;pPlace1.trans.z:=pPlace1.trans.z+nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 8:pPlace1:=pBase270;

20、pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans.z;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 9:pPlace1:=pBase270

21、;pPlace1.trans.x:=pPlace1.trans.x+2*nBagW-25;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans.z;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=2;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 10:pPla

22、ce1:=pBase270;pPlace1.trans.x:=pPlace1.trans.x+nBagW;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans.z;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=2;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;!Lay

23、er3CASE 11:pPlace1:=pBase180;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans.z+2*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick

24、.trans.z+400;bTag:=True;CASE 12:pPlace1:=pBase0;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans.z+2*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAboveP

25、lace.trans.z:=pPick.trans.z+400;CASE 13:pPlace1:=pBase90;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y+25;pPlace1.trans.z:=pPlace1.trans.z+2*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPl

26、ace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 14:pPlace1:=pBase90;pPlace1.trans.x:=pPlace1.trans.x+2*nBagW;pPlace1.trans.y:=pPlace1.trans.y+25;pPlace1.trans.z:=pPlace1.trans.z+2*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+

27、400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 15:pPlace1:=pBase90;pPlace1.trans.x:=pPlace1.trans.x+nBagW;pPlace1.trans.y:=pPlace1.trans.y+25;pPlace1.trans.z:=pPlace1.trans.z+2*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-500;nLevel:=1;bTwice:=FALSE;pAfterPick.trans

28、.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;!layer4 CASE 16:pPlace1:=pBase180;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y+nBagL-5;pPlace1.trans.z:=pPlace1.trans.z+3*nBagH+80;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-300;nLevel:=1;b

29、Twice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 17:pPlace1:=pBase0;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y+nBagL-5;pPlace1.trans.z:=pPlace1.trans.z+3*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=

30、-300;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 18:pPlace1:=pBase270;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans.z+2*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2

31、;nOffsZ2:=-300;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 19:pPlace1:=pBase270;pPlace1.trans.x:=pPlace1.trans.x+2*nBagW-25;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans.z+2*nBagH;nOffsX1:=0;nOffsY1:

32、=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-300;nLevel:=2;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;CASE 20:pPlace1:=pBase270;pPlace1.trans.x:=pPlace1.trans.x+nBagW;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans.z+2*nBagH;nOf

33、fsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-300;nLevel:=2;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+400;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+400;!layer5CASE 21:pPlace1:=pBase180;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.tr

34、ans.z+4*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-300;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+500;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+500;bTag:=True;CASE 22:pPlace1:=pBase0;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.

35、trans.z:=pPlace1.trans.z+4*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-300;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+500;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+500;CASE 23:pPlace1:=pBase90;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y

36、+25;pPlace1.trans.z:=pPlace1.trans.z+4*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-300;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+500;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+500;CASE 24:pPlace1:=pBase90;pPlace1.trans.x:=pPlace1.trans.x+2*nBagW;pPlace1.tran

37、s.y:=pPlace1.trans.y+25;pPlace1.trans.z:=pPlace1.trans.z+4*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-300;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+500;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+500;CASE 25:pPlace1:=pBase90;pPlace1.trans.x:=pPlace1.trans.x+

38、nBagW;pPlace1.trans.y:=pPlace1.trans.y+25;pPlace1.trans.z:=pPlace1.trans.z+4*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-300;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+500;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+500;!layer6CASE 26:pPlace1:=pBase180;pPlace1

39、.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y+nBagL-5;pPlace1.trans.z:=pPlace1.trans.z+5*nBagH-50;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-100;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+450+nBagH;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+450+nBagH;

40、CASE 27:pPlace1:=pBase0;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y+nBagL-5;pPlace1.trans.z:=pPlace1.trans.z+5*nBagH-50;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-100;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+500+nBagH;pAbovePlace:=pPlace1;pAbovePlace.tr

41、ans.z:=pPick.trans.z+500+nBagH;CASE 28:pPlace1:=pBase270;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans.z+4*nBagH-45;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-100;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+500+nBagH;pAbovePlac

42、e:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+450+nBagH;CASE 29:pPlace1:=pBase270;pPlace1.trans.x:=pPlace1.trans.x+2*nBagW-25;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans.z+4*nBagH-45;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-100;nLevel:=2;bTwice:=FALSE;pAfterPick.trans.z

43、:=pPick.trans.z+500+nBagH;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+500+nBagH;CASE 30:pPlace1:=pBase270;pPlace1.trans.x:=pPlace1.trans.x+nBagW;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans.z+4*nBagH-45;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-100;nLevel:=2;b

44、Twice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+500+nBagH;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+500+nBagH;!layer7CASE 31:pPlace1:=pBase180;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans.z+6*nBagH;nOffsX1:=0;nOffsY1:=0;nOffsZ1:=nBagH*1.2;nOffsZ2:=-100;nLevel:=1;bTwice:=FALSE;pAfterPick.trans.z:=pPick.trans.z+450+2*nBagH;pAbovePlace:=pPlace1;pAbovePlace.trans.z:=pPick.trans.z+450+2*nBagH;bTag:=True;CASE 32:pPlace1:=pBase0;pPlace1.trans.x:=pPlace1.trans.x;pPlace1.trans.y:=pPlace1.trans.y;pPlace1.trans.z:=pPlace1.trans