EC機(jī)器人簡(jiǎn)單碼垛（4點(diǎn)法）
簡(jiǎn)介
該文檔適用于艾利特機(jī)器人EC/EA系列機(jī)械臂，可用于一些機(jī)械臂碼垛應(yīng)用場(chǎng)景。該碼垛程序包使用時(shí)只需要設(shè)置碼盤的4個(gè)對(duì)角點(diǎn)的物料位置，以及物料總數(shù)，碼盤層數(shù)，層高即可，操作簡(jiǎn)單，實(shí)用易懂。初始程序包是由陳瞭編寫。
1 僅碼垛
僅碼垛的意思就是該程序只支持碼垛，單點(diǎn)取料放入碼盤中
1.1 操作步驟及所需變量
（1）使用前需打開以下變量：
V158 //碼垛上方位
V159 //碼垛位
P150 //碼盤第一個(gè)點(diǎn)
P151 //碼盤第二個(gè)點(diǎn)
P152 //碼盤第三個(gè)點(diǎn)
P153 //碼盤第四個(gè)點(diǎn)
I 100 //碼垛計(jì)數(shù)
（2）將JBI主程序和子程序?qū)氲綑C(jī)械臂里面，LUA程序?qū)霗C(jī)械臂并保存設(shè)置。
（3）打開JBI子程序，更新P150-153這4個(gè)點(diǎn)位，點(diǎn)位設(shè)置示意圖如下：
（4）設(shè)置好子程序里面 I150-153 等參數(shù)，參數(shù)解析如下：
I150 第一個(gè)方向個(gè)數(shù)（行數(shù)）
I151 第二個(gè)方向個(gè)數(shù)（列數(shù)）
I152 Z方向?qū)訑?shù)
I153 單層層高，單位mm
（5）在JBI主程序里面，設(shè)置固定抓取點(diǎn)，固定抓取點(diǎn)的位置為第17行
（6）將機(jī)械臂程序運(yùn)行模式設(shè)置為 “ 連續(xù)循環(huán) “ 模式，運(yùn)行主程序
1.2 JBI程序
● 主程序
該程序是主運(yùn)行程序（JBI），無(wú)需修改
NOP
CALL JOB:test_pallet_setting
//調(diào)用子程序
RESTARTLUA INDEX=1
//B000與后臺(tái)LUA交互
//1:計(jì)算碼垛
//2:計(jì)算拆垛
SET B000 0
//I100 當(dāng)前碼垛個(gè)數(shù)
SET I100 1
LABEL *startpallet
SET B000 1
TIMER T=0.1 S
WAIT B000 = 0
// 此處需增加固定點(diǎn)抓取程序
// V158為當(dāng)前計(jì)算得到的碼垛點(diǎn)
// v159 上方偏移30mm
SET V159 V158
CCOOD CART
ADD V159(2) 30
//碼垛
MOVL V159 V=1000MM/S CR=10.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
● 子程序
在該程序里面設(shè)置碼盤4個(gè)對(duì)角點(diǎn)點(diǎn)位，碼垛層數(shù)，層高，碼垛總個(gè)數(shù)（JBI）
NOP
// 以下為碼垛設(shè)置部分
// **************
// 150-159用于碼垛
// P150 startpose 第一個(gè)點(diǎn)
// P151 rowpose 第一個(gè)方向的末端點(diǎn)
// P152 columnpose 第二個(gè)方向的末端點(diǎn)
// P153 refpose 遠(yuǎn)端點(diǎn)
//P150---------->P152
// | 第二個(gè)方向 |
// | |
// | |
//第一個(gè)方向 |
// | |
//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 第一個(gè)方向個(gè)數(shù)
// I151 第二個(gè)方向個(gè)數(shù)
// I152 Z方向?qū)訑?shù)
// I153 單層層高，單位mm
SET I150 3
SET I151 4
SET I152 2
SET I153 50
// **************
// 以上為碼垛設(shè)置部分
//以下部分無(wú)需設(shè)置和修改
JOINTTOPOSE P150 V150
JOINTTOPOSE P151 V151
JOINTTOPOSE P152 V152
JOINTTOPOSE P153 V153
SET I159 I150
MUL I159 I151
MUL I159 I152
INC I159
// I159為碼垛總個(gè)數(shù)
TPWRITE FinishSetting
END
1.3 Lua程序
該lua腳本在機(jī)器人后臺(tái)運(yùn)行即可，無(wú)需修改（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
--set_global_variable("B0", 0)
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 --獲取在當(dāng)前層第幾個(gè)
local current_row = math.fmod(count - 1, count_row) -- 取余數(shù) get residual
local current_column = math.floor((count - 1) / count_row) -- 取整數(shù) 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
2 拆垛與碼垛
拆垛與碼垛程序包的意思就是取料垛盤依次取料，依次放入碼垛盤中。
2.1 操作步驟及所需變量
（1）使用前需在示教器上打開以下變量：
V158 //碼垛上方位
V159 //碼垛放料位
V168 //拆垛上方位
V169 //拆垛取料位
P150 //碼垛盤第一個(gè)點(diǎn)
P151 //碼垛盤第二個(gè)點(diǎn)
P152 //碼垛盤第三個(gè)點(diǎn)
P153 //碼垛盤第四個(gè)點(diǎn)
P160 //拆垛盤第一個(gè)點(diǎn)
P161 //拆垛盤第二個(gè)點(diǎn)
P162 //拆垛盤第三個(gè)點(diǎn)
P163 //拆垛盤第四個(gè)點(diǎn)
I 100 //碼垛計(jì)數(shù)
I 101 //拆垛計(jì)數(shù)
（2）將JBI主程序和子程序?qū)氲綑C(jī)械臂控制柜里面，LUA程序?qū)霗C(jī)械臂并保存設(shè)置。
（3）打開JBI子程序更新P150-153，P160-163這8個(gè)點(diǎn)位，點(diǎn)位設(shè)置示意圖如下。以及設(shè)置好I150-153,I160-I163等參數(shù)，參數(shù)解析如下：
（4）設(shè)置好I150-153等參數(shù)，參數(shù)解析如下：
● 碼垛參數(shù)：
I150 第一個(gè)方向個(gè)數(shù)
I151 第二個(gè)方向個(gè)數(shù)
I152 Z方向?qū)訑?shù)
I153 單層層高，單位mm
● 拆垛參數(shù)：
I160 第一個(gè)方向個(gè)數(shù)
I161 第二個(gè)方向個(gè)數(shù)
I162 Z方向?qū)訑?shù)
I163 單層層高，單位mm
（5）將機(jī)械臂程序運(yùn)行模式設(shè)置為 “ 連續(xù)循環(huán) “ 模式，運(yùn)行主程序
2.2 JBI程序
該程序是主運(yùn)行程序，無(wú)需修改
NOP
CALL JOB:test_pallet_setting
//調(diào)用下面的子程序
RESTARTLUA INDEX=1
//B000與后臺(tái)LUA交互
//1:計(jì)算碼垛
//2:計(jì)算拆垛
SET B000 0
//I100 當(dāng)前碼垛個(gè)數(shù)
SET I100 1
//I101 當(dāng)前拆垛個(gè)數(shù)
SET I101 1
LABEL *startpallet
SET B000 1
TIMER T=0.1 S
WAIT B000 = 0
SET B000 2
TIMER T=0.1 S
WAIT B000 = 0
// V168為當(dāng)前計(jì)算得到的拆垛點(diǎn)
// v169 上方點(diǎn)
SET V169 V168
CCOOD CART
ADD V169(2) 30
// V158為當(dāng)前計(jì)算得到的碼垛點(diǎn)
// v159 上方點(diǎn)
SET V159 V158
CCOOD CART
ADD V159(2) 30
//拆垛
MOVL V169 V=1000MM/S CR=10.0MM ACC=50 DEC=50
MOVL V168 V=100MM/S CR=0.0MM ACC=50 DEC=50
TIMER T=0.5 S
MOVL V169 V=1000MM/S CR=0.0MM ACC=50 DEC=50
INC I101
//碼垛
MOVL V159 V=1000MM/S CR=10.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< div="">
END
● 子程序
在該程序里面設(shè)置碼盤4個(gè)對(duì)角點(diǎn)點(diǎn)位，碼垛層數(shù)，層高，碼垛總個(gè)數(shù)（JBI）
NOP
// 以下為碼垛設(shè)置部分
// **************
// 150-159用于碼垛
// P150 startpose 第一個(gè)點(diǎn)
// P151 rowpose 第一個(gè)方向的末端點(diǎn)
// P152 columnpose 第二個(gè)方向的末端點(diǎn)
// P153 refpose 遠(yuǎn)端點(diǎn)
//P150---------->P152
// | 第二個(gè)方向 |
// | |
// | |
//第一個(gè)方向 |
// | |
//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 第一個(gè)方向個(gè)數(shù)
// I151 第二個(gè)方向個(gè)數(shù)
// I152 Z方向?qū)訑?shù)
// I153 單層層高，單位mm
SET I150 3
SET I151 4
SET I152 2
SET I153 50
// **************
// 以上為碼垛設(shè)置部分
// 以下為拆垛設(shè)置部分
// **************
// 160-169用于拆垛
SETJOINT P160 -46.5353,-94.3171,101.4025,-67.3931,125.3434,-221.8873
SETJOINT P161 -56.9397,-80.2992,84.8697,-59.2620,119.7353,-232.2963
SETJOINT P162 -30.5356,-59.9489,63.1652,-75.3110,131.8696,-203.1308
SETJOINT P163 -40.2939,-46.5040,38.5012,-56.4464,128.2411,-214.9811
// I160 第一個(gè)方向個(gè)數(shù)
// I161 第二個(gè)方向個(gè)數(shù)
// I162 Z方向?qū)訑?shù)
// I163 單層層高，單位mm
SET I160 3
SET I161 4
SET I162 2
SET I163 -50
// 以上為拆垛設(shè)置部分
//以下部分無(wú)需設(shè)置和修改
// **************
JOINTTOPOSE P150 V150
JOINTTOPOSE P151 V151
JOINTTOPOSE P152 V152
JOINTTOPOSE P153 V153
JOINTTOPOSE P160 V160
JOINTTOPOSE P161 V161
JOINTTOPOSE P162 V162
JOINTTOPOSE P163 V163
SET I159 I150
MUL I159 I151
MUL I159 I152
INC I159
// I159為碼垛總個(gè)數(shù)
SET I169 I160
MUL I169 I161
MUL I169 I162
INC I169
// I169為拆垛總個(gè)數(shù)
TPWRITE FinishSetting
END
2.3 Lua程序
該lua腳本在機(jī)器人后臺(tái)運(yùn)行即可，無(wú)需修改
-- 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
--set_global_variable("B0", 0)
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 --獲取在當(dāng)前層第幾個(gè)
local current_row = math.fmod(count - 1, count_row) -- 取余數(shù) get residual
local current_column = math.floor((count - 1) / count_row) -- 取整數(shù) 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
附件
1. 僅碼垛
（1）簡(jiǎn)易說(shuō)明
reademe.md
（2）LUA腳本程序
pallet_maduo.lua
（3）JBI程序
主程序：
test_pallet_only_maduo.jbi
子程序：
test_pallet_setting_only_maduo.jbi
2. 碼垛與拆垛
（1）簡(jiǎn)易說(shuō)明
reademe.md
（2）LUA腳本程序
pallet_calculation2.lua
（3）JBI程序
主程序
test_pallet.jbi
子程序
test_pallet_setting2.jbi
