案例/模板说明 ============================ 机械臂主控主函数说明 ++++++++++++++++++++++++++++++++ 以下为机械臂主控模板代码，包含坐标移动基础模板、坐标移动二次定位模板、轨迹移动同步模板、轨迹移动异步模板。 用户需要根据项目情况，参考模板程序编写机械臂前台主程序。 nachi的一部分全局变量已经被占用，用户不允许使用这些全局变量，详见nachi api界面。 关于模板中的 API 可以查阅 nachi 的 “API 说明” 部分。 **请注意模板函数并不能直接运行，请一定根据项目现场环境和流程需求进行修改** 坐标移动基础模板 -------------------------------------- ``MZ04-01-A.001`` .. code-block:: 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`` .. code-block:: 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`` .. code-block:: 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`` .. code-block:: 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