应用需求

仅需示教一层垛型的四个角点（无需创建user坐标系），按照示教的方向进行码垛和拆垛（不必先x后y方向）

导入test_pallet.jbi及test_pallet_setting.jbi程序，

导入pallet_calculation.lua，并加载到lua的第一个脚本

以下为码垛设置部分

150-159用于码垛

//P150--------------------->P152

// | 第二个方向 |

// | |

// | 第一个方向 |

// | |

//P151 -------------------- P153

P150 起点

P151第一个方向的末端点

P152 第二个方向的末端点

P153 远端参考点

I150 第一个方向码垛个数 （若该方向个数为1 ，设置为1）

I151 第二个方向码垛个数 （若该方向个数为1 ，设置为1）

I152 Z方向层数I153 单层层高，单位mm

仅码垛，打开test_pallet_setting.jbi，完成以上码垛数据的设置

使用时，打开机器人的V150-V153及V158 V159，P150-153变量

使用时，仅需导入test_pallet_setting.jbi和test_pallet.jbi，导入pallet_calculation.lua并加载到第一个lua脚本！！

test_pallet_setting.jbi

NOP
// 以下为码垛设置部分
// **************
// 150-159用于码垛
// P150 startpose 第一个点
// P151 rowpose  第一个方向的末端点
// P152 columnpose  第二个方向的末端点
// P153 refpose  远端点
//P150---------->P152
// |  第二个方向    |
// |               |
// |               |
//第一个方向        |
// |               |
//P151  -------- P153
SETJOINT P150 -13.5978,-104.8072,145.8060,-129.0093,134.8434,-180.1184
SETJOINT P151 -47.0715,-104.5984,133.2336,-88.6142,125.0733,-222.4587
SETJOINT P152 -5.5062,-53.8769,86.6584,-128.8164,134.5548,-168.7319
SETJOINT P153 -23.4796,-55.2147,81.2515,-104.4537,133.6682,-193.8322
// I150 第一个方向个数
// I151 第二个方向个数
// I152 Z方向层数
// I153 单层层高，单位mm
SET I150 3
SET I151 4
SET I152 2
SET I153 50
// **************
// 以上为码垛设置部分


//以下部分无需设置和修改
// **************
JOINTTOPOSE P150 V150
JOINTTOPOSE P151 V151
JOINTTOPOSE P152 V152
JOINTTOPOSE P153 V153

SET I159 I150
MUL I159 I151
MUL I159 I152
INC I159
// I159为码垛总个数

TPWRITE FinishSetting
END

主程序test_pallet.jbi

NOP
CALL JOB:test_pallet_setting
RESTARTLUA INDEX=1
//B000与后台LUA交互
//1:计算码垛
//2:计算拆垛
SET B000 0
//I100 当前码垛个数
SET I100 1

LABEL *startpallet
SET B000 1
TIMER T=0.1 S
WAIT B000 = 0

// 此处需增加固定点抓取程序

// V158为当前计算得到的码垛点
// v159 上方偏移30mm
SET V159 V158
CCOOD CART
ADD V159(2) 30

//码垛
MOVL V159 V=1000MM/S CR=0.0MM ACC=50 DEC=50
MOVL V158 V=100MM/S CR=0.0MM ACC=50 DEC=50
TIMER T=0.5 S
MOVL V159 V=1000MM/S CR=0.0MM ACC=50 DEC=50
INC I100
JUMP *startpallet IF I100<I159
END

后台计算pallet_calculation.lua

-- 1st pallet direction:  startpos--->rowpos
-- 2nd pallet direction:  startpos--->columnpos
-- 3rd pallet direction:  startpos--->Z direction

-- startpos(1)   ------> columnpos(2)
--    |                      |
--    |                      |
--    |                      |
-- rowpos(3)  --------   refpos(4)

-- count:        current pallet number   global variable :I100
-- count_row:    total row number        global variable :I150
-- count_column: total column nubmer     global variable :I151
-- count_layer:  total layer  nubmer     global variable :I152
-- height:       single layer height     global variable :I153
-- startpos                              global variable :V150
-- rowpos                                global variable :V151
-- columnpos                             global variable :V152
-- refpos                                global variable :V153

-- outpos  : calculation result          global variable :V160

function Interpolate_pose(p_from, p_to, alpha)
    -- Linear interpolation of tool position
    -- When alpha is 0, returns p_from. When alpha is 1, returns p_to. As alpha goes from 0 to 1, returns a
    -- pose going in a straight line from p_from to p_to.
    -- If alpha is less than 0, returns a point before p_from on the line.
    -- If alpha is greater than 1, returns a pose after p_to on the line.
    local p = {}
    p[1] = p_from[1] + (p_to[1] - p_from[1]) * alpha
    p[2] = p_from[2] + (p_to[2] - p_from[2]) * alpha
    p[3] = p_from[3] + (p_to[3] - p_from[3]) * alpha
    p[4] = p_from[4]
    p[5] = p_from[5]
    p[6] = p_from[6]
    return p
end

function WaitUntil(var, value)
    --local vtmp =
    while get_global_variable(var) ~= value do
        sleep(0.05)
    end
end

function PalletCal(j_count,j_row,j_column,j_height,j_startpos,j_rowpos,j_columnpos,j_refpos)
    local count = get_global_variable(j_count)
    local count_row = get_global_variable(j_row)
    local count_column = get_global_variable(j_column)
    local layer_height = get_global_variable(j_height)


    local startpos = {get_global_variable(j_startpos)}
    local rowpos = {get_global_variable(j_rowpos)}
    local columnpos = {get_global_variable(j_columnpos)}
    local refpos = {get_global_variable(j_refpos)}

    local current_layer = math.floor((count - 1) / (count_row*count_column))
    count = math.fmod(count-1, (count_row*count_column)) +1 --获取在当前层第几个
    local current_row = math.fmod(count - 1, count_row) -- 取余数  get residual
    local current_column = math.floor((count - 1) / count_row) -- 取整数 get integer+1
    if(count_row==1)  then
        count_row =2
    end
    if(count_column==1)  then
        count_column =2
    end
    local outpos1 = Interpolate_pose(startpos, rowpos, current_row / (count_row - 1))
    local outpos2 = Interpolate_pose(columnpos, refpos, current_row / (count_row - 1))
    local outpos3 = Interpolate_pose(outpos1, outpos2, current_column / (count_column - 1))

    outpos3[3] = outpos3[3] + layer_height * current_layer
    return outpos3
end

while true do
    local state = get_global_variable('B0')
    if state ==1 then
        -- 码垛
        local outpos3 =PalletCal("I100","I150","I151","I153","V150","V151","V152","V153")
        set_global_variable('V158',outpos3[1],outpos3[2],outpos3[3],outpos3[4],outpos3[5],outpos3[6])
        state = 0
        set_global_variable("B0", 0)

    elseif state == 2 then
        -- 拆垛
        local outpos3 =PalletCal("I101","I160","I161","I163","V160","V161","V162","V163")
        set_global_variable('V168',outpos3[1],outpos3[2],outpos3[3],outpos3[4],outpos3[5],outpos3[6])
        state = 0
        set_global_variable("B0", 0)
    end
    sleep(0.01)
end