Proeasy机器人:二轴增加延伸臂案例

--- <summary>*返回指定PUU的ACS坐标系点位\n </summary>
--- <argument name="J1_PUU">指定点位J1的PUU，单位PUU，不给定时为0</argument>
--- <argument name="J2_PUU">指定点位J2的PUU，单位PUU，不给定时为0</argument>
--- <argument name="J3_PUU">指定点位J3的PUU，单位PUU，不给定时为0</argument>
--- <argument name="J4_PUU">指定点位J4的PUU，单位PUU，不给定时为0</argument>
--- <argument name="J5_PUU">指定点位J5的PUU，单位PUU，不给定时为0</argument>
--- <argument name="J6_PUU">指定点位J6的PUU，单位PUU，不给定时为0</argument>
function PUU(J1_PUU,J2_PUU,J3_PUU,J4_PUU,J5_PUU,J6_PUU)
    return {x=J1_PUU,y=J2_PUU,z=J3_PUU,a=J4_PUU,b=J5_PUU,c=J6_PUU,Coord=3, Active=1}
end

--- <summary>延伸臂参数设定</summary>
--- <summary>只在SCARA类机构生效</summary>
--- <argument name="LeftHand_P">左手参考点，需要ACS 模式示教</argument>
--- <argument name="RightHand_P">右手参考点，需要ACS 模式示教</argument>
--- <argument name="tTF">生效的工具坐标编号</argument>
function AuxArmDef(LeftHand_P,RightHand_P,tTF)
    if type(LeftHand_P) ~= "number" and type(LeftHand_P) ~= "table" then
        motion.ProgramStop("Input type [LeftHand_P] is error !\n")
    elseif type(LeftHand_P) == "number"  then
        LeftHand_P= P[LeftHand_P]
    end
    if type(RightHand_P) ~= "number" and type(RightHand_P) ~= "table" then
        motion.ProgramStop("Input type [RightHand_P] is error !\n")
    elseif type(RightHand_P) == "number"  then
        RightHand_P= P[RightHand_P]
    end
    if LeftHand_P.Coord~=3 or RightHand_P.Coord~=3 then
        motion.ProgramStop("Input Point.Coord is not ACS mode !\n")
    end

para.Write(0,2,7,0x00010013)
para.Write(0,2,8,LeftHand_P.x)
para.Write(0,2,7,0x01010013)
para.Write(0,2,8,LeftHand_P.y)
para.Write(0,2,6,tonumber("0x0".. tostring(tTF).."011232"))    -- 注意此處第7bit(右邊數來第七個)需修改為欲使用之工具座標系編號(此例為2號工具座標系)
DELAY(0.004)

para.Write(0,2,7,0x00010013)
para.Write(0,2,8,RightHand_P.x)
para.Write(0,2,7,0x01010013)
para.Write(0,2,8,RightHand_P.y)
para.Write(0,2,6,tonumber("0x0".. tostring(tTF).."012232"))    -- 注意此處第7bit(右邊數來第七個)需修改為欲使用之工具座標系編號
DELAY(0.004)

end
---------------------------------------------------------

-- 延伸臂旗標寫入
--- <summary>延伸臂功能开关</summary>
--- <summary>不输入Switch时为读取当前tf是否开启了延伸臂功能</summary>
--- <argument name="tTF">延伸臂占用的工具坐标编号</argument>
--- <argument name="Switch">开关，类型为Number，范围为0~1</argument>
function AuxArm(tTF,Switch) 
    if Switch == nil then
        -- Print and Check
        para.Write(0,2,6,tonumber("0x0".. tostring(tTF).."000132"))    -- 注意此處第7bit(右邊數來第七個)需修改為欲使用之工具座標系編號
        local valid,ToolFrameType_read = para.Read(0,2,9)
        DELAY(0.004)
        --print('ToolFrameType_read = ',ToolFrameType_read,'\n')
        return ToolFrameType_read
    elseif type(Switch) == "boolean" then
        if Switch then Switch = 1 else Switch = 0 end
    elseif type(Switch) == "number" then
        if Switch >1 or Switch < 0 then 
            motion.ProgramStop("Input value [Switch] is out of range !\n")
        end
    else
            motion.ProgramStop("Input type [Switch] is error !\n")
    end
    
    para.Write(0,2,7,0x00010013)
    para.Write(0,2,8,Switch)
    para.Write(0,2,6,tonumber("0x0".. tostring(tTF).."010132"))    -- 注意此處第7bit(右邊數來第七個)需修改為欲使用之工具座標系編號
    DELAY(0.004)
    para.Write(0,2,6,tonumber("0x0".. tostring(tTF).."010731"))    -- 注意此處第7bit(右邊數來第七個)需修改為欲使用之工具座標系編號
    DELAY(0.004)
end


A = PUU(3225955,-1809271)         ----左手系点J1,J2的PUU值
B = PUU(712935,1801774)           ----右手系点J1,J2的PUU值
AuxArmDef(A,B,1)
AuxArm(1,1)