法奥机器人程序

1. 智能防撞搬运 (Function banyun)

function banyun(zqd, fzd, gd1, gd2, do1, do2, y1, y2, type)
    -- ================= 1. 去抓取侧 =================
    PTP(gd1, 100, -1, 0) -- 快速移动到全局过渡点

    -- [避障核心] L型进刀逻辑
    -- 先到达带Y偏移的高位安全点 (Z=150)
    Lin(zqd, 100, -1, 0, 1, 0, y1, 150, 0, 0, 0)
    -- 下降到低位偏移点 (Z=35)
    Lin(zqd, 100, -1, 0, 1, 0, y1, 35, 0, 0, 0) 
    -- 消除Y偏移，水平切入相机正下方 (Z=35)
    Lin(zqd, 100, -1, 0, 1, 0, 0, 35, 0, 0, 0) 
    -- 垂直下降抓取 (Z=0)
    Lin(zqd, 100, -1, 0, 1, 0, 0, 0, 0, 0, 0) 

    -- 执行抓取动作
    SetDO(do1, 1, 0, 0) 
    if type == 2 then SetDO(do2, 0, 0, 0) end
    WaitMs(500)

    -- [避障核心] L型退刀逻辑
    Lin(zqd, 100, -1, 0, 1, 0, 0, 35, 0, 0, 0)  -- 垂直抬起
    Lin(zqd, 100, -1, 0, 1, 0, y1, 35, 0, 0, 0) -- 水平退出 (恢复Y偏移)
    Lin(zqd, 100, -1, 0, 1, 0, y1, 150, 0, 0, 0)-- 抬升离开
    PTP(gd1, 100, -1, 0) -- 回过渡点

    -- ================= 2. 去放置侧 =================
    -- (逻辑同上，目标点换为 fzd, 偏移量换为 y2)
    PTP(gd2, 100, -1, 0)
    Lin(fzd, 100, -1, 0, 1, 0, y2, 150, 0, 0, 0)
    Lin(fzd, 100, -1, 0, 1, 0, y2, 35, 0, 0, 0)
    Lin(fzd, 100, -1, 0, 1, 0, 0, 35, 0, 0, 0)
    Lin(fzd, 100, -1, 0, 1, 0, 0, 0, 0, 0, 0)
    
    -- 执行放置动作
    SetDO(do1, 0, 0, 0)    
    if type == 2 then SetDO(do2, 1, 0, 0) end
    WaitMs(500)

    Lin(fzd, 100, -1, 0, 1, 0, 0, 35, 0, 0, 0)
    Lin(fzd, 100, -1, 0, 1, 0, y2, 35, 0, 0, 0)
    Lin(fzd, 100, -1, 0, 1, 0, y2, 150, 0, 0, 0)
    PTP(gd2, 100, -1, 0)
end

参数全解析

参数名	类型	说明
zqd / fzd	Point	抓取点 / 放置点 (视觉计算后的坐标)
gd1 / gd2	Point	过渡点。连接原点与工作区，防止飞线撞机。
y1 / y2	Real	Y轴安全偏移量。正值向Y+偏，负值向Y-偏。用于避开正上方相机。
do1	Int	主IO。吸真空或夹爪闭合。
do2	Int	副IO。(仅type=2时) 夹爪张开信号。
type	Int	1=吸盘(单IO); 2=夹爪(双IO)

L型避障逻辑图解

graph TD A[PTP: 过渡点] --> B[Lin: 高位安全点 Z=150] B -- 保持 Y 偏移 --> C[Lin: 低位安全点 Z=35] C -- 关键: 消除 Y 偏移 --> D[Lin: 抓取正上方 Z=35] D -- 垂直下降 --> E[Lin: 抓取点 Z=0] style C stroke:#f43f5e,stroke-width:2px style D stroke:#f43f5e,stroke-width:2px

Y=0 的情况：
当传入 y1=0 时，偏移点与正上方点重合，机器人将直接垂直下降，不执行水平平移。这使得函数具有通用性。

4. 主程序流程 (Main Logic)

graph TD Start((Start)) --> Init[chushihua] Init --> Loop{循环监听 INT 1} Loop -- 0 --> Wait[Wait 50ms] --> Loop Loop -- 100/101 --> T1[圆工件] Loop -- 102 --> T2[轴工件] Loop -- 103 --> T3[复杂装配] Loop -- 99 --> Init T1 --> Fin[INT 1 = 200/400] --> Loop T2 --> Fin T3 --> Fin

CMD 100/101 (圆)

视觉定位 -> 气缸二次定位 -> 搬运至暂存 -> 补充托盘。

CMD 102 (轴)

同上，但使用夹爪 (Type=2)，且过渡路径不同。

CMD 103 (装配)

大圆/小圆/轴依次搬运至装配台 -> 气缸夹紧翻转 -> 成品出库。

附录：完整程序源代码 (可以直接复制)

