码垛工作站案例分析

最新推荐文章于 2024-08-26 08:59:00 发布

原创最新推荐文章于 2024-08-26 08:59:00 发布 · 912 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#数据库 #java #开发语言

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],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 robtarget
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.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,-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 pPickSafe;
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:=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];
PERS speeddata vMinLoad:=[1000,200,6000,1000];
PERS speeddata vMidLoad:=[2500,500,6000,1000];
PERS speeddata vMaxLoad:=[4000,500,6000,1000];
PERS num
Compensation{15,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 bReady THEN
rPick;
rPlace;
ENDIF
rCycleCheck;

3
ENDWHILE
ENDPROC
5.其他例行程序
PROC rInitAll()
rCheckHomePos;
ConfL\OFF;
ConfJ\OFF;
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;
ClkReset Timer1;
TriggEquip HookAct,100,0.1\DOp:=do01_HookAct,1;
TriggEquip HookOff,100\Start,0.1\DOp:=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,iPallet_R;
ENDPROC
PROC rPick()
ClkReset Timer1;
ClkStart Timer1;
rCalPosition;
MoveJ Offs(pPick,0,0,nPickH),vMaxEmpty,z50,tGripper\WObj:=wobj0;
MoveL pPick,vMinLoad,fine,tGripper\WObj:=wobj0;
Set do00_ClampAct;
Waittime 0.3;
GripLoad LoadFull;
TriggL Offs(pPick,0,0,nPickH),vMinLoad,HookAct,z50,tGripper\WObj:=wobj0;
MoveL pPickSafe,vMaxLoad,z100,tGripper\WObj:=wobj0;
ENDPROC
PROC rPlace()
MoveJ Offs(pPlace,0,0,nPlaceH),vMaxLoad,z50,tGripper\WObj:=CurWobj;
TriggL pPlace,vMinLoad,HookOff,fine,tGripper\WObj:=CurWobj;
Reset do00_ClampAct;
Waittime 0.3;
GripLoad Load0;
MoveL Offs(pPlace,0,0,nPlaceH),vMinEmpty,z50,tGripper\WObj:=CurWobj;
rPlaceRD;
MoveJ pPickSafe,vMaxEmpty,z50,tGripper\WObj:=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 Left 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.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(nCount_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);
bGetPosition:=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<=5 THEN
pPickSafe:=Offs(pPick,0,0,400);
ELSEIF nCount>=6 AND nCount<=10 THEN
pPickSafe:=Offs(pPick,0,0,600);
ELSEIF nCount>=11 AND nCount<=15 THEN
pPickSafe:=Offs(pPick,0,0,800);
ENDIF
TEST nCount
CASE 1:
pTarget.trans.x:=pPlaceBase0.trans.x;
pTarget.trans.y:=pPlaceBase0.trans.y;
pTarget.trans.z:=pPlaceBase0.trans.z;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 2:
pTarget.trans.x:=pPlaceBase0.trans.x+nBoxL;
pTarget.trans.y:=pPlaceBase0.trans.y;
pTarget.trans.z:=pPlaceBase0.trans.z;
pTarget.rot:=pPlaceBase0.rot;
pTarget.robconf:=pPlaceBase0.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 3:
pTarget.trans.x:=pPlaceBase90.trans.x;
pTarget.trans.y:=pPlaceBase90.trans.y;
pTarget.trans.z:=pPlaceBase90.trans.z;
pTarget.rot:=pPlaceBase90.rot;
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,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.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,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;

6
pTarget.robconf:=pPlaceBase90.robconf;
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,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:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,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(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,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,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,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,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,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,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,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,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,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;

7
pTarget:=Offs(pTarget,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,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,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 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,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
CASE 15:
pTarget.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,Compensation{nCount,1},Compensation{nCount,2},Compensation{nCount,3});
DEFAULT:
TPErase;
TPWrite "The data 'nCount' is error,please check it !";
stop;
ENDTEST
Return pTarget;
ENDFUNC
PROC rPlaceRD()
TEST nPalletNo
CASE 1:
Incr nCount_L;
IF nCount_L>15 THEN
Set do02_PalletFull_L;
bPalletFull_L:=TRUE;
nCount_L:=1;
ENDIF
CASE 2:
Incr nCount_R;
IF nCount_R>15 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 pActualPos;
IF NOT CurrentPos(pHome,tGripper) THEN
pActualpos:=CRobT(\Tool:=tGripper\WObj:=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 robtarget ActualPos;
ActualPos:=CRobT(\Tool:=TCP\WObj:=wobj0);
IF ActualPos.trans.x>ComparePos.trans.x-25 AND ActualPos.trans.x<ComparePos.trans.x+25 Counter:=Counter+1;
IF ActualPos.trans.y>ComparePos.trans.y-25 AND ActualPos.trans.y<ComparePos.trans.y+25 Counter:=Counter+1;
IF ActualPos.trans.z>ComparePos.trans.z-25 AND ActualPos.trans.z<ComparePos.trans.z+25 Counter:=Counter+1;
IF ActualPos.rot.q1>ComparePos.rot.q1-0.1 AND ActualPos.rot.q1<ComparePos.rot.q1+0.1 Counter:=Counter+1;
IF ActualPos.rot.q2>ComparePos.rot.q2-0.1 AND ActualPos.rot.q2<ComparePos.rot.q2+0.1 Counter:=Counter+1;
IF ActualPos.rot.q3>ComparePos.rot.q3-0.1 AND ActualPos.rot.q3<ComparePos.rot.q3+0.1 Counter:=Counter+1;
IF ActualPos.rot.q4>ComparePos.rot.q4-0.1 AND ActualPos.rot.q4<ComparePos.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 rMoveAbsj()
MoveAbsJ jposHome\NoEOffs, v100, fine, tGripper\WObj:=wobj0;
ENDPROC
PROC rModPos()
MoveL pHome,v100,fine,tGripper\WObj:=Wobj0;
MoveL pPick_L,v100,fine,tGripper\WObj:=Wobj0;
MoveL pPick_R,v100,fine,tGripper\WObj:=Wobj0;
MoveL pPlaceBase0_L,v100,fine,tGripper\WObj:=WobjPallet_L;
MoveL pPlaceBase90_L,v100,fine,tGripper\WObj:=WobjPallet_L;
MoveL pPlaceBase0_R,v100,fine,tGripper\WObj:=WobjPallet_R;
MoveL pPlaceBase90_R,v100,fine,tGripper\WObj:=WobjPallet_R;
ENDPROC