Kuka机器人设计通用码垛程序

假设需要一个码垛程序, 从输送线抓到托盘, 托盘每层4个, 需要码5层, 可以用以下程序架构设计:

1, 再config中定义层数cengshu , 每层码垛的个数(码垛的次数)cishu , 每层的高度levelHeight , 码垛放置点的集合putPoint[,] ,预放点1集合prePut1[,], 预放点2集合prePut2[,]

DECL INT cengshu = 0DECL INT cishu = 0DECL INT levelHeight = 200DECL FRAME putPoint[10,10]DECL FRAME prePut1[10,10]DECL FRAME prePut2[10,10]

码垛放件程序

&ACCESS RVO1
&COMMENT put2
DEF put2 ( )
;FOLD INI;FOLD BASISTECH INIGLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )INTERRUPT ON 3 BAS (#INITMOV,0 );ENDFOLD (BASISTECH INI);FOLD USER INI;Make your modifications here;ENDFOLD (USER INI)
;ENDFOLD (INI)IF FALSE THEN;FOLD LIN putBase CONT Vel=2 m/s CPDAT0 Tool[1]:Tool1 Base[0];%{PE}%R 8.3.22,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:putBase, 3:C_DIS C_DIS, 5:2, 7:CPDAT0
$BWDSTART=FALSE
LDAT_ACT=LCPDAT0
FDAT_ACT=FputBase
BAS(#CP_PARAMS,2)
LIN XputBase C_DIS C_DIS
;ENDFOLDENDIFIF (cengshu == 1) OR (cengshu == 2) OR (cengshu == 3) OR (cengshu == 4) OR (cengshu == 5) THENIF cishu == 1 THENputPoint[cengshu,cishu]= XputBaseputPoint[cengshu,cishu].X = putPoint[cengshu,cishu].XputPoint[cengshu,cishu].Y = putPoint[cengshu,cishu].YputPoint[cengshu,cishu].Z = putPoint[cengshu,cishu].Z+(cengshu-1)*levelHeightENDIFIF cishu == 2 THENputPoint[cengshu,cishu]= XputBaseputPoint[cengshu,cishu].X = putPoint[cengshu,cishu].X+10putPoint[cengshu,cishu].Y = putPoint[cengshu,cishu].YputPoint[cengshu,cishu].Z = putPoint[cengshu,cishu].Z+(cengshu-1)*levelHeightENDIFIF cishu == 3 THENputPoint[cengshu,cishu]= XputBaseputPoint[cengshu,cishu].X = putPoint[cengshu,cishu].X+10putPoint[cengshu,cishu].Y = putPoint[cengshu,cishu].Y+10putPoint[cengshu,cishu].Z = putPoint[cengshu,cishu].Z+(cengshu-1)*levelHeightENDIFIF cishu == 3 THENputPoint[cengshu,cishu]= XputBaseputPoint[cengshu,cishu].X = putPoint[cengshu,cishu].XputPoint[cengshu,cishu].Y = putPoint[cengshu,cishu].Y+10putPoint[cengshu,cishu].Z = putPoint[cengshu,cishu].Z+(cengshu-1)*levelHeightENDIFENDIF;shoudong dao preput1 prePut1[cengshu,cishu]= putPoint[cengshu,cishu]prePut1[cengshu,cishu].X = prePut1[cengshu,cishu].X+200prePut1[cengshu,cishu].Y = prePut1[cengshu,cishu].Y+200prePut1[cengshu,cishu].Z = prePut1[cengshu,cishu].Z+200prePut2[cengshu,cishu]= putPoint[cengshu,cishu]prePut2[cengshu,cishu].X = prePut2[cengshu,cishu].X+100prePut2[cengshu,cishu].Y = prePut2[cengshu,cishu].Y+100prePut2[cengshu,cishu].Z = prePut2[cengshu,cishu].Z+100PTP prePut1[cengshu,cishu] C_DISPTP prePut2[cengshu,cishu] C_DIS LIN putPoint[cengshu,cishu] C_DIS$out[1]=FALSE ;fangjianWAIT SEC 0.5PTP prePut2[cengshu,cishu] C_PTP PTP prePut1[cengshu,cishu] C_PTPEND

主程序:

c&ACCESS RVO
&REL 7
&PARAM EDITMASK = *
&PARAM TEMPLATE = C:\KRC\Roboter\Template\vorgabe
&PARAM DISKPATH = KRC:\R1\Program\PRO
DEF test( )
;FOLD INI;%{PE};FOLD BASISTECH INIGLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )INTERRUPT ON 3 BAS (#INITMOV,0 );ENDFOLD (BASISTECH INI);FOLD USER INI;Make your modifications here;ENDFOLD (USER INI)
;ENDFOLD (INI)$out[1]=FALSE$out[2]=FALSE;FOLD PTP HOME  Vel= 100 % DEFAULT;%{PE}%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART = FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS (#PTP_PARAMS,100 )
$H_POS=XHOME
PTP  XHOME
;ENDFOLDLOOPFOR cengshu = 1 TO 5FOR cishu = 1 TO 4WAIT FOR $in[1]pick ();FOLD PTP p1 CONT Vel=100 % DEFAULT Tool[1]:Tool1 Base[0];%{PE}%R 8.3.22,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:p1, 3:C_PTP, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=Fp1
BAS(#PTP_PARAMS,100)
PTP Xp1 C_PTP
;ENDFOLDput2 ();FOLD PTP HOME  Vel= 100 % DEFAULT;%{PE}%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:HOME, 3:, 5:100, 7:DEFAULT
$BWDSTART = FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=FHOME
BAS (#PTP_PARAMS,100 )
$H_POS=XHOME
PTP  XHOME
;ENDFOLDENDFOR
ENDFOR$out[3]=TRUEWAIT SEC 1$out[3]=FALSEENDLOOP END

取件程序:

&ACCESS RVO1
&COMMENT put1
DEF pick ( )
;FOLD INI;FOLD BASISTECH INIGLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )INTERRUPT ON 3 BAS (#INITMOV,0 );ENDFOLD (BASISTECH INI);FOLD USER INI;Make your modifications here;ENDFOLD (USER INI)
;ENDFOLD (INI);FOLD PTP p2 CONT Vel=100 % DEFAULT Tool[1]:Tool1 Base[0];%{PE}%R 8.3.22,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:p2, 3:C_PTP, 5:100, 7:DEFAULT
$BWDSTART=FALSE
PDAT_ACT=PDEFAULT
FDAT_ACT=Fp2
BAS(#PTP_PARAMS,100)
PTP Xp2 C_PTP
;ENDFOLD;FOLD LIN p3 CONT Vel=2 m/s CPDAT0 Tool[1]:Tool1 Base[0];%{PE}%R 8.3.22,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:p3, 3:C_DIS C_DIS, 5:2, 7:CPDAT0
$BWDSTART=FALSE
LDAT_ACT=LCPDAT0
FDAT_ACT=Fp3
BAS(#CP_PARAMS,2)
LIN Xp3 C_DIS C_DIS
;ENDFOLD$OUT[1]=TRUEWAIT SEC 1;FOLD LIN p4 CONT Vel=2 m/s CPDAT0 Tool[1]:Tool1 Base[0];%{PE}%R 8.3.22,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:p4, 3:C_DIS C_DIS, 5:2, 7:CPDAT0
$BWDSTART=FALSE
LDAT_ACT=LCPDAT0
FDAT_ACT=Fp4
BAS(#CP_PARAMS,2)
LIN Xp4 C_DIS C_DIS
;ENDFOLDEND