!实际点
PERS robtarget
pPick1:=[[1488.007792464,376.826660408,476.964684195],[0,0.707106307,0.707107256,0],[0,0,1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
!1路拾取目标点
PERS robtarget pPlace1:=[[-292.446,1263.27,55.4492],[0,0.707107,0.707106,0],[1,0,2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
!1路放置基准点
PERS robtarget pBase1_0:=[[-292.446294945,1263.272085268,55.449220723],[0,0.707107387,0.707106176,0],[1,0,2,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
!1路放置0度姿态
PERS robtarget pBase1_90:=[[-391.976797324,1362.469634994,55.449159414],[0,1,-0.000030621,0],[1,0,3,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
!1路放置90度姿态
PERS robtarget pPick2:=[[1488.013130905,-358.406014736,476.965039287],[0,0.707106307,0.707107256,0],[-1,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
PERS robtarget pPlace2:=[[-317.378,-1857.99,55.449],[0,0.707108,0.707106,0],[-2,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS robtarget pBase2_0:=[[-317.378137718,-1857.993871961,55.448967354],[0,0.707107745,0.707105817,0],[-2,0,-1,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
PERS robtarget pBase2_90:=[[-407.525988074,-1755.902485322,55.449282402],[0,1,-0.000031217,0],[-2,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
PERS speeddata MinSpeed:=[1000,300,5000,1000]; PERS speeddata MidSpeed:=[2500,400,5000,1000]; PERS speeddata MaxSpeed:=[4000,500,5000,1000];
!搬运速度定义
PERS bool bPalletFull1:=FALSE;
PERS bool bPalletFull2:=FALSE;
!逻辑布尔量,拾取后为UE,放置后为FALSE PERS num nCount1:=1; PERS num nCount2:=1;
!输送链计数 PROC Main() rInitAll;
WHILE TRUE DO
IF diBoxInPos1=1 AND diPalletInPos1=1 AND bPalletFull1=FALSE THEN rPick1; rPlace1; ENDIF
IF diBoxInPos2=1 AND diPalletInPos2=1 AND bPalletFull2=FALSE THEN rPick2; rPlace2; ENDIF WaitTime 0.1; ENDWHILE ENDPROC
PROC rInitAll() Reset doGrip;
pActualPos:=CRobT(\\tool:=tGrip); pActualPos.trans.z:=pHome.trans.z;
MoveL pActualPos,MinSpeed,fine,tGrip\\WObj:=wobj0; MoveJ pHome,MidSpeed,fine,tGrip\\WObj:=wobj0;
bPalletFull1:=FALSE;
nCount1:=1; bPalletFull2:=FALSE; nCount2:=1; ENDPROC
PROC rPick1()
MoveJ Offs(pPick1,0,0,400),MaxSpeed,z50,tGrip\\WObj:=wobj0; MoveL pPick1,MinSpeed,fine,tGrip\\WObj:=wobj0; Set doGrip; WaitTime 0.3; GripLoad LoadFull;
MoveL Offs(pPick1,0,0,400),MinSpeed,z50,tGrip\\WObj:=wobj0; ENDPROC
PROC rPick2()
MoveJ Offs(pPick2,0,0,400),MaxSpeed,z50,tGrip\\WObj:=wobj0; MoveL pPick2,MinSpeed,fine,tGrip\\WObj:=wobj0; Set doGrip; WaitTime 0.3; GripLoad LoadFull;
MoveL Offs(pPick2,0,0,400),MinSpeed,z50,tGrip\\WObj:=wobj0; ENDPROC
PROC rPlace1() rPosition1;
MoveJ Offs(pPlace1,0,0,400),MidSpeed,z50,tGrip\\WObj:=wobj0; MoveL pPlace1,MinSpeed,fine,tGrip\\WObj:=wobj0; Reset doGrip; WaitTime 0.3;
GripLoad LoadEmpty;
MoveL Offs(pPlace1,0,0,400),MidSpeed,z50,tGrip\\WObj:=wobj0; MoveJ Offs(pPick1,0,0,400),MaxSpeed,z50,tGrip\\WObj:=wobj0; nCount1:=nCount1+1; IF nCount1>20 THEN bPalletFull1:=TRUE; ENDIF ENDPROC
PROC rPlace2() rPosition2;
MoveJ Offs(pPlace2,0,0,400),MaxSpeed,z50,tGrip\\WObj:=wobj0; MoveL pPlace2,MinSpeed,fine,tGrip\\WObj:=wobj0; Reset doGrip; WaitTime 0.3; GripLoad LoadEmpty;
MoveL Offs(pPlace2,0,0,400),MidSpeed,z50,tGrip\\WObj:=wobj0; MoveJ Offs(pPick2,0,0,400),MaxSpeed,z50,tGrip\\WObj:=wobj0; nCount2:=nCount2+1; IF nCount2>20 THEN bPalletFull2:=TRUE; ENDIF ENDPROC
PROC rPosition1() TEST nCount1 CASE 1:
pPlace1:=Offs(pBase1_0,0,0,0); CASE 2:
pPlace1:=Offs(pBase1_0,600+10,0,0);
CASE 3:
pPlace1:=Offs(pBase1_90,0,400+10,0); CASE 4:
pPlace1:=Offs(pBase1_90,400+10,400+10,0); CASE 5:
pPlace1:=Offs(pBase1_90,800+20,400+10,0); CASE 6:
pPlace1:=Offs(pBase1_0,0,600+10,200); CASE 7:
pPlace1:=Offs(pBase1_0,600+10,600+10,200); CASE 8:
pPlace1:=Offs(pBase1_90,0,0,200); CASE 9:
pPlace1:=Offs(pBase1_90,400+10,0,200); CASE 10:
pPlace1:=Offs(pBase1_90,800+20,0,200); CASE 11:
pPlace1:=Offs(pBase1_0,0,0,400); CASE 12:
pPlace1:=Offs(pBase1_0,600+10,0,400); CASE 13:
pPlace1:=Offs(pBase1_90,0,400+10,400); CASE 14:
pPlace1:=Offs(pBase1_90,400+10,400+10,400); CASE 15:
pPlace1:=Offs(pBase1_90,800+20,400+10,400); CASE 16:
pPlace1:=Offs(pBase1_0,0,600+10,600); CASE 17:
pPlace1:=Offs(pBase1_0,600+10,600+10,600);