案例/模板说明

机械臂主控主函数说明

以下为机械臂主控模板代码，包含坐标移动基础模板、坐标移动二次定位模板、轨迹移动同步模板、轨迹移动异步模板。

用户需要根据项目情况，参考模板程序编写机械臂前台主程序。

nachi的一部分全局变量已经被占用，用户不允许使用这些全局变量，详见nachi api界面。

关于模板中的 API 可以查阅 nachi 的 “API 说明” 部分。

请注意模板函数并不能直接运行，请一定根据项目现场环境和流程需求进行修改

坐标移动基础模板

MZ04-01-A.001

INCLUDE "XYZSETUP"

'
' S1: 初始化
'
' 初始化
CallProc MasterInit()
' 关闭用户任务300
KILLMCR 300, 0, 10000
DELAY 2
' 启动用户任务300
FORKMCR 300, 10000
DELAY 2
' io设置
RESET O1

'
' S2: 连接到Max
'
' 设置服务端ip地址(ip地址的最后一段)
MST_IP = 101
' 设置端口号
MST_PORT = 11111
CallProc xyzMasterConnect()

' 切换任务流图，根据需要取消注释
' MST_TASK_CODENAME = "cart_basic.t"
' CallProc xyzSwitchTask()
' CallProc CheckErrorCode()

' 重置任务
CallProc xyzResetTask()
CallProc CheckErrorCode()

'
' S3: 切换成当前工件
'
' 设置视觉服务id
MST_VS_ID = 0
' 设置工件代号
MST_ITEM_CODENAME = "item1"
CallProc xyzSwitchItem()
CallProc CheckErrorCode()

'
' S4: 运动到Home点
'
' 需要提前示教Home点
MOVEX A=1P, AC=0, SM=0, M1X, P, (100.0,200.0,300.0,0.0,0.0,0.0),R=100.0,MS

' 如果是眼在手上的案例，则将V%[100]设置为1，否则设置为0
V%[100] = 1


*MAIN_LOOP

'
' S5: 眼在手上
'
IF V%[100] = 1
    '
    ' S6: 运动到拍照位姿点
    ' 需要提前示教拍照位姿
    MOVEX A=1P, AC=0, SM=0, M1X, L, (100.0,200.0,300.0,0.0,0.0,0.0),R=100.0,MS

    '
    ' S7: 发送拍照位姿
    '
    CallProc xyzSendCurrentCartPose()
    CallProc CheckErrorCode()
ENDIF

'
' S8: 请求抓取目标点位
'
MST_VS_ID = 0
CallProc xyzReqGraspPose()
CallProc CheckErrorCode()
V%[101] = MST_OUT_TOKEN

'
' S9: 获取抓取目标点位
'
MST_SET_TOKEN = V%[101]
CallProc xyzGetGraspPose()
CallProc CheckErrorCode()
IF MST_POSE_NUM < 1
    DELAY 5
    GOTO *MAIN_LOOP
ENDIF

'
' S10: 运动到抓取点位
'
' 首先走到抓取点位上方
MOVEX A=3, AC=0, SM=0, M1X, L, (MST_POSE_X,MST_POSE_Y,MST_POSE_Z+100,MST_POSE_A,MST_POSE_B,MST_POSE_C),R=100.0,MS
' 抓取
MOVEX A=1P, AC=0, SM=0, M1X, L, (MST_POSE_X,MST_POSE_Y,MST_POSE_Z,MST_POSE_A,MST_POSE_B,MST_POSE_C),R=100.0,MS
SET O1

'
' S11: 运动到放置位姿
'
' 首先走到抓取点位上方
MOVEX A=3, AC=0, SM=0, M1X, L, (MST_POSE_X,MST_POSE_Y,MST_POSE_Z+200,MST_POSE_A,MST_POSE_B,MST_POSE_C),R=100.0,MS
' 走到放置位姿
' 放置位姿需要提前示教好
MOVEX A=1P, AC=0, SM=0, M1X, L, (100.0,200.0,300.0,0.0,0.0,0.0),R=100.0,MS
RESET O1

