Kuka机器人设计通用码垛程序
假设需要一个码垛程序, 从输送线抓到托盘, 托盘每层4个, 需要码5层, 可以用以下程序架构设计:
1, 再config中定义层数cengshu , 每层码垛的个数(码垛的次数)cishu , 每层的高度levelHeight , 码垛放置点的集合putPoint[,] ,预放点1集合prePut1[,], 预放点2集合prePut2[,]
DECL INT cengshu = 0
DECL INT cishu = 0
DECL INT levelHeight = 200
DECL FRAME putPoint[10,10]
DECL FRAME prePut1[10,10]
DECL FRAME prePut2[10,10]
码垛放件程序
&ACCESS RVO1
&COMMENT put2
DEF put2 ( )
;FOLD INI
;FOLD BASISTECH INI
GLOBAL 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
;ENDFOLD
ENDIF
IF (cengshu == 1) OR (cengshu == 2) OR (cengshu == 3) OR (cengshu == 4) OR (cengshu == 5) THEN
IF cishu == 1 THEN
putPoint[cengshu,cishu]= XputBase
putPoint[cengshu,cishu].X = putPoint[cengshu,cishu].X
putPoint[cengshu,cishu].Y = putPoint[cengshu,cishu].Y
putPoint[cengshu,cishu].Z = putPoint[cengshu,cishu].Z+(cengshu-1)*levelHeight
ENDIF
IF cishu == 2 THEN
putPoint[cengshu,cishu]= XputBase
putPoint[cengshu,cishu].X = putPoint[cengshu,cishu].X+10
putPoint[cengshu,cishu].Y = putPoint[cengshu,cishu].Y
putPoint[cengshu,cishu].Z = putPoint[cengshu,cishu].Z+(cengshu-1)*levelHeight
ENDIF
IF cishu == 3 THEN
putPoint[cengshu,cishu]= XputBase
putPoint[cengshu,cishu].X = putPoint[cengshu,cishu].X+10
putPoint[cengshu,cishu].Y = putPoint[cengshu,cishu].Y+10
putPoint[cengshu,cishu].Z = putPoint[cengshu,cishu].Z+(cengshu-1)*levelHeight
ENDIF
IF cishu == 3 THEN
putPoint[cengshu,cishu]= XputBase
putPoint[cengshu,cishu].X = putPoint[cengshu,cishu].X
putPoint[cengshu,cishu].Y = putPoint[cengshu,cishu].Y+10
putPoint[cengshu,cishu].Z = putPoint[cengshu,cishu].Z+(cengshu-1)*levelHeight
ENDIF
ENDIF
;shoudong dao preput1
prePut1[cengshu,cishu]= putPoint[cengshu,cishu]
prePut1[cengshu,cishu].X = prePut1[cengshu,cishu].X+200
prePut1[cengshu,cishu].Y = prePut1[cengshu,cishu].Y+200
prePut1[cengshu,cishu].Z = prePut1[cengshu,cishu].Z+200
prePut2[cengshu,cishu]= putPoint[cengshu,cishu]
prePut2[cengshu,cishu].X = prePut2[cengshu,cishu].X+100
prePut2[cengshu,cishu].Y = prePut2[cengshu,cishu].Y+100
prePut2[cengshu,cishu].Z = prePut2[cengshu,cishu].Z+100
PTP prePut1[cengshu,cishu] C_DIS
PTP prePut2[cengshu,cishu] C_DIS
LIN putPoint[cengshu,cishu] C_DIS
$out[1]=FALSE ;fangjian
WAIT SEC 0.5
PTP prePut2[cengshu,cishu] C_PTP
PTP prePut1[cengshu,cishu] C_PTP
END
主程序:
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 INI
GLOBAL 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
;ENDFOLD
LOOP
FOR cengshu = 1 TO 5
FOR cishu = 1 TO 4
WAIT 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
;ENDFOLD
put2 ()
;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
;ENDFOLD
ENDFOR
ENDFOR
$out[3]=TRUE
WAIT SEC 1
$out[3]=FALSE
ENDLOOP
END
取件程序:
&ACCESS RVO1
&COMMENT put1
DEF pick ( )
;FOLD INI
;FOLD BASISTECH INI
GLOBAL 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]=TRUE
WAIT 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
;ENDFOLD
END