VB脚本码垛库移植到AR机器人上使用

--主程序--
function main()
	local H,L,C = 3,4,2 --行、列、层
	local F = 1               --运动方式(1：点到点 2：直线 3：拱形)
	local XX,YY,ZZ,CC = 0,0,10,0
	while true do	   
		for I=1,H*L*C do
			--码垛 1 取--
            InitPlt(1,H,L,C,p1,p2,p3,p4) --码垛1初始化
			MovPlt(F,1,I,XX,YY,ZZ,CC)    --拱形时指令：MovPlt(F,No,I,XX,YY,ZZ,CC,R,S,T)
			MovPlt(F,1,I,0,0,0,0)
			DO(1,ON) --取
			MovPlt(F,1,I,0,0,10,0)
			--码垛 2 放--
            InitPlt(2,H,L,C,p5,p6,p7,p8) --码垛2初始化
			MovPlt(F,2,I,XX,YY,ZZ,CC)
			MovPlt(F,2,I,0,0,0,0)
			DO(1,OFF) --放
			MovPlt(F,2,I,0,0,10,0)
		end
	end
end

--PALLET.LIB(库文件)
-------------------------------------------------------------------
function InitPlt (INITPLT_NO,INITPLT_H,INITPLT_L,INITPLT_C,POS1,POS2,POS3,POS4)
	POS5,POS6,POS7,PLT_PAR = {},{},{},{}
	PLT_PAR[INITPLT_NO] = {POS1,POS2,POS3,POS4,POS5,POS6,POS7}
	PLT = {} 
	PLT = PLT_PAR[INITPLT_NO]
	INITPLT1P,INITPLT2P,INITPLT3P,INITPLT4P= PLT[1],PLT[2],PLT[3],PLT[4]
	PLT[5] = {x=INITPLT_H,y=INITPLT_L,z=INITPLT_C,c=POS1.c,h=POS1.h}
::INITPLT010::
    if INITPLT_H < 1 then goto INITPLT_ERR end
    if INITPLT_H < 2 then goto INITPLT015 end
    INITPLT5I = INITPLT_H - 1 
    INITPLTXX = (INITPLT2P.x - INITPLT1P.x) / INITPLT5I
    INITPLTXY = (INITPLT2P.y - INITPLT1P.y) / INITPLT5I
    goto INITPLT020
::INITPLT015::
    INITPLTXX,INITPLTXY = 0,0
::INITPLT020::
    if INITPLT_L < 1 then goto INITPLT_ERR end
    if INITPLT_L < 2 then goto INITPLT025 end
    INITPLT5J = INITPLT_L - 1
    INITPLTYX = (INITPLT3P.x - INITPLT1P.x) / INITPLT5J
    INITPLTYY = (INITPLT3P.y - INITPLT1P.y) / INITPLT5J
    goto INITPLT030
::INITPLT025::
    INITPLTYX,INITPLTYY = 0,0
::INITPLT030::
    if INITPLT_C < 1 then goto INITPLT_ERR end
    if INITPLT_C < 2 then goto INITPLT035 end
    INITPLT5K = INITPLT_C - 1    
    INITPLTZZ  = (INITPLT4P.z - INITPLT1P.z) / INITPLT5K
    goto INITPLT040
::INITPLT035::
    INITPLTZZ = 0
::INITPLT040::
	PLT[6] = {x=INITPLTXX,y=INITPLTXY,z=INITPLTZZ,c=POS1.c,h=POS1.h}
	PLT[7] = {x=INITPLTYX,y=INITPLTYY,z=INITPLTZZ,c=POS1.c,h=POS1.h}
    goto INITPLTEND
::INITPLT_ERR::
    print("错误 !!  ELEMENT IS TOO SMALL.")
    Exit()
::INITPLTEND::
end
-------------------------------------------------------------------
function MovPlt (MOVEPLTWAY,MOVEPLTNO,MOVEPLTPSN,MOVEPLTX,MOVEPLTY,MOVEPLTZ,MOVEPLTC,MOVEPLTR,MOVEPLTS,MOVEPLTT)
    MOVEPLTI,MOVEPLTJ,MOVEPLTK = 0,0,0
	PLT = PLT_PAR[MOVEPLTNO]
    MOVEPLTPS1 = MOVEPLTPSN -1
	MOVEPLT1P,MOVEPLT5P,MOVEPLT6P,MOVEPLT7P = PLT[1],PLT[5],PLT[6],PLT[7]
    MOVEPLT8P = MOVEPLT5P.x * MOVEPLT5P.y
	MOVEPLT9P = MOVEPLTPS1 % MOVEPLT8P
    MOVEPLTMAX = MOVEPLT8P *  MOVEPLT5P.z
    if 1 > MOVEPLTPSN then goto MOVEPLTER2 end
    if MOVEPLTMAX < MOVEPLTPSN then goto MOVEPLTER3 end
	MOVEPLTI = MOVEPLT9P %  MOVEPLT5P.x
	MOVEPLTJ = MOVEPLT9P // MOVEPLT5P.x
    MOVEPLTK = MOVEPLTPS1 // MOVEPLT8P
    MOVEPLTXXX = MOVEPLTI * MOVEPLT6P.x  + MOVEPLTJ * MOVEPLT7P.x + MOVEPLTX
    MOVEPLTYYY = MOVEPLTI * MOVEPLT6P.y  + MOVEPLTJ * MOVEPLT7P.y + MOVEPLTY
    MOVEPLTZZZ = MOVEPLTK * MOVEPLT6P.z  + MOVEPLTZ
	MOVEPLTCCC = MOVEPLTC
	--print("X="..MOVEPLTXXX,",Y="..MOVEPLTYYY,",Z="..MOVEPLTZZZ,",C="..MOVEPLTCCC)
	if MOVEPLTWAY == 1 then
	    MovP(MOVEPLT1P+XYZC(MOVEPLTXXX,MOVEPLTYYY,MOVEPLTZZZ,MOVEPLTCCC))
	elseif MOVEPLTWAY == 2 then
	    MovL(MOVEPLT1P+XYZC(MOVEPLTXXX,MOVEPLTYYY,MOVEPLTZZZ,MOVEPLTCCC))
	elseif MOVEPLTWAY == 3 then
	    MArchP(MOVEPLT1P+XYZC(MOVEPLTXXX,MOVEPLTYYY,MOVEPLTZZZ,MOVEPLTCCC),MOVEPLTR,MOVEPLTS,MOVEPLTT)
	end
    goto MOVEPLTEND
::MOVEPLTER2::    
    print("ERR !!  ELEMENT NO. IS TOO SMALL.")
    Exit()
::MOVEPLTER3::
    print("ERR !!  ELEMENT NO. IS TOO LARGE.")
    Exit()
::MOVEPLTEND::
end