GOTO *MAIN_LOOP

END

坐标移动二次定位模板

MZ04-01-A.002

INCLUDE "XYZSETUP"

'
' S1: 初始化
'
' 初始化
CallProc MasterInit()
' 关闭用户任务300
KILLMCR 300, 0, 10000
DELAY 2
' 启动用户任务300
FORKMCR 300, 10000
DELAY 2
' io设置
RESET O1
RESET O2

'
' S2: 连接到Max
'
' 设置服务端ip地址(ip地址的最后一段)
MST_IP = 101
' 设置端口号
MST_PORT = 11111
CallProc xyzMasterConnect()

' 切换任务流图，根据需要取消注释
' MST_TASK_CODENAME = "cart_repo.t"
' CallProc xyzSwitchTask()
' CallProc CheckErrorCode()

' 重置任务
CallProc xyzResetTask()
CallProc CheckErrorCode()

'
' S3: 切换成当前工件
'
' 视觉服务id
MST_VS_ID = 0
' 工件代号
MST_ITEM_CODENAME = "item1"
CallProc xyzSwitchItem()
CallProc CheckErrorCode()


'
' S4: 运动到Home点
'
' 需要提前示教Home点
MOVEX A=1P, AC=0, SM=0, M1X, P, (100.0,200.0,300.0,0.0,0.0,0.0),R=100.0,MS

*MAIN_LOOP

'
' S5+S6: 请求+获取抓取目标点位
'
MST_VS_ID = 0
CallProc xyzCalculateGraspPose()
CallProc CheckErrorCode()

'
' S7: 判断是否识别到工件
'
IF MST_POSE_NUM < 1
    '
    ' S15: 没有识别到工件，机械臂外的相机切换识别隔板
    '
    ' set vs_id
    MST_VS_ID = 0
    ' set item_codename
    MST_ITEM_CODENAME = "board"
    CallProc xyzSwitchItem()
    CallProc CheckErrorCode()

    '
    ' S16+S17: 请求+获取隔板抓取位姿
    '
    MST_VS_ID = 0
    CallProc xyzCalculateGraspPose()
    CallProc CheckErrorCode()
    IF MST_POSE_NUM < 1
        ' 未识别到隔板的处理
        GOTO *ERROR_EXIT
    ENDIF

    '
    ' S18: 运动到隔板抓取位姿，并抓取隔板
    '
    ' 首先走到隔板上方
    MOVEX A=3, AC=0, SM=0, M1X, L, (MST_POSE_X,MST_POSE_Y,MST_POSE_Z+100,MST_POSE_A,MST_POSE_B,MST_POSE_C),R=100.0,MS
    ' 抓隔板
    MOVEX A=1P, AC=0, SM=0, M1X, L, (MST_POSE_X,MST_POSE_Y,MST_POSE_Z,MST_POSE_A,MST_POSE_B,MST_POSE_C),R=100.0,MS
    SET O1

    '
    ' S19: 运动到隔板的放置位姿
    '
    ' 首先走到隔板上方
    MOVEX A=3, AC=0, SM=0, M1X, L, (MST_POSE_X,MST_POSE_Y,MST_POSE_Z+200,MST_POSE_A,MST_POSE_B,MST_POSE_C),R=100.0,MS
    ' 走到隔壁放置位姿
    ' 需要提前示教好放置位姿
    MOVEX A=1P, AC=0, SM=0, M1X, L, (100.0,200.0,300.0,0.0,0.0,0.0),R=100.0,MS
    RESET O1

    '
    ' S20: 机械臂外的相机切换成识别当前工件
    '
    ' set vs_id
    MST_VS_ID = 0
    ' set item_codename
    MST_ITEM_CODENAME = "item1"
    CallProc xyzSwitchItem()
    CallProc CheckErrorCode()