-- ==========================================================
-- 函数名称：banyun
-- 参数说明：
-- zqd: 抓取点
-- fzd: 放置点
-- gd1: 抓取侧过渡点
-- gd2: 放置侧过渡点
-- do1: 第1个输出口 (例如 1)
-- do2: 第2个输出口 (例如 2, 仅当 type=2 时生效)
-- type: 抓手类型 (1=吸盘, 2=夹爪) 夹爪需要控制两个输出口
-- y1: 抓取侧Y轴偏移   最后两个参数主要用于防止视觉端撞击相机,凡是靠近传送带的变量，都带上y偏移
-- y2: 放置侧Y轴偏移
-- ==========================================================
function banyun(zqd, fzd, gd1, gd2,do1,do2,y1,y2,type)
    -- 1. 去抓取侧
    PTP(gd1, 100, -1, 0) --去过渡点1
    Lin(zqd, 100, -1, 0, 1, 0, y1, 150, 0, 0, 0)-- 去抓取点上方(如果有y偏移，则带上y偏移)
    Lin(zqd, 100, -1, 0, 1, 0, y1, 35, 0, 0, 0) --下降到安全点    
    Lin(zqd, 100, -1, 0, 1, 0, 0, 35, 0, 0, 0) -- 平移到抓取正上方点     
    Lin(zqd, 100, -1, 0, 1, 0, 0, 0, 0, 0, 0)  -- 到达抓取点
    -- 动作——抓取，夹爪需要控制第二个输出口
    SetDO(do1, 1, 0, 0) 
    if type == 2 then
        SetDO(do2, 0, 0, 0)
    end
    WaitMs(500)
    -- 抬起
    Lin(zqd, 100, -1, 0, 1, 0, 0, 35, 0, 0, 0)  -- 抬起
    Lin(zqd, 100, -1, 0, 1, 0, y1, 35, 0, 0, 0) -- 平移到安全点
    Lin(zqd, 100, -1, 0, 1, 0, y1, 150, 0, 0, 0) -- 抬到上方
    PTP(gd1, 100, -1, 0) --回过渡点1
    -- 2. 去放置侧
    PTP(gd2, 100, -1, 0)                                    --过渡点2
    Lin(fzd, 100, -1, 0, 1, 0, y2, 150, 0, 0, 0)   -- 去放置点上方 (如果有y偏移，则带上y偏移)
    Lin(fzd, 100, -1, 0, 1, 0, y2, 35, 0, 0, 0)    --下降到安全点        
    Lin(fzd, 100, -1, 0, 1, 0, 0, 35, 0, 0, 0)     -- 平移到放置正上方点        
    Lin(fzd, 100, -1, 0, 1, 0, 0, 0, 0, 0, 0)      --到达放置点    
    -- 动作——放置，夹爪需要控制第二个输出口
    SetDO(do1, 0, 0, 0)    
    if type == 2 then
        SetDO(do2, 1, 0, 0)
    end
    WaitMs(500)
    Lin(fzd, 100, -1, 0, 1, 0, 0, 35, 0, 0, 0)     -- 抬起
    Lin(fzd, 100, -1, 0, 1, 0, y2, 35, 0, 0, 0)    -- 平移到安全点
    Lin(fzd, 100, -1, 0, 1, 0, y2, 150, 0, 0, 0)   -- 抬到上方
    PTP(gd2, 100, -1, 0)       -- 回过渡点2 (完成)
end
-- ==========================================================
-- 函数名称：pianyi
-- 功能说明：根据视觉偏移量计算新的点位
-- 参数说明：   
-- pt: 原始点位
-- dx: X轴偏移量
-- dy: Y轴偏移量
-- da: 角度偏移量
-- ==========================================================
function pianyi(pt,dx,dy,da)
    return {
        pt[1] + dx,         -- x
        pt[2] + dy,         -- y
        pt[3],              -- z
        pt[4], 
        pt[5], 
        pt[6] + da          -- Rz
    }
end
-- ==========================================================
-- 函数名称：tuopan
-- 功能说明：根据托盘层数计算托盘点位
-- 参数说明：
-- pt: 原始托盘点位
-- n: 托盘层数  托盘高度17mm，每多一层加17mm，n从0开始
-- ==========================================================
function tuopan(pt,n)
    return {
        pt[1],
        pt[2],
        pt[3] + n * 17,
        pt[4],
        pt[5],
        pt[6]
    }
end
-- ==========================================================
-- 4. 初始化程序
-- ==========================================================
function chushihua()
    -- 1. IO 复位 (复位所有输出口，根据实际接线调整)
    for i = 0, 7 do
        SetDO(i, 0)
    end
    -- 2. 回原点 ()
        PTP(原点, 100, -1, 0)
    -- 3. 告知 PLC 复位完成
    -- 状态 10: 复位完成
    SetPLCCustVar("INT", 1, 10) 
    WaitMs(500)
end

-- 脉冲函数，先置位，后复位，主要用于圆定位，减少重复代码
function PDO(port, time)
    SetDO(port, 1, 0, 0)
    WaitMs(time)
    SetDO(port, 0, 0, 0)
end
-- 置位函数，带延时，减少重复代码
function SDO(port, time)
    SetDO(port, 1, 0, 0)
    WaitMs(time)
end
-- 复位函数，带延时，减少重复代码
function RDO(port, time)
    SetDO(port, 0, 0, 0)
    WaitMs(time)
