ProEasy机器人案例：电池边包胶

如下图所示，对一个电池三边包边，因客户现场有很多规格电池的大小，所以就需要建立动态的工具坐标来实现适配所有种类的电池

程序如下：Ddome程序

function Speed(num) --速度设置

MaxSpdL(2000) --movl最大速度

MaxAccL(20000) --movl最大加速度

SetMovL_MODE(1) --movl运动的速度参数模式

AccL(num) --movl加速度

DecL(num) --movl减速度

SpdL(num) --movl速度

MaxSpdJ(9000) --movp最大速度

MaxAccJ(300) --movp最大加速度

AccJ(num) --movp加速度

DecJ(num) --movp减速度

SpdJ(num) --movp速度

end

function TLSet(TF_Idx, W, H, E, dC) --设置工具坐标系的值

local tValid = _LUA_NONE_ERR

local TmpValid = {}

local tmpIdx = 1

local TmpData = {}

local dA, dB = 0, 0

local function_code_write_teach_buffer_1 = 0x00010013 --W

local function_code_write_teach_buffer_2 = 0x01010013 --H

local function_code_write_teach_buffer_3 = 0x02010013 --E

local function_code_write_teach_buffer_4 = 0x03010013 --A

local function_code_write_teach_buffer_5 = 0x04010013 --B

local function_code_write_teach_buffer_6 = 0x05010013 --C

local function_code_write_tf_whe = 0x00011132 + (TF_Idx*0x01000000) --工具坐标系中w，h，e的地址功能码

local function_code_write_tf_abc = 0x00010632 + (TF_Idx*0x01000000) --工具坐标系中a，b，c的地址功能码

local function_code_write_tf_whe_Cal = 0x00000231 + (TF_Idx*0x01000000) --计算w, h, e教导点功能码

local function_code_write_tf_abc_Cal = 0x00000631 + (TF_Idx*0x01000000) --计算a, b, c教导点功能码

--输入数据类型判断

if type(W) ~= "number" or type(H) ~= "number" or type(E) ~= "number" or type(dA) ~= "number" or type(dB) ~= "number" or type(dC) ~= "number" or type(TF_Idx) ~= "number" then

motion.ProgramStop(string.format("Function TLSet(): input type is error!"))

end

--判断工具坐标系编号

if TF_Idx < 1 or TF_Idx > 9 then

motion.ProgramStop(string.format("Function TLSet(): Value of TF_Idx is out of range!"))

end

--写入a, b, c索引值，写入参数值

TmpValid[tmpIdx] = para.Write(0,2,07,function_code_write_teach_buffer_4)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,08, dA)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,07,function_code_write_teach_buffer_5)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,08, dB)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,07,function_code_write_teach_buffer_6)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,08, dC)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,06, function_code_write_tf_abc)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,06, function_code_write_tf_abc_Cal)

DELAY(0.004)

--写入w, h, e索引值，写入参数值

TmpValid[tmpIdx] = para.Write(0,2,07,function_code_write_teach_buffer_1)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,08, W)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,07,function_code_write_teach_buffer_2)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,08, H)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,07,function_code_write_teach_buffer_3)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,08, E)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,06, function_code_write_tf_whe)

tmpIdx = tmpIdx + 1

DELAY(0.004)

TmpValid[tmpIdx] = para.Write(0,2,06, function_code_write_tf_whe_Cal)

tmpIdx = tmpIdx + 1

DELAY(0.004)

return 0

end

function Tool_Generate(Length,Width,Num) --动态工具坐标生成，Length物料长度，Width物料宽度,Num工具坐标编号

tcmd, data = motion.GetFeedbackPosition(1)

-----------------------使用方法如下----------------------------

---

local Length1 = data.x + Length/2 --计算电池对角点的X坐标

local Width1 = data.Y + Width/2 --计算电池对角点的Y坐标

Relative_X = Length1 - data.x --工具中心X - 丝杆中心X (X方向偏移量)，单位μm

Relative_Y = Width1- data.y --工具中心Y - 丝杆中心Y (Y方向偏移量)，单位μm

Relative_W = math.sqrt(Relative_X^2 + Relative_Y^2) --工具坐标长度，三角函数√(X^2 + Y^2)

Relative_E = math.deg(math.atan2(Relative_Y, Relative_X))*1000 --工具坐标角度，反正切函数atan2(Y/X)。不能用Atan，因为需要注意角度象限。注意单位。

TLSet(Num, Relative_W, 0, Relative_E - data.c, 0) --将工具长度与工具角度填入TLSet生成Num工具坐标系，工具角度需要减去当前C轴的值。

DELAY(0.5)

end

Speed(20) --速度设置

----例如长50000um，宽20000um

Tool_Generate(50000,-20000,1) --动态工具坐标1生成

Tool_Generate(-50000,-20000,2) --动态工具坐标2生成

while 1 do

MovL(1) --物料起始点

-------第一条边贴胶-------------

MovL(1 + P.Y(50000)) --直线偏移物料长边贴胶

-------第一角贴胶-------------

para.Write(0,2,6, 0x01010050) --修改控制器参数 P2-06 ，把当前坐标系 TF=1

_,Here = motion.GetFeedbackPosition(1) --获取当前位置

MovL(Here + P.C(90000)) --以旋转中心1旋转90贴胶

-------第二条边贴胶-------------

para.Write(0,2,6, 0x00010050) --修改控制器参数 P2-06 ，把当前坐标系 TF=0

_,Here = motion.GetFeedbackPosition(1) --获取当前位置

MovL(Here + P.Y(20000)) --直线偏移物料宽边贴胶

-------第二角贴胶-------------

para.Write(0,2,6, 0x02010050) --修改控制器参数 P2-06 ，把当前坐标系 TF=2

_,Here = motion.GetFeedbackPosition(1) --获取当前位置

MovL(Here + P.C(90000)) --以旋转中心2旋转90

-------第三条边贴胶-------------

para.Write(0,2,6, 0x00010050) --修改控制器参数 P2-06 ，把当前坐标系 TF=0

_,Here = motion.GetFeedbackPosition(1) --获取当前位置

MovL(Here + P.Y(50000)) --直线偏移物料长边贴胶

end