个人笔记：码垛机器人简单编程

最近要考试了，刚好分享下笔记
具体程序如下，适用于简单的码垛机器人编程
共有三个点需要经过，原点、p1、p2、p3三个点，
其中p1点为吸盘或抓手在空中运行的位置，
p2为正在抓取实物的位置，
p3为抓手释放物体的位置
大致的运行轨迹为：
原点开始运动——到达p1的位置——向下贴近传送带到达p2吸附住（或抓取）物体——抬升机器臂，回到位于半空的p1——然后机器臂将物体移动到p3点开始释放物体——重复该运动四次——最终回归原点
注：每次停留的时间为一秒

实际编程如下（部分代码有编程软件自带的部分代码，实际代码自行底部看图）
DEF Modul ( )
DECL E6POS zhua,fang
DECL INT a
;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 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
FOR a=o TO 3
zhua=XP2
fang=XP3
zhua.z=zhua.z+100
LIN zhua
zhua.z=zhua.z-100
LIN zhua
;FOLD OUT 4 '' State= TRUE;%{PE}%R 5.4.33,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:4, 3:, 5:TRUE, 6:

$OUT[4]=TRUE
;ENDFOLD
;FOLD WAIT Time= 1 sec;%{PE}%R 5.4.33,%MKUKATPBASIS,%CWAIT,%VWAIT,%P 2:1
WAIT SEC 1
;ENDFOLD
zhua.z=zhua.z+100
LIN zhua
fang.z=famg.z+100+17*a
LIN fang
fang.z=fang.z-100
LIN fang
;FOLD OUT 4 '' State= FALSE;%{PE}%R 5.4.33,%MKUKATPBASIS,%COUT,%VOUTX,%P 2:4, 3:, 5:FALSE, 6:

$OUT[4]=FALSE
;ENDFOLD
;FOLD WAIT Time= 1 sec;%{PE}%R 5.4.33,%MKUKATPBASIS,%CWAIT,%VWAIT,%P 2:1
WAIT SEC 1
;ENDFOLD
fang.z=fang.z+100
LIN fang
ENDFOR
;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
END
DEF ()
;FOLD PTP P1 Vel=50 % PDAT1 Tool[0] Base[0];%{PE}%R 5.5.29,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P1, 3:, 5:50, 7:PDAT1
$BWDSTART=FALSE
PDAT_ACT=PPDAT1
FDAT_ACT=FP1
BAS(#PTP_PARAMS,50)
PTP XP1
;ENDFOLD
;FOLD PTP P2 Vel=50 % PDAT2 Tool[0] Base[0];%{PE}%R 5.5.29,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P2, 3:, 5:50, 7:PDAT2
$BWDSTART=FALSE
PDAT_ACT=PPDAT2
FDAT_ACT=FP2
BAS(#PTP_PARAMS,50)
PTP XP2
;ENDFOLD
;FOLD PTP P3 Vel=50 % PDAT3 Tool[0] Base[0];%{PE}%R 5.5.29,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P3, 3:, 5:50, 7:PDAT3
$BWDSTART=FALSE
PDAT_ACT=PPDAT3
FDAT_ACT=FP3
BAS(#PTP_PARAMS,50)
PTP XP3
;ENDFOLD