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