ELSE

    '
    ' S8: 运动到拍照位姿
    ' 需要提前示教好拍照位姿
    MOVEX A=1P, AC=0, SM=0, M1X, L, (100.0,200.0,300.0,0.0,0.0,0.0),R=100.0,MS

    '
    ' S9: 发送拍照位姿
    '
    CallProc xyzSendCurrentCartPose()
    CallProc CheckErrorCode()

    '
    ' S10: 机械臂上的相机切换成识别当前工件
    '
    ' set vs_id
    MST_VS_ID = 1
    ' set item_codename
    MST_ITEM_CODENAME = "item1"
    CallProc xyzSwitchItem()
    CallProc CheckErrorCode()

    '
    ' S11+S12: 机械臂上的相机请求+获取工件的抓取位姿
    '
    MST_VS_ID = 1
    CallProc xyzCalculateGraspPose()
    CallProc CheckErrorCode()
    IF MST_POSE_NUM < 1
        GOTO *ERROR_EXIT
    ENDIF

    '
    ' S13: 运动到抓取点位
    '
    ' 首先走到抓取点位上方
    MOVEX A=3, AC=0, SM=0, M1X, L, (MST_POSE_X,MST_POSE_Y,MST_POSE_Z+100,MST_POSE_A,MST_POSE_B,MST_POSE_C),R=100.0,MS
    ' 抓取
    MOVEX A=1P, AC=0, SM=0, M1X, L, (MST_POSE_X,MST_POSE_Y,MST_POSE_Z,MST_POSE_A,MST_POSE_B,MST_POSE_C),R=100.0,MS
    SET O2

    '
    ' S14: 运动到放置位姿
    '
    ' 先走到抓取点位上方
    MOVEX A=3, AC=0, SM=0, M1X, L, (MST_POSE_X,MST_POSE_Y,MST_POSE_Z+200,MST_POSE_A,MST_POSE_B,MST_POSE_C),R=100.0,MS
    ' 走到放置位姿
    ' 放置位姿需要提前示教好
    MOVEX A=1P, AC=0, SM=0, M1X, L, (100.0,200.0,300.0,0.0,0.0,0.0),R=100.0,MS
    RESET O2

ENDIF

GOTO *MAIN_LOOP

*ERROR_EXIT
STOP

END

轨迹移动同步模板

MZ04-01-A.003

INCLUDE "XYZSETUP"

'
' S1: 初始化
'
' 初始化
CallProc MasterInit()
' 关闭用户任务300
KILLMCR 300, 0, 10000
DELAY 2
' 启动用户任务300
FORKMCR 300, 10000
DELAY 2
' io设置
RESET O1

'
' S2: 连接到Max
'
' 设置服务端ip地址(ip地址的最后一段)
MST_IP = 101
' 设置端口号
MST_PORT = 11111
CallProc xyzMasterConnect()

' 切换任务流图，根据需要取消注释
' MST_TASK_CODENAME = "traj_sync.t"
' CallProc xyzSwitchTask()
' CallProc CheckErrorCode()

' 重置任务
CallProc xyzResetTask()
CallProc CheckErrorCode()

'
' S3: 切换成当前工件
'
' set vs_id
MST_VS_ID = 0
' set item_codename
MST_ITEM_CODENAME = "item1"
CallProc xyzSwitchItem()
CallProc CheckErrorCode()

'
' S4: 运动到Home点
'
' 需要提前示教Home点
MOVEX A=1P, AC=0, SM=0, M1X, P, (100.0,200.0,300.0,0.0,0.0,0.0),R=100.0,MS

*MAIN_LOOP

'
' S5: 请求抓取和放置规划
'
' set vs_id
MST_VS_ID = 0
CallProc xyzReqPickPlace()
CallProc CheckErrorCode()

'
' S6: 获取 pick-in 轨迹
'
' set vs_id
MST_VS_ID = 0
CallProc xyzGetPickIn()
CallProc CheckErrorCode()

'
' S7: 检查是否识别到工件
'
IF MST_POSE_NUM < 1
    GOTO *ERROR_EXIT
ENDIF

'
' S8: 执行 pick-in 轨迹
'
CallProc xyzExecPickInTraj()
SET O1