end

-- ==========================================================
-- 5. 主程序 (Main Logic)
-- ==========================================================
function main()
    -- 执行初始化
    chushihua()
    while true do
        -- [步骤 1]: 获取 PLC 命令 (INT 1)
        local cmd = GetPLCCustVar("INT", 1)
        -- 获取料仓托盘的层数
        local n = GetPLCCustVar("INT", 2)
        -- 只有当 cmd 不为0时才执行
        if cmd ~= 0  then
            -- [步骤 2]: 获取视觉偏移量 (Real 1, 2, 3)
            local dx = GetPLCCustVar("Real", 1) -- X轴
            local dy = GetPLCCustVar("Real", 2) -- Y轴
            local da = GetPLCCustVar("Real", 3) -- 角度
            -- [步骤 3]: 任务分发 (根据 CMD 选择流程)
            -- === 任务 100: 大圆 ===
            if cmd == 100 then
                SetPLCCustVar("INT", 1, 100) --开始工作
                local zqd = pianyi(大圆抓取,dx, dy, da) --获取抓取点偏移
                banyun(zqd, 圆定位点, 过渡1, 过渡2, 2, 0, -200,0,1) --搬运到定位点
                PDO(9, 500) --定位
                banyun(圆定位点, 大圆放置, 过渡2, 过渡2, 2, 0, 0,0,1) --搬运到放置点
                banyun(托盘传送带, tuopan(托盘料仓,n), 过渡2, 过渡2, 3, 0, -200,0,1) --托盘入仓
                SetPLCCustVar("INT", 1, 200) --任务完成
            -- === 任务 101: 小圆 ===
            elseif cmd == 101 then
                SetPLCCustVar("INT", 1, 100) --开始工作
                local zqd = pianyi(小圆抓取, dx, dy, da) --获取抓取点偏移
                banyun(zqd, 圆定位点, 过渡1, 过渡2, 2, 0, -200,0,1) --搬运到定位点
                PDO(9, 500) --定位
                banyun(圆定位点, 小圆放置, 过渡2, 过渡2, 2, 0, 0,0,1) --搬运到放置点
                banyun(托盘传送带, tuopan(托盘料仓,n), 过渡2, 过渡2, 3, 0, -200,0,1) --托盘入仓
                SetPLCCustVar("INT", 1, 200) --任务完成
            -- === 任务 102: 轴 (夹爪) ===
            elseif cmd == 102 then
                SetPLCCustVar("INT", 1, 100) --开始工作
                local zqd = pianyi(轴抓取, dx, dy, da) --获取抓取点偏移
                banyun(zqd, 轴放置, 过渡4, 过渡5, 0, 1, -200,0,2) --搬运到放置点
                banyun(托盘传送带, tuopan(托盘料仓,n), 过渡2, 过渡2, 3, 0,-200,0,1) --托盘入仓
                SetPLCCustVar("INT", 1, 200) --任务完成
            -- === 任务 103: 装配并出库 ===
            elseif cmd == 103 then
                SetPLCCustVar("INT", 1, 300) --开始工作
                --把大圆抓到装配位置，先加紧，再下翻
                banyun(大圆放置, 大圆装配, 过渡2, 过渡3, 2, 0,0,0,1)
                SDO(6, 500) --大圆加紧
                SDO(5, 500) --大圆下翻
                --把大圆抓到装配位置，先加紧，再下翻
                banyun(小圆放置, 小圆装配, 过渡2, 过渡3, 2, 0,0,0,1)
                SDO(8, 500) --小圆加紧
                SDO(7, 500) --小圆下翻
                --把轴抓到装配位置，加紧
                banyun(轴放置, 轴装配, 过渡5, 过渡6, 0, 1, 0,0,2)
                SDO(4, 500) --轴加紧
                --大圆小圆松开夹爪
                RDO(6, 500) --大圆松开
                RDO(8, 500) --小圆松开
                RDO(4, 500) --轴松开
                --装配完成，把轴搬走
                banyun(轴装配, 轴放置, 过渡6, 过渡5, 0, 1,0,0,2)
                RDO(5, 500) --上翻托盘
                RDO(7, 500) --上翻托盘
                -- 吸一个托盘出去
                banyun(tuopan(托盘料仓,n), 托盘传送带, 过渡2, 过渡1, 3, 0, 0,-200,1)
                -- 把成品搬到传送带的出库位置
                banyun(轴放置, 成品放置, 过渡5, 过渡4, 0, 1, 0,-200,2)
                -- 回原点
                PTP(原点, 100, -1, 0)
                SetPLCCustVar("INT", 1, 400) --任务完成
            -- === 任务 99: 复位 ===
            elseif cmd == 99 then
                chushihua()
            elseif cmd == 1 then
                SetPLCCustVar("INT", 1, 0) --待机状态
            end
            cmd = 0 -- 清除命令，等待下一个指令
        end
        -- 等待PLC写入新命令
        WaitMs(50)
    end
end

法奥机器人完整程序

参数全解析

L型避障逻辑图解

视觉偏移 (pianyi)

托盘层高 (tuopan)