MODULE MainModule
!**************************************************************************************************************************************
!Data declarations
!**************************************************************************************************************************************
!User data
RECORD UserData
num EnterLimit;
num ExitLimit;
pos ItemOffset;
string VisionFile;
ENDRECORD
!User adjust data
PERS UserData setData:=[-350,350,[10,15,142],"2.job"];
PERS UserData setData1:=[-350,350,[5,15,160],"1.job"];
PERS UserData setData2:=[-350,350,[10,15,142],"2.job"];
PERS UserData setData3:=[-350,350,[15,20,160],"5.job"];
PERS UserData setData4:=[-350,350,[0,0,160],"4.job"];
!Work object data
PERS wobjdata WObjPick:=[FALSE,FALSE,"CNV1",[[0,0,0],[1,0,0,0]],[[-28.7972,-42.129,0],[1,0,0,0]]];
!Stoppoint data
VAR stoppointdata spdPick:=[3,FALSE,[0,0,0,0],0,0.03,"",0,0];
VAR stoppointdata spdPlace:=[3,TRUE,[0,0,0,0],0,0.01,"",0,0];
!Index data(conveyor number)
VAR num PickIndex:=1;
VAR num PlaceIndex:=3;
PERS bool bPlace180:=FALSE;
PERS num nPlaceType:=1;
PERS num npiancha;
!
VAR intnum intno1:=0;
CONST robtarget pHome10:=[[-318.69,-202.31,-1266.84],[0,0.999481,0.032214,0],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,467.289,0]];
CONST robtarget pPrePick10:=[[-23.08,-25.67,-1130],[0,0.839508,-0.543347,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,15.46]];
VAR triggdata trigg1;
VAR triggdata trigg2;
VAR triggdata trigg3;
VAR triggdata trigg4;
VAR triggdata trigg5;
VAR triggdata trigg6;
VAR triggdata trNewTgtReq;
VAR triggdata trPos;
VAR triggdata triggpick;
PERS stoppointdata stoppoint1:=[3,TRUE,[0.1,100,0,0.1],0,0.05,"",0,0];
PERS stoppointdata stoppoint2:=[3,TRUE,[0.1,100,0,0.1],0,0.05,"",0,0];
PERS stoppointdata stoppoint3:=[3,TRUE,[0.1,100,0,0.1],0,0.05,"",0,0];
VAR intnum intNewTgtReq;
VAR intnum intPos;
VAR intnum inStopCnv2;
VAR intnum inBoxShift;
VAR intnum inPartLost;
VAR intnum inNewBox;
VAR intnum instopBox;
VAR intnum in2box;
VAR bool bPartLost:=FALSE;
VAR bool pick_ok:=FALSE;
VAR bool bchangeboxtepy1:=FALSE;
PERS speeddata vMinLoad_1:=[1600,500,6000,1000];
PERS speeddata vMinLoad_2:=[1300,500,6000,1000];
PERS num nbox:=6;
PERS num nOffsX:=-15;
PERS num nspare:=6;
PERS num nBoxInReject:=1;
PERS num nBoxStatus{3}:=[1,1,1];
PERS num nAccration:=80;
PERS num nAccration1:=80;
PERS num c2BarCount:=1;
PERS num c2BarPos{10,2}:=[[439.7,433.5],[430.5,430.5],[463.1,463.1],[364.9,365],[439.1,439],[367.4,367.5],[403.6,403.5],[366.4,366.5],[440.2,440.3],[364.4,364.5]];
PERS num PreBlowTime:=0.01;
PERS num nInterval:=125;
PERS num nBoxType:=2;
PERS robtarget pCurPos:=[[-69.6982,22.999,-1036.2],[0,0.999225,-0.039364,0],[0,4,0,0],[9E+09,9E+09,9E+09,9E+09,450.969,0]];
!CONST robtarget pPrePlace20:=[[-2.22,0.07,-1083.02],[0,0.996792,-0.0800409,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pPrePlace30:=[[895.00,-0.55,0.00],[0.000398845,0.981003,0.193931,-0.00497001],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,226.86,-145.038]];
CONST robtarget p10:=[[47.77,158.35,-1294.03],[0,0.671035,-0.741426,0],[0,1,0,0],[9E+09,9E+09,9E+09,9E+09,441.459,0]];
CONST robtarget p20:=[[48.44,37.68,-1130.60],[0,0.885995,-0.463695,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST speeddata vmaxspeed:=[3000,500,5000,1000];
CONST speeddata vloadspeed:=[1600,500,5000,1000];
CONST robtarget p30:=[[-87.34,-164.14,-1113.69],[0,0.999233,-0.0391604,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,0,0]];
VAR robtarget pPlaceRD:=[[-317.05,-170.58,-1117.96],[0,0.999977,-0.00676505,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,434.419,0]];
CONST robtarget pDengDai:=[[-69.70,23.00,-1036.20],[0,0.999225,-0.039364,0],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,453.559,1.61357]];
!********************************************************************
! Program Factory IRB 660/2.5-150 !
! (c) ABB Engineering (Shanghai) Ltd. !
! No.31 Industrial Building 1/F, Fu Te Dong San Road, !
! Waigaoqiao Free Trade Zone,Shanghai,P.R. China. !
! Hotline :021-61056117 !
! Fax :021-61056902 !
! Post code:200131 !
! Programmer : . !
! Last modified: 20080219 !
! By : Frank wanyan !
! Reason : Installation !
!!********************************************************************
!******************************************************************
! Procedure: rInitAll
! Description: Reset I/O Signal
! Reset number data and interrupt
!******************************************************************
!**************************************************************************************************************************************
! Description:
! This procedure sets up initial values for conveyor tracking and related unit
!**************************************************************************************************************************************
PROC Init()
Reset SoAutoStop;
IF nBoxType=1 then
setdata:=setdata1;
SetupTrack PickIndex,"VISION","DISTANCE"\TrigDist:=100\DistFilter:=130\TgtOffset:=setData.ItemOffset;
elseIF nBoxType=2 then
setdata:=setdata2;
SetupTrack PickIndex,"VISION","DISTANCE"\TrigDist:=100\DistFilter:=130\TgtOffset:=setData.ItemOffset;
elseIF nBoxType=3 then
setdata:=setdata3;
SetupTrack PickIndex,"VISION","DISTANCE"\TrigDist:=150\DistFilter:=250\TgtOffset:=setData.ItemOffset;
elseIF nBoxType=4 then
setdata:=setdata4;
SetupTrack PickIndex,"VISION","DISTANCE"\TrigDist:=100\DistFilter:=130\TgtOffset:=setData.ItemOffset;
endif
!Define work area for conveyor 2 which is also the conveyor with vision system
SetupWorkArea PickIndex,setData.EnterLimit,setData.ExitLimit;
!Set up vision system
SetupVision setData.VisionFile;
SingArea\Wrist;
TPErase;
ENDPROC
!**************************************************************************************************************************************
! Description:
! Pick routine
!**************************************************************************************************************************************
PROC Pick(num Index)
ConfL\Off;
GetTargets TrckSource{Index},PickTarget{Index};
WObjPick:=TrckSource{Index}.Wobj;
!IF nBoxTypye=3 THEN
! PickTarget{Index}.rbTarget:=RelTool(PickTarget{Index}.rbTarget,0,0,0\Rz:=90);
!ENDIF
triggL offs(PickTarget{Index}.rbTarget,0,0,100),vloadspeed,triggpick,z10,tGriper\WObj:=WObjPick;
! TriggL\Conc,PickTarget{Index}.rbTarget,LowSpeed,trGripON,z5\Inpos:=spdPick,tGriper\WObj:=WObjPick;
MoveL Offs(PickTarget{Index}.rbTarget,0,0,0),vloadspeed,fine,tGriper\WObj:=WObjPick;
!GripLoad lod100ml;
MoveL offs(PickTarget{Index}.rbTarget,0,0,30),vloadspeed,z20,tGriper\WObj:=WObjPick;
TriggL\Conc,offs(PickTarget{Index}.rbTarget,0,0,40),vloadspeed,trAckPick{Index},z10,tGriper\WObj:=WObjPick;
!MoveL offs(PickTarget{Index}.rbTarget,0,0,140),speed1,z50,tGriper\WObj:=WObjPick;
ENDPROC
PROC InPick()
B:
!TPReadNum nBoxType,"please input the type(1-3)";
TPReadFK nBoxType,"qing xuan ze chan pin lei xing?","300g","200g","D500g","X500g","";
TEST nBoxType
CASE 1:
nCountInBox:=50;
ppick:=Offs(p30,npick1x,npick1y,npick1Z);
CASE 2:
nCountInBox:=80;
ppick:=offs(p30,npick2X,npick2Y,npick2Z);
CASE 3:
nCountInBox:=30;
ppick:=offs(ppick1,npick3x,npick3y,npick3z);
CASE 4:
nCountInBox:=20;
ppick:=offs(ppick1,npick3x,npick3y,npick3z);
DEFAULT:
TPWrite "lei xing cuo wu";
GOTO B;
Stop;
ENDTEST
pPickrd:=RelTool(pPick,0,0,0\Rz:=95);
pPickrd:=Offs(pPickrd,0,0,60);
ENDPROC
PROC rInitAll1()
rMoveHome;
VelSet 100,10000;
AccSet 100,100;
nP1Count:=1;
nS1Count:=0;
Reset DOpick;
c2BarCount:=1;
!bBoxFull:=TRUE;
ActUnit CNV1;
ActUnit CNV2;
inPick;
IDelete inStopbox;
CONNECT inStopbox WITH tStopbox;
ISignalAI c2Position,AIO_ABOVE_HIGH,0.42,0,0,inStopbox;
ConfL\Off;
Set DOStartCnv2;
Reset DOBoxFull;
ActUnit CNV2;
DropWObj wobjcnv2;
Set DOStartCnv2;
Bb:
WaitUntil c2ObjectsInQ>0\MaxTime:=1\Timeflag:=TimeOut;
IF TimeOut THEN
GOTO Bb;
endif
WaitWObj wobjcnv2;
!
WaitTime 5;
ISleep instopBox;
endproc
PROC rInitAll()
VAR num nStepCount;
Reset DO10_7;
! shu song lian chu shi hua
!IDelete in2box;
!CONNECT in2box WITH tinbox;
!ISignalDI DI10_3_yuandian,0,in2box;
!ISleep in2box;
IF debug=TRUE SpyStart "HOME:/spy.log";
ConfJ\On;
ConfL\On;
rMoveHome;
bRobBusy:=FALSE;
bPickPart:=FALSE;
bOFEnd:=FALSE;
TimeOut:=FALSE;
bPartLost:=FALSE;
!
!rSetLoad;
ClkStop Timer;
ClkReset Timer;
!
!number
nbox:=1;
nInput:=0;
ncode:=0;
nPassword:=0;
nCycleTime:=0;
!motion setting
VelSet 100,10000;
AccSet 100,100;
!ActUnit CNV2;
nP1Count:=1;
nS1Count:=0;
Reset DOpick;
c2BarCount:=1;
!bBoxFull:=TRUE;
ActUnit CNV1;
ActUnit CNV2;
bBoxFull:=FALSE;
!while DI10_3_yuandian=0 DO
!Set DO10_7;
!ENDWHILE
Reset DO10_7;
inPick;
Init;
!IWatch in2box;
!Ca:
!Set DOStartCnv2;
!WHILE nxiangzi<2 DO
! GOTO ca;
!ENDWHILE
!Reset DOStartCnv2;
! ISleep in2box;
!MoveL Offs(pPick,0,0,60),speed1,z50,tGriper;
!QInit;
!
DropWObj wobjcnv2;
InitTriggs;
ConfL\Off;
Set DOStartCnv2;
Reset DOBoxFull;
!! WaitWObj wobjcnv2\MaxTime:=1\Timeflag:=TimeOut;
!WaitUntil c2Position > 0.5\MaxTime:=1\TimeFlag:=TimeOut;
TPWrite "Waiting for boxes";
!! WaitTime 1;
nStepCount:=1;
!Reset DOBoxErr;
!PulseDO\PLength:=0.5, DOBoxErr;
WaitWObj wobjcnv2;
!
Set DOStartCnv1;
!
FOR i FROM 9 TO 1 DO
nCheckPak{i+1}:=nCheckPak{i};
ENDFOR
FOR i FROM 9 TO 1 DO
nCheckPak{i+1}:=nCheckPak{i};
ENDFOR
nCheckPak{1}:=0;
nCheckPak{2}:=1;
nCheckPak{3}:=1;
ENDPROC
PROC InitTriggs()
!
TriggEquip trigg1,20,0\GOp:=goVacBlow1,1;
TriggEquip trigg2,20,0\GOp:=goVacBlow1,3;
TriggEquip trigg3,0,PreBlowTime\GOp:=goVacBlow1,12;
TriggEquip trigg4,10\Start,-0.15\GOp:=goVacBlow1,0;
TriggEquip trigg5,0,0.2\GOp:=goVacBlow1,0;
TriggEquip trigg6,10,0\GOp:=goVacBlow1,4;
TriggEquip triggpick,30,0\GOp:=GOVACBLOW1,1;
!CONNECT intNewTgtReq WITH DropCnv1;
!TriggInt trNewTgtReq, 25\Start, intNewTgtReq;
!CONNECT intPos WITH tGetPos;
!TriggInt trPos, 5, intPos;
IDelete iCheckPak;
CONNECT iCheckPak WITH tCheckPak;
TriggCheckIO trigg5,25,dilost,EQ,1,iCheckPak;
IDelete inBoxShift;
CONNECT inBoxShift WITH tBoxOut;
ISignalAI c2Position,AIO_OUTSIDE,0,0,0.3\DNeg,inBoxShift;
ISleep inBoxShift;
IDelete inNewBox;
CONNECT inNewBox WITH tNewBox;
ISignalGI c2ObjectsInQ,inNewBox;
ISleep inNewBox;
IDelete inStopbox;
CONNECT inStopbox WITH tStopbox;
ISignalDI DI10_3_yuandian,1,instopBox;
!ISignalAI c2Position,AIO_ABOVE_HIGH,0.42,0.41,0,inStopbox;
IWatch instopbox;
IDelete inStopCnv2;
CONNECT inStopCnv2 WITH tStopCnv2;
ISignalAI\Single,c2Position,AIO_ABOVE_HIGH,1,0,0,inStopCnv2;
ENDPROC
!******************************************************************
! Procedure: SetPlace
! Description: Set Place data
!******************************************************************
PROC rSetLoad()
IF bPickPart=False THEN
GripLoad LoadEmpty;
else
GripLoad LoadFull;
ENDIF
ENDPROC
!******************************************************************
! Procedure: MoveHome
! Description: Move to home position
!******************************************************************
PROC rMoveHome()
ActualPos:=CRobT(\Tool:=tGriper\WObj:=wobj0);
ActualPos.trans.z:=pHome.trans.z;
MoveL ActualPos,v500,z10,tGriper;
MoveL pHome,v500,fine,tGriper;
SetGO goVacBlow1,12;
WaitTime 1;
SetGO goVacBlow1,0;
MoveL pHome,v500,fine,tGriper;
ENDPROC
PROC rPick1()
!IF DIpick=1 and distop=0 THEN
Pa:
!MoveL Offs(pPick,0,0,-5),speed1,z50,tGriper;
!TriggL Offs(pPick,0,0,-19),speed1,triggpick,fine,tGriper;
!WaitTime 0.2;
MoveL Offs(pPick,0,0,80), vloadspeed, z20, tGriper;
SetGO goVacBlow1,1;
MoveL Offs(pPick,0,0,-5),vloadspeed,z50,tGriper;
MoveL Offs(pPick,0,0,80),vloadspeed,z20,tGriper;
! IF DI10_9=0 OR DI10_10=0 THEN
! MoveL offs(pPick,180,0,180),speed2,z20,tGriper;
! setgo goVacBlow1,0;
! PulseDO\PLength:=0.2,DOpick;
! GOTO pa;
! ENDIF
PulseDO\PLength:=0.2,DOpick;
!ENDIF
endproc
!******************************************************************
! Procedure: PickPart
! Description: Pick part from infeed
!******************************************************************
proc rPlace()
VAR bool bFlag:=FALSE;
VAR num nWaitUpdate:=0;
!ActUnit CNV2;
rCalPos;
IF bBoxFull=TRUE THEN
!IF c2ObjectsInQ<1 AND bPartLost THEN
IF c2ObjectsInQ<1 THEN
!IDelete inPartLost;
!CONNECT inPartLost WITH tPartLost;
!ISignalDI\Single, c2Connected, 1, inPartLost;
nWaitUpdate:=0.25;
ENDIF
!bPartLost:=FALSE;
!WaitTime 0.1;
!! WaitWObj wobjcnv2\RelDist:=100\MaxTime:=0.3\Timeflag:=bFlag;
c2BarPos{c2BarCount,2}:=Round(c2Position*1000\Dec:=1);
Set DOStartCnv2;
IF c2position<0.1 THEN
DropWobj wobjcnv2;
WaitUntil c2Connected=0;
Set DOStartCnv2;
Reset DOBoxFull;
PulseDO\PLength:=0.5,DOBoxFull;
WaitUntil c2ObjectsInQ>0\MaxTime:=0.2\Timeflag:=bFlag;
!WaitWObj wobjcnv2\MaxTime:=0.2\Timeflag:=bFlag;
IF bFlag THEN
!MoveL offs(pPickS,-nNextObj,0,nPickH), vMinLoad, z5, tGriper\WObj:=wobjcnv1;
WaitTime\Inpos,0;
pCurPos:=CRobT(\Tool:=tGriper\WObj:=wobj0);
MoveL pCurPos,vMinLoad,fine,tGriper;
DropWobj WobjCnv1;
ErrWrite\W,"Waiting for box","Waiting fox box conveyor";
!
WaitBoxCnv wobjcnv2;
ENDIF
ELSE
WaitTime 0.05;
DropWobj wobjcnv2;
WaitUntil c2Connected=0;
ENDIF
!
WaitTime nWaitUpdate;
WaitWObj wobjcnv2;
!WaitWObj wobjcnv2\RelDist:=100;
!! WaitBoxCnv wobjcnv2\RelDist:=100;
bBoxFull:=FALSE;
IDelete inStopCnv2;
CONNECT inStopCnv2 WITH tStopCnv2;
ISignalAI\Single,c2Position,AIO_ABOVE_HIGH,0.42,0,0,inStopCnv2;
ENDIF
! IF c2Connected = 0 THEN
! ErrWrite\W, "CNV2 not connected", "c2connected=0 for unknown reason";
!
! Aa:
! WaitWObj wobjcnv2\MaxTime:=0.3\Timeflag:=bFlag;
! IF bFlag THEN
! PulseDO\PLength:=0.2, DOBoxFull;
! GOTO Aa;
! ENDIF
! ENDIF
! below comment by alan 2012/06/01
!IF c2Position<0.3 AND c2NullSpeed=1 AND nS1Count>4 THEN
!ErrWrite\W, "W04: Wrong Box Stop Position", "CNV2 restarted by robot"
!\RL2:="c2Position="+ValToStr(AInput(c2Position))+" c2Speed="+ValToStr(AInput(c2Speed));
!Reset DOBoxFull;
!PulseDO\PLength:=0.4, DOBoxFull;
!
!modify by alan 2012/06/01,to avoid robot working out of work area
IF c2Position<0.3 AND c2NullSpeed=1 AND nCountInBox-nS1Count<=nspare THEN
W04:
!ErrWrite\W, "W04: Wrong Box Stop Position", "CNV2 restarted by robot";
!\RL2:="c2Position="+ValToStr(AInput(c2Position))+" c2Speed="+ValToStr(AInput(c2Speed));
Reset DOBoxFull;
PulseDO\PLength:=0.4,DOBoxFull;
ELSEIF c2Position>=0.6 THEN
ErrWrite "Box too far","the position of box is too far on CNV2"\RL2:="c2Position="+ValToStr(AInput(c2Position))\RL3:="nS1Count="+ValToStr(nS1Count);
Set DOStartCnv2;
DropWobj wobjcnv2;
WaitUntil c2Connected=0\MaxTime:=0.4\Timeflag:=bFlag;
IF bFlag THEN
ErrWrite\W,"CNV2 can't dropped","c2connected=1 for unknown reason";
Reset c2DropWObj;
WaitTime 0.1;
Set c2DropWObj;
! IF c2NullSpeed=0 PulseDO ;
!PulseDO\PLength:=0.5, DOBoxErr;
WaitTime 0.3;
Reset c2DropWObj;
ENDIF
nP1Count:=1;
nS1Count:=0;
rCalPos;
!! WaitWObj wobjcnv2\RelDist:=100\MaxTime:=0.3\Timeflag:=bFlag;
WaitWObj wobjcnv2\MaxTime:=0.3\Timeflag:=bFlag;
IF bFlag THEN
!MoveL offs(pPickS,-nNextObj,0,nPickH), vMinLoad, z5, tGriper\WObj:=wobjcnv1;
WaitTime\Inpos,0;
pCurPos:=CRobT(\Tool:=tGriper\WObj:=wobj0);
MoveL pCurPos,vMinLoad,fine,tGriper;
DropWobj WobjCnv1;
ErrWrite\W,"Waiting for CNV2","Waiting fox box conveyor";
!
WaitBoxCnv wobjcnv2;
ENDIF
!WaitWObj wobjcnv2\RelDist:=100;
!! WaitBoxCnv wobjcnv2\RelDist:=100;
bBoxFull:=FALSE;
IDelete inStopCnv2;
CONNECT inStopCnv2 WITH tStopCnv2;
ISignalAI\Single,c2Position,AIO_ABOVE_HIGH,0.6,0,0,inStopCnv2;
!Stop;
!ExitCycle;
ENDIF
!
!MoveL pPrePlace, vMidLoad, z10, tGriper\WObj:=wobjcnv2;
!
!TriggL pPlace, vMinLoad, trigg3\T2:=trNewTgtReq, z5\Inpos:=stoppoint2, tGriper\WObj:=Wobjcnv2;
!IF pPlace.trans.x<pPlace10.trans.x WaitBoxCnv wobjcnv2\RelDist:=100;
!TriggL Offs(pPlace,0,0,50), vMidLoad, trigg5, z10, tGriper\WObj:=Wobjcnv2;
!MoveL Offs(pPrePlace,0,0,60),speed2,z10,tGriper\WObj:=wobjcnv2;
IF c2Position>0.4 THEN
npiancha:=(c2Position-0.43)*1000;
ELSE
npiancha:=0;
ENDIF
moveL Offs(pPlacerd,0,0,140),vloadspeed,z20,tGriper\WObj:=Wobjcnv2;
IF DI10_11=0 THEN
nbox:=nbox-1;
nS1Count:=nS1Count-1;
ENDIF
moveL pPlaceRD,vloadspeed,z20,tGriper\WObj:=Wobjcnv2;
moveL Offs(pPlace,npiancha+0,0,60),vloadspeed,fine,tGriper\WObj:=Wobjcnv2;
triggL Offs(pPlace,npiancha+0,0,0),vloadspeed,trigg6,fine,tGriper\WObj:=Wobjcnv2;
!TriggL Offs(pPlace,npiancha+0,0,0),speed3,trigg5,fine,tGriper\WObj:=Wobjcnv2;
!SetGO goVacBlow1,4;
WaitTime 0.1;
SetGO goVacBlow1,0;
! TriggL Offs(pPlace,0,0,40),vMinLoad_1,trigg5,z10,tGriper\WObj:=Wobjcnv2;
! TriggL Offs(pPlace,0,0,0),vMinLoad_2,trigg3,z5\Inpos:=stoppoint2,tGriper\WObj:=Wobjcnv2;
AccSet 100,100;
bPickPart:=FALSE;
!rsetLoad;
!WaitTime time;
!TriggL\Conc,pPrePlace,vMaxEmpty,trigg4,z10,tGriper\WObj:=wobjcnv2;
moveL Offs(pPlace,0,0,250),vloadspeed,z20,tGriper\WObj:=Wobjcnv2;
moveL pDengDai,vloadspeed,z20,tGriper\WObj:=wobj0;
!! IF nP1Count > 2 AND c2Speed = 0 AND c2Position > 0.5 AND nCheckPak{nEject} = 1 THEN
IF nP1Count>2 AND c2Speed=0 AND c2Position>0.3 AND nCheckPak{nEject}=1 THEN
!PulseDO\PLength:=2, do5_ServoReset;
nCheckPak{nEject}:=0;
ENDIF
rPlaceRD;
ERROR
IF ERRNO=ERR_CNV_DROPPED THEN
ErrWrite\W,"Object dropped on CNV2","the object has dropped";
RETRY;
ELSE
ErrWrite\W,"Error on CNV2","Unknown reason";
RETRY;
ENDIF
ENDPROC
PROC WaitBoxCnv(PERS wobjdata wobjcnv\num RelDist)
VAR bool bFlag:=TRUE;
VAR num nWaitDistance;
IF Present(RelDist) THEN
WaitWObj wobjcnv\RelDist:=RelDist\MaxTime:=0.1\Timeflag:=bFlag;
nWaitDistance:=RelDist;
ELSE
WaitUntil c2ObjectsInQ>0\MaxTime:=0.1\Timeflag:=bFlag;
!WaitWObj wobjcnv\MaxTime:=0.1\Timeflag:=bFlag;
nWaitDistance:=0;
ENDIF
IF bFlag AND c2NullSpeed=1 THEN
ErrWrite\W,"Box belt Stopped","Waiting fox box conveyor start"\RL2:=" current c2Position="+ValToStr(AInput(c2Position)*1000)\RL3:=" Waiting position to "+ValToStr(nWaitDistance);
ENDIF
WHILE bFlag DO
IF c2NullSpeed=1 THEN
Set DOStartCnv2;
Reset DOBoxFull;
PulseDO\PLength:=0.4,DOBoxFull;
ENDIF
IF Present(RelDist) THEN
WaitWObj wobjcnv\RelDist:=RelDist\MaxTime:=1\Timeflag:=bFlag;
ELSE
WaitUntil c2ObjectsInQ>0\MaxTime:=1\Timeflag:=bFlag;
!WaitWObj wobjcnv\MaxTime:=1\Timeflag:=bFlag;
ENDIF
ENDWHILE
ERROR
IF ERRNO=ERR_CNV_DROPPED THEN
ErrWrite\W,"Object dropped on CNV2","the object has dropped";
RETRY;
ELSEIF ERRNO=ERR_CNV_CONNECT THEN
TRYNEXT;
ELSE
ErrWrite\W,"Error on CNV2","Unknown reason";
RETRY;
ENDIF
ENDPROC
PROC rPlaceRD()
nS1Count:=nS1Count+1;
nP1Count:=nP1Count+1;
IF nS1Count>=nCountInBox THEN
IDelete inStopCnv2;
bBoxFull:=TRUE;
IF c2ObjectsInQ<1 THEN
!ErrWrite\W, "Miss Next Box", "Objects In Queue is "+ValToStr(GInput(c2ObjectsInQ))
!\RL2:="c2Position="+ValToStr(AInput(c2Position))+" c2Speed="+ValToStr(AInput(c2Speed));
ENDIF
!IF c2Position>0.3 AND c2NullSpeed=1 THEN
!IF c2Position>0.3 THEN
IF c2ObjectsInQ=1 THEN
Incr c2BarCount;
IF c2BarCount>10 c2BarCount:=1;
c2BarPos{c2BarCount,1}:=Round(c2Position*1000\Dec:=1);
ENDIF
!! Set DOStartCnv2;
!! Reset DOBoxFull;
!! PulseDO\PLength:=0.5, DOBoxFull;
nP1Count:=1;
nS1Count:=0;
nBoxCount:=nBoxCount+nCountInBox;
FOR i FROM 9 TO 1 DO
nCheckPak{i+1}:=nCheckPak{i};
ENDFOR
nCheckPak{1}:=0;
ENDIF
nCount:=nCount+2;
IF nCount>9E+06 nCount:=0;
IF nBoxCount>9E+06 nBoxCount:=0;
ENDPROC
PROC BoxType300g()
!1-40
IF nS1Count<40 THEN
IF nbox=1 THEN
pPlace:=Offs(pplace10,nxoff1,nyoff1-30,0);
nbox:=2;
ELSEIF nbox=2 then
pPlace:=Offs(pplace10,nxoff1,nyoff1+nBoxW1+12,0);
nbox:=3;
ELSEIF nbox=3 then
pPlace:=Offs(pPlace10,nxoff1-nBoxL1+20,nyoff1-30,0);
nbox:=4;
ELSEIF nbox=4 then
pPlace:=Offs(pplace10,nxoff1-nBoxL1+20,nyoff1+nBoxW1+12,0);
nbox:=5;
ELSEIF nbox=5 then
pPlace:=Offs(pplace10,nxoff1-2*nBoxL1-17,nyoff1-35,0);
nbox:=6;
ELSEIF nbox=6 then
pPlace:=Offs(pplace10,nxoff1-2*nBoxL1-17,nyoff1+nBoxW1+7,0);
nbox:=7;
ELSEIF nbox=7 then
pPlace:=Offs(pplace10,nxoff1-3*nBoxL1,nyoff1-35,0);
nbox:=8;
ELSEIF nbox=8 then
pPlace:=Offs(pplace10,nxoff1-3*nBoxL1,nyoff1+nBoxW1+5,0);
nbox:=1;
endif
IF nbox<5 THEN
pPlaceRD:=Offs(pPlace10,-30,20,100);
ELSE
pPlaceRD:=Offs(pPlace10,-140,20,100);
ENDIF
ELSE
!****************41*********************
IF nbox=1 THEN
pPlace:=Offs(pplace10,nxoff1,nyoff1-35,0);
nbox:=3;
!****************42*********************
ELSEIF nbox=3 then
pPlace:=Offs(pPlace10,nxoff1-nBoxL1+17,nyoff1-35,0);
nbox:=5;
!****************43*********************
ELSEIF nbox=5 then
pPlace:=Offs(pplace10,nxoff1-2*nBoxL1-17,nyoff1-35,0);
nbox:=6;
!****************44*********************
ELSEIF nbox=6 then
pPlace:=Offs(pplace10,nxoff1-2*nBoxL1-17,nyoff1+nBoxW1+7,0);
nbox:=7;
!****************45*********************
ELSEIF nbox=7 then
pPlace:=Offs(pplace10,nxoff1-3*nBoxL1,nyoff1-35,0);
nbox:=8;
!****************46*********************
ELSEIF nbox=8 then
pPlace:=Offs(pplace10,nxoff1-3*nBoxL1,nyoff1+nBoxW1+5,0);
nbox:=9;
!****************47*********************
ELSEIF nbox=9 THEN
pPlace:=Offs(pplace10,nxoff1,nyoff1-35,0);
nbox:=10;
!****************48*********************
ELSEIF nbox=10 then
pPlace:=Offs(pPlace10,nxoff1-nBoxL1+17,nyoff1-35,0);
nbox:=11;
!****************49*********************
ELSEIF nbox=11 then
pPlace:=Offs(pplace10,nxoff1-2*nBoxL1-17,nyoff1+nBoxW1+7,0);
nbox:=12;
!****************50*********************
ELSEIF nbox=12 then
pPlace:=Offs(pplace10,nxoff1-3*nBoxL1,nyoff1+nBoxW1+5,0);
nbox:=1;
endif
IF nbox<5 OR nbox=9 or nbox=10 THEN
pPlaceRD:=Offs(pPlace10,-30,20,100);
ELSE
pPlaceRD:=Offs(pPlace10,-140,20,100);
ENDIF
ENDIF
ENDPROC
PROC BoxType200g()
!1-72
IF nS1Count<72 THEN
IF nbox=1 THEN
pPlace:=pplace200G;
nbox:=2;
ELSEIF nbox=2 then
pPlace:=Offs(pplace200G,0,n200GL,0);
nbox:=3;
ELSEIF nbox=3 then
pPlace:=Offs(pplace200G,-n200GW,0,0);
nbox:=4;
ELSEIF nbox=4 then
pPlace:=Offs(pplace200G,-n200GW,n200GL,0);
nbox:=5;
ELSEIF nbox=5 then
pPlace:=Offs(pplace200G,-N200GW*2-n200GZ,0,0);
nbox:=6;
ELSEIF nbox=6 then
pPlace:=Offs(pplace200G,-n200GW*2-n200GZ,n200GL,0);
nbox:=7;
ELSEIF nbox=7 then
pPlace:=Offs(pplace200G,-n200GW*3-n200GZ,0,0);
nbox:=8;
ELSEIF nbox=8 then
pPlace:=Offs(pplace200G,-n200GW*3-n200GZ,n200GL,0);
nbox:=1;
endif
IF nbox<5 THEN
pPlaceRD:=Offs(pPlace10,-30,20,100);
ELSE
pPlaceRD:=Offs(pPlace10,-140,20,100);
ENDIF
ELSEIF nS1Count>71 then
!***************73******************
IF nbox=1 THEN
!pPlace:=Offs(pplace10,nxoff1,nyoff1+nBoxW1+7,0);
pPlace:=Offs(pplace200G,0,n200GW,0);
nbox:=2;
!***************74******************
ELSEIF nbox=2 THEN
!pPlace:=Offs(pplace10,nxoff1-nBoxL1+17,nyoff1+nBoxW1+7,0);
pPlace:=Offs(pplace200G,-n200GW,n200GL,0);
nbox:=3;
!***************75******************
ELSEIF nbox=3 THEN
!pPlace:=Offs(pplace10,nxoff1-2*nBoxL1-17,nyoff1-35,0);
pPlace:=Offs(pplace200G,-n200GW-n200GZ,0,0);
nbox:=4;
!***************76******************
ELSEIF nbox=4 THEN
!pPlace:=Offs(pplace10,nxoff1-2*nBoxL1-17,nyoff1+nBoxW1+7,0);
pPlace:=Offs(pplace200G,-N200GW*2-n200GZ,n200GL,0);
nbox:=5;
!***************77******************
ELSEIF nbox=5 THEN
!pPlace:=Offs(pplace10,nxoff1-3*nBoxL1,nyoff1-35,0);
pPlace:=Offs(pplace200G,-n200GW*3-n200GZ,0,0);
nbox:=6;
!***************78******************
ELSEIF nbox=6 THEN
!pPlace:=Offs(pplace10,nxoff1-3*nBoxL1,nyoff1+nBoxW1+5,0);
pPlace:=Offs(pplace200G,-n200Gw*3-n200GZ,n200GL,0);
nbox:=7;
!***************79******************
ELSEIF nbox=7 THEN
! pPlace:=Offs(pplace10,nxoff1-nBoxL1+20,nyoff1+nBoxW1+7,0);
pPlace:=Offs(pplace200G,-n200Gw,n200Gl,0);
nbox:=8;
!***************80******************
ELSEIF nbox=8 THEN
!pPlace:=Offs(pplace10,nxoff1-3*nBoxL1,nyoff1-35,0);
pPlace:=Offs(pplace200G,-n200GL*3-n200GZ,0,0);
nbox:=1;
endif
IF nbox=1 OR nbox=2 OR nbox=7 THEN
pPlaceRD:=Offs(pPlace200G,-30,20,100);
ELSE
pPlaceRD:=Offs(pPlace200G,-140,20,100);
ENDIF
ENDIF
ENDPROC
PROC BoxTpyeD500g()
IF nbox=1 THEN
!pPlace:=Offs(pplace10,nxoff3-40,nyoff3,0);
pPlace:=pPlace30;
nbox:=2;
ELSEIF nbox=2 then
!pPlace:=Offs(pplace10,nxoff3-40,nyoff3+nBoxW1,0);
pPlace:=Offs(pPlace30,0,nD500gW,0);
nbox:=3;
ELSEIF nbox=3 then
!pPlace:=Offs(pplace10,nxoff3-nBoxL3-30,0,0);
pPlace:=Offs(pplace30,-nd500gL,0,0);
nbox:=4;
ELSEIF nbox=4 then
!pPlace:=Offs(pplace10,nxoff3-nBoxL3-30,nyoff3+nBoxW1,0);
pPlace:=Offs(pplace30,-nD500gL,nD500gW,0);
nbox:=1;
endif
!IF nbox<3 THEN
!pPlaceRD:=Offs(pplace30,-30,20,100);
!else
!pPlacerd:=Offs(pPlace30,-140,20,100);
!ENDIF
pPlacerd:=Offs(pPlace,0,0,nD500gH);
!pPlaceRD:=RelTool(pPlaceRD,0,0,0\Rz:=-95);
!pPlace:=RelTool(pPlace,0,0,0\Rz:=-95);
ENDPROC
PROC BoxTpyeX500g()
IF nbox=1 THEN
pPlace:=Offs(pplace10,nxoff3,nyoff3,0);
pPlaceRD:=Offs(pPlace,-10,10,30);
nbox:=2;
ELSEIF nbox=2 then
pPlace:=Offs(pplace10,nxoff3,nyoff3+nBoxW4,0);
pPlaceRD:=Offs(pPlace,-10,10,30);
nbox:=3;
ELSEIF nbox=3 then
pPlace:=Offs(pplace10,nxoff3-nBoxL4,0,0);
pPlaceRD:=Offs(pPlace,-10,10,30);
nbox:=4;
ELSEIF nbox=4 then
pPlace:=Offs(pplace10,nxoff3-nBoxL1,nyoff3+nBoxW1,0);
pPlaceRD:=Offs(pPlace,-10,10,30);
nbox:=1;
endif
IF nbox<3 THEN
pPlaceRD:=Offs(pplace10,-30,20,100);
else
pPlacerd:=Offs(pPlace10,-140,20,100);
ENDIF
ENDPROC
PROC rCalPos()
nDiv:=(nP1Count-1) DIV nRow;
nMod:=(nP1Count-1) MOD nRow;
!*****300g**************
IF nBoxType=1 THEN
Boxtype300g;
!*****200g***************
ELSEIF nBoxType=2 THEN
BoxType200g;
!***500g da******
ELSEIF nBoxType=3 THEN
BoxTpyed500g;
!***500g xiao****
ELSEIF nBoxType=4 THEN
BoxTpyex500g;
ENDIF
pPrePlace:=pPlace;
nOffsX:=-15;
pPrePlace.trans.x:=pPlace.trans.x+nOffsX;
pPrePlace.trans.z:=pPlace10.trans.z+270;
ENDPROC
!******************************************************************
! Procedure: Main
! Description: Program flow
!******************************************************************
PROC Main()
rInitAll;
WHILE TRUE DO
Pick PickIndex;
!text111;
rPlace;
ENDWHILE
ENDPROC
TRAP tCheckPak
nCheckPak{1}:=1;
!! PulseDO\PLength:=0.5, DOBoxErr;
TPWrite "One or two objects may lost";
ErrWrite\W,"Vacuum Error","One or two objects may lost";
nLost:=nLost+1;
!IF bPartLost RETURN;
IF c2ObjectsInQ=1 THEN
! WaitTime 0.2;
!Reset DOBoxErr;
!PulseDO\PLength:=0.5, DOBoxErr;
!ErrWrite\W, "Reject Signal Sent", "Box error signal was sent to PLC immediately";
nBoxStatus{2}:=0;
ELSE
!IDelete inPartLost;
!CONNECT inPartLost WITH tPartLost;
!ISignalGI\Single, c2ObjectsInQ, inPartLost;
!ISignalDI\Single, c2Connected, 1, inPartLost;
bPartLost:=TRUE;
ENDIF
!bPartLost:=TRUE;
ENDTRAP
TRAP tNewBox
!
TPWrite "7";
IF c2ObjectsInQ=0 THEN
RETURN ;
ELSEIF c2ObjectsInQ=1 THEN
nBoxInReject:=nBoxStatus{1};
IF nBoxStatus{1}=0 THEN
Reset DOBoxErr;
PulseDO\PLength:=0.5,DOBoxErr;
ErrWrite\W,"Reject Signal Sent","Box error signal was sent to PLC";
TPWrite "The next out box isn't full";
ELSE
TPWrite "The next out box is full";
ENDIF
nBoxStatus{1}:=nBoxStatus{2};
nBoxStatus{2}:=nBoxStatus{3};
nBoxStatus{3}:=1;
IF bPartLost THEN
nBoxStatus{2}:=0;
bPartLost:=FALSE;
ENDIF
ELSE
ErrWrite "Wrong Queue Length","c2ObjectsInQ:="+ValToStr(GInput(c2ObjectsInQ));
Stop;
Exit;
ENDIF
!
ENDTRAP
TRAP tPartLost
!WaitTime 0.5;
bPartLost:=FALSE;
Reset DOBoxErr;
PulseDO\PLength:=0.5,DOBoxErr;
ErrWrite\W,"Reject Signal Sent","Box error signal was sent to PLC now";
nBoxStatus{2}:=0;
TPWrite "6";
ENDTRAP
TRAP tStopCNV2
!
!IF NOT bBoxFull THEN
ErrWrite\W,"CNV2 Stopped by Prg","the position of box is too far on CNV2"\RL2:="c2Position="+ValToStr(AInput(c2Position))+" c2Speed="+ValToStr(AInput(c2Speed))\RL3:="nS1Count="+ValToStr(nS1Count)+" DOBoxFull="+ValToStr(DOutput(DOBoxFull))\RL4:="DOStartCnv2="+ValToStr(DOutput(DOStartCnv2));
ReSet DOStartCnv2;
TPWrite "5";
!ENDIF
!ReSet DOStartCnv2;
!
ENDTRAP
TRAP tBoxOut
!
TPWrite "1";
!
ENDTRAP
TRAP tstopbox
!
Reset dostartcnv2;
ISleep inStopbox;
TPWrite "2";
!
ENDTRAP
TRAP tinbox
! Reset DOStartCnv2;
Incr nxiangzi;
TPWrite "3";
ENDTRAP
PROC rRestartPrg()
!
TPWrite "4";
IF OpMode()<>OP_AUTO RETURN ;
ExitCycle;
!
ENDPROC
PROC rStopPrg()
!
IF OpMode()<>OP_AUTO RETURN ;
pCurPos:=CRobT(\Tool:=tGriper\WObj:=wobj0);
ErrWrite\W,"Program Stopped","c1Position="+ValToStr(AInput(c1Position))+" c2Position="+ValToStr(AInput(c2Position))\RL2:="c1Speed="+ValToStr(AInput(c1Speed))+" c2Speed="+ValToStr(AInput(c2Speed))\RL3:="c2ObjectsInQ="+ValToStr(GInput(c2ObjectsInQ))\RL4:="nS1Count="+ValToStr(nS1Count)+" X="+ValToStr(pCurPos.trans.x)+" Y="+ValToStr(pCurPos.trans.y)+" Z="+ValToStr(pCurPos.trans.z);
ReSet DOStartCnv2;
!
ENDPROC
PROC p1()
MoveL Offs(pPrePlace,0,0,0),vMidLoad,z10,tGriper\WObj:=wobjcnv2;
moveL pPlace10,vMinLoad_1,z10,tGriper\WObj:=Wobjcnv2;
moveL Offs(pPlace,0,0,0),vMinLoad_2,z5\Inpos:=stoppoint2,tGriper\WObj:=Wobjcnv2;
MoveJ p30,vMinLoad_2,z5,tGriper;
ENDPROC
PROC text111()
WHILE true DO
MoveJ [[-214.25,-174.53,-995.08],[0,0.999487,0.0320164,0],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,3.68393]],v2000,z50,tGriper\WObj:=wobj0;
MoveJ [[-53.36,-174.54,-995.09],[0,0.999488,0.0320044,0],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,5.87117]],v2000,z50,tGriper\WObj:=wobj0;
MoveJ [[-214.66,-58.14,-995.05],[0,0.999487,0.0320283,0],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,3.21245]],v2000,z50,tGriper\WObj:=wobj0;
ENDWHILE
ENDPROC
ENDMODULE
