码垛案例分析.docx

1、 1 码垛工作站案例分析 1、IO 单元信号定义 DSQC652 IO 单元 2、系统信号的关联 3、程序数据 PERS wobjdata WobjPallet_L:=FALSE,TRUE,“,-456.216,-2058.49,-233.373,1,0,0,0,0,0,0,1,0,0,0; PERS wobjdata WobjPallet_R:=FALSE,TRUE,“,-421.764,1102.39,-233.373,1,0,0,0,0,0,0,1,0,0,0; PERS tooldata tGripper:=TRUE,0,0,527,1,0,0,0,20,0,0,150,1,0,0,0

2、,0,0,0; PERS loaddata LoadFull:=20,0,0,300,1,0,0,0,0,0,0.1; PERS wobjdata CurWobj; PERS jointtarget jposHome:=0,0,0,0,0,0,9E+09,9E+09,9E+09,9E+09,9E+09,9E+09; 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 ro

3、btarget pPlaceBase90_L:=218.407102669,695.953395421,3.210997808,0,-0.000001669,1,0,-2,0,-2,0,9E9,9E9,9E9,9E9,9E9,9E9; CONST robtarget 2 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

4、.953395421,3.210997808,0,-0.00038594,0.999999926,0,1,0,1,0,9E9,9E9,9E9,9E9,9E9,9E9; 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,-

5、0.706993181,0,0,0,-1,0,9E9,9E9,9E9,9E9,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; PERS robtarget pPlaceBase0; PERS robtarget pPlaceBase90; PERS robtarget pPick; PERS robtarget pPlace; PERS robtarget p

6、PickSafe; PERS num nCycleTime:=3.263; PERS num nCount_L:=9; PERS num nCount_R:=13; PERS num nPallet:=1; PERS num nPalletNo:=2; PERS num nPickH:=300; PERS num nPlaceH:=400; PERS num nBoxL:=605; PERS num nBoxW:=405; PERS num nBoxH:=300; VAR clock Timer1; PERS bool bReady:=TRUE; PERS bool bPalletFull_L

7、:=FALSE; PERS bool bPalletFull_R:=FALSE; PERS bool bGetPosition:=TRUE; VAR triggdata HookAct; VAR triggdata HookOff; VAR intnum iPallet_L; VAR intnum iPallet_R; PERS speeddata vMinEmpty:=2000,400,6000,1000; PERS speeddata vMidEmpty:=3000,400,6000,1000; PERS speeddata vMaxEmpty:=5000,500,6000,1000; P

8、ERS speeddata vMinLoad:=1000,200,6000,1000; PERS speeddata vMidLoad:=2500,500,6000,1000; PERS speeddata vMaxLoad:=4000,500,6000,1000; PERS num Compensation15,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; 4.主程序 PROC main() rInitAll; WHILE TRUE DO IF bRe

9、ady THEN rPick; rPlace; ENDIF rCycleCheck; 3 ENDWHILE ENDPROC 5.其他例行程序 PROC rInitAll() rCheckHomePos; ConfLOFF; ConfJOFF; nCount_L:=1; nCount_R:=1; nPallet:=1; nPalletNo:=1; bPalletFull_L:=FALSE; bPalletFull_R:=FALSE; bGetPosition:=FALSE; Reset do00_ClampAct; Reset do01_HookAct; ClkStop Timer1; ClkR

10、eset Timer1; TriggEquip HookAct,100,0.1DOp:=do01_HookAct,1; TriggEquip HookOff,100Start,0.1DOp:=do01_HookAct,0; IDelete iPallet_L; CONNECT iPallet_L WITH tEjectPallet_L; ISignalDI di02_PalletInPos_L,0,iPallet_L; IDelete iPallet_R; CONNECT iPallet_R WITH tEjectPallet_R; ISignalDI di03_PalletInPos_R,0

11、,iPallet_R; ENDPROC PROC rPick() ClkReset Timer1; ClkStart Timer1; rCalPosition; MoveJ Offs(pPick,0,0,nPickH),vMaxEmpty,z50,tGripperWObj:=wobj0; MoveL pPick,vMinLoad,fine,tGripperWObj:=wobj0; Set do00_ClampAct; Waittime 0.3; GripLoad LoadFull; TriggL Offs(pPick,0,0,nPickH),vMinLoad,HookAct,z50,tGrip

12、perWObj:=wobj0; MoveL pPickSafe,vMaxLoad,z100,tGripperWObj:=wobj0; ENDPROC PROC rPlace() MoveJ Offs(pPlace,0,0,nPlaceH),vMaxLoad,z50,tGripperWObj:=CurWobj; TriggL pPlace,vMinLoad,HookOff,fine,tGripperWObj:=CurWobj; Reset do00_ClampAct; Waittime 0.3; GripLoad Load0; MoveL Offs(pPlace,0,0,nPlaceH),vMi