'
' S9: 获取 pick-out 轨迹
'
' set vs_id
MST_VS_ID = 0
CallProc xyzGetPickOut()
CallProc CheckErrorCode()

'
' S10: 执行 pick-out 轨迹
'
CallProc xyzExecPickOutTraj()

'
' S11: 获取 place-in 轨迹
'
' set vs_id
MST_VS_ID = 0
CallProc xyzGetPlaceIn()
CallProc CheckErrorCode()

'
' S12: 执行 place-in 轨迹
'
CallProc xyzExecGeneralTraj()
RESET O1

'
' S13: 获取 place-out 轨迹
'
' set vs_id
MST_VS_ID = 0
CallProc xyzGetPlaceOut()
CallProc CheckErrorCode()

'
' S14: 执行 place-out 轨迹
'
CallProc xyzExecGeneralTraj()

GOTO *MAIN_LOOP

*ERROR_EXIT
STOP

END

轨迹移动异步模板

MZ04-01-A.004

INCLUDE "XYZSETUP"

'
' S1: 初始化
'
' 初始化
CallProc MasterInit()
' 关闭用户任务300
KILLMCR 300, 0, 10000
DELAY 2
' 启动用户任务300
FORKMCR 300, 10000
DELAY 2
' io设置
RESET O1

'
' S2: 连接到Max
'
' 设置服务端ip地址(ip地址的最后一段)
MST_IP = 101
' 设置端口号
MST_PORT = 11111
CallProc xyzMasterConnect()

' 切换任务流图，根据需要取消注释
' MST_TASK_CODENAME = "traj_async.t"
' CallProc xyzSwitchTask()
' CallProc CheckErrorCode()

' 重置任务
CallProc xyzResetTask()
CallProc CheckErrorCode()

'
' S3: 切换成当前工件
'
' set vs_id
MST_VS_ID = 0
' set item_codename
MST_ITEM_CODENAME = "item1"
CallProc xyzSwitchItem()
CallProc CheckErrorCode()

'
' S4: 运动到Home点
'
' 需要提前示教Home点
MOVEX A=1P, AC=0, SM=0, M1X, P, (100.0,200.0,300.0,0.0,0.0,0.0),R=100.0,MS

'
' S5: 请求抓取和放置规划
'
' set vs_id
MST_VS_ID = 0
CallProc xyzReqPickPlace()
CallProc CheckErrorCode()

*MAIN_LOOP

'
' S6: 获取 pick-in 轨迹
'
' set vs_id
MST_VS_ID = 0
CallProc xyzGetPickIn()
CallProc CheckErrorCode()

'
' S7: 检查是否识别到工件
'
IF MST_POSE_NUM < 1
    GOTO *ERROR_EXIT
ENDIF

'
' S8: 执行 pick-in 轨迹
'
CallProc xyzExecPickInTraj()
SET O1

'
' S9: 获取 pick-out 轨迹
'
' set vs_id
MST_VS_ID = 0
CallProc xyzGetPickOut()
CallProc CheckErrorCode()

'
' S10: 执行 pick-out 轨迹
'
CallProc xyzExecPickOutTraj()

'
' S11: 请求下一次的抓取和放置规划
'
' set vs_id
MST_VS_ID = 0
CallProc xyzReqPickPlace()
CallProc CheckErrorCode()

'
' S12: 获取本次 place-in 轨迹
'
' set vs_id
MST_VS_ID = 0
CallProc xyzGetPlaceIn()
CallProc CheckErrorCode()

'
' S13: 执行本次 place-in 轨迹
'
CallProc xyzExecGeneralTraj()
RESET O1

'
' S14: 获取本次 place-out 轨迹
'
' set vs_id
MST_VS_ID = 0
CallProc xyzGetPlaceOut()
CallProc CheckErrorCode()

'
' S15: 执行本次 place-out 轨迹
'
CallProc xyzExecGeneralTraj()

GOTO *MAIN_LOOP

*ERROR_EXIT
STOP

END