13、nEmpty,z50,tGripperWObj:=CurWobj; rPlaceRD; MoveJ pPickSafe,vMaxEmpty,z50,tGripperWObj:=wobj0; 4 ClkStop Timer1; nCycleTime:=ClkRead(Timer1); ENDPROC PROC rCycleCheck() TPErase; TPWrite “The Robot is running!“; TPWrite “Last cycle time is : “Num:=nCycleTime; TPWrite “The number of the Boxes in the L

14、eft pallet is:“Num:=nCount_L-1; TPWrite “The number of the Boxes in the Right pallet is:“Num:=nCount_R-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:=FALSE; WaitTime 0.

15、1; ENDIF ENDPROC PROC rCalPosition() bGetPosition:=FALSE; WHILE bGetPosition=FALSE DO TEST nPallet CASE 1: IF bPalletFull_L=FALSE AND di02_PalletInPos_L=1 AND di00_BoxInPos_L=1 THEN pPick:=pPick_L; pPlaceBase0:=pPlaceBase0_L; pPlaceBase90:=pPlaceBase90_L; CurWobj:=WobjPallet_L; pPlace:=pPattern(nCou

16、nt_L); bGetPosition:=TRUE; nPalletNo:=1; ELSE bGetPosition:=FALSE; ENDIF nPallet:=2; CASE 2: IF bPalletFull_R=FALSE AND di03_PalletInPos_R=1 AND di01_BoxInPos_R=1 THEN pPick:=pPick_R; pPlaceBase0:=pPlaceBase0_R; pPlaceBase90:=pPlaceBase90_R; CurWobj:=WobjPallet_R; pPlace:=pPattern(nCount_R); bGetPos

17、ition:=TRUE; nPalletNo:=2; ELSE bGetPosition:=FALSE; ENDIF nPallet:=1; DEFAULT: 5 TPERASE; TPWRITE “The data nPallet is error,please check it!“; Stop; ENDTEST ENDWHILE ENDPROC FUNC robtarget pPattern(num nCount) VAR robtarget pTarget; IF nCount=1 AND nCount=6 AND nCount=11 AND nCount15 THEN Set do02

18、_PalletFull_L; bPalletFull_L:=TRUE; nCount_L:=1; ENDIF CASE 2: Incr nCount_R; IF nCount_R15 THEN Set do03_PalletFull_R; bPalletFull_R:=TRUE; nCount_R:=1; ENDIF DEFAULT: TPERASE; TPWRITE “The data nPalletNo is error,please check it!“; Stop; 8 ENDTEST ENDPROC PROC rCheckHomePos() VAR robtarget pActual

19、Pos; IF NOT CurrentPos(pHome,tGripper) THEN pActualpos:=CRobT(Tool:=tGripperWObj:=wobj0); pActualpos.trans.z:=pHome.trans.z; MoveL pActualpos,v500,z10,tGripper; MoveJ pHome,v1000,fine,tGripper; ENDIF ENDPROC FUNC bool CurrentPos(robtarget ComparePos,INOUT tooldata TCP) VAR num Counter:=0; VAR robtar

20、get ActualPos; ActualPos:=CRobT(Tool:=TCPWObj:=wobj0); IF ActualPos.trans.xComparePos.trans.x-25 AND ActualPos.trans.xComparePos.trans.y-25 AND ActualPos.trans.yComparePos.trans.z-25 AND ActualPos.trans.zComparePos.rot.q1-0.1 AND ActualPos.rot.q1ComparePos.rot.q2-0.1 AND ActualPos.rot.q2ComparePos.r

21、ot.q3-0.1 AND ActualPos.rot.q3ComparePos.rot.q4-0.1 AND ActualPos.rot.q4ComparePos.rot.q4+0.1 Counter:=Counter+1; RETURN Counter=7; ENDFUNC TRAP tEjectPallet_L Reset do02_PalletFull_L; bPalletFull_L:=FALSE; ENDTRAP TRAP tEjectPallet_R Reset do03_PalletFull_R; bPalletFull_R:=FALSE; ENDTRAP PROC rMove

22、Absj() MoveAbsJ jposHomeNoEOffs, v100, fine, tGripperWObj:=wobj0; ENDPROC PROC rModPos() MoveL pHome,v100,fine,tGripperWObj:=Wobj0; MoveL pPick_L,v100,fine,tGripperWObj:=Wobj0; MoveL pPick_R,v100,fine,tGripperWObj:=Wobj0; MoveL pPlaceBase0_L,v100,fine,tGripperWObj:=WobjPallet_L; MoveL pPlaceBase90_L,v100,fine,tGripperWObj:=WobjPallet_L; MoveL pPlaceBase0_R,v100,fine,tGripperWObj:=WobjPallet_R; MoveL pPlaceBase90_R,v100,fine,tGripperWObj:=WobjPallet_R; ENDPROC