案例/模板说明 ============================ 机械臂主控主函数说明 ++++++++++++++++++++++++++++++++ 注意对工控机返回的 err_code 进行判断。 轨迹移动同步模板： ---------------------------------- .. code-block:: Func run() LB[1] = 0; While LB[1] <>1 Open Socket("192.168.37.101",11111,2,Single, LB[1]); # 打开端口2 gMasterPort = 2; Get Port[gMasterPort],T[0.2],Goto L[2]; # 获得端口2数据，清空端口2缓存 L[2]: If GetPortState(gMasterPort) <> 1 Halt 3; Goto L[1]; EndIf; Int err_code; gMasterFlag = True; #-------------------------------------------# String flow_name ="traj_sync.t"; # 切换任务 api.xyzSwitchFlow(flow_name, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; Int ws_id = 0; String item_codename = "item1"; # 切换工件 api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # io operation # 初始化IO Set Out[1],ON; # move To Home # 移动到初始位姿 LP[1] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj LP[1], V[30], Z[0], Tool[0]; Int pipeline_num = 0; Int register_num = 0; Int num = 0; Int wp_type[30]={}; Double pos[30][6]={}; While True ws_id = 0; api.xyzReqPickPlace(ws_id,err_code); # 请求计算抓取放置 If err_code <> 0 Halt 3; Goto L[1]; EndIf; ws_id = 0; api.xyzGetPickin(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); # 获取抓取入筐位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; If num < 1 Goto L[1]; EndIf; api.xyzExecutePickInTraj(num, wp_type, pos); # 执行抓取入筐位姿 # io operation Set Out[1],OFF; ws_id = 0; api.xyzGetPickout(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); # 获取抓取出筐位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; api.xyzExecutePickOutTraj(num, wp_type, pos); # 执行抓取出筐位姿 ws_id = 0; api.xyzGetPlacein(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); # 获取放置入筐位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; api.xyzExecuteTraj(num, wp_type, pos); # 执行放置入筐位姿 # io operation Set Out[1],ON; ws_id = 0; api.xyzGetPlaceout(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); # 获取放置出筐位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; api.xyzExecuteTraj(num, wp_type, pos); # 执行放置出筐位姿 EndWhile; L[1]: Close Socket, gMasterPort; EndFunc; 轨迹移动异步模板 ------------------------------------- .. code-block:: Func run() LB[1] = 0; While LB[1] <>1 Open Socket("192.168.37.101",11111,2,Single, LB[1]); # 打开端口2 EndWhile; gMasterPort = 2; Get Port[gMasterPort],T[0.2],Goto L[2]; # 获得端口2数据，清空端口2缓存 L[2]: If GetPortState(gMasterPort) <> 1 Halt 3; Goto L[1]; EndIf; Int err_code; gMasterFlag = True; #-------------------------------------------# String flow_name ="traj_async.t"; # 切换任务 api.xyzSwitchFlow(flow_name, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; Int ws_id = 0; String item_codename = "item1"; # 切换工件 api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # io operation # 初始化IO Set Out[1],ON; # move To Home # 移动到初始位姿 LP[1] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj LP[1], V[30], Z[0], Tool[0]; Int pipeline_num = 0; Int register_num = 0; Int num = 0; Int wp_type[30]={}; Double pos[30][6]={}; While True ws_id = 0; api.xyzReqPickPlace(ws_id,err_code); # 请求计算抓取放置 If err_code <> 0 Halt 3; Goto L[1]; EndIf; ws_id = 0; api.xyzGetPickin(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); # 获取抓取入筐位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; If num < 1 # 没有可抓取位姿，结束运行 Goto L[1]; EndIf; api.xyzExecutePickInTraj(num, wp_type, pos); # 执行抓取入筐位姿 # io operation # 抓取 Set Out[1],OFF; ws_id = 0; api.xyzGetPickout(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); # 获取抓取出筐位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; api.xyzExecutePickOutTraj(num, wp_type, pos); # 执行抓取出筐位姿 # request next pick and place plan in advance # 提前请求下一次抓取放置 ws_id = 0; api.xyzReqPickPlace(ws_id,err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; ws_id = 0; api.xyzGetPlacein(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); # 获取放置入筐位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; api.xyzExecuteTraj(num, wp_type, pos); # 执行放置入筐位姿 # io operation Set Out[1],ON; ws_id = 0; api.xyzGetPlaceout(ws_id,err_code,pipeline_num,register_num,num,wp_type,pos); # 获取放置出筐位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; api.xyzExecuteTraj(num, wp_type, pos); # 执行放置出筐位姿 EndWhile; L[1]: Close Socket, gMasterPort; EndFunc; 座标移动基础模板： ------------------------------------- .. code-block:: Func run() LB[1] = 0; While LB[1] <>1 Open Socket("192.168.37.101",11111,2,Single, LB[1]); # 打开端口2 EndWhile; gMasterPort = 2; Get Port[gMasterPort],T[0.2],Goto L[2]; # 获得端口2数据，清空端口2缓存 L[2]: If GetPortState(gMasterPort) <> 1 Halt 3; Goto L[1]; EndIf; Int err_code; gMasterFlag = True; #-------------------------------------------# String flow_name ="cart_basic.t"; # 切换任务 api.xyzSwitchFlow(flow_name, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; Int ws_id = 0; String item_codename = "item1"; # 切换工件 api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # io operation # 初始化IO Set Out[1],ON; Bool is_eye_in_hand = True; Int grasp_pose_token = 0; Double grasp_pose[6] = {0,0,0,0,0,0}; Int grasp_pose_num = 0; Int pipeline_num = 0; Int register_num = 0; While True If is_eye_in_hand # 如果眼在手上 # Move To scan pose LP[1] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj LP[1], V[30], Z[0], Tool[0]; # 移动到拍照位姿 api.xyzSendCurrentCartPose(err_code); # 发送当前位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; EndIf; # req grasp pose(including cap image) # 请求抓取位姿（包含拍照） grasp_pose_token = 0; api.xyzReqGraspPose(0, err_code, grasp_pose_token); # 请求抓取位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; grasp_pose_num = 0; pipeline_num = 0; register_num = 0; # 获取抓取位姿 api.xyzGetGraspPose(grasp_pose_token, err_code, grasp_pose, grasp_pose_num, pipeline_num, register_num); If err_code <> 0 Halt 3; Goto L[1]; EndIf; LP[2] = (grasp_pose[0],grasp_pose[1],grasp_pose[2],grasp_pose[3],grasp_pose[4],grasp_pose[5]),(0,0,0,1),(2,0,0); If grasp_pose_num < 1 Wait T[5]; Continue; EndIf; # Move To grasp pose # 移动到抓取位姿 Movj LP[2], V[30], Z[0], Tool[0]; # io operation # 抓取 Set Out[1],OFF; # Move To place pose # 移动到放置位姿 LP[3] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj LP[3], V[30], Z[0], Tool[0]; # io operation # 放置 Set Out[1],ON; EndWhile; L[1]: Close Socket, gMasterPort; EndFunc; 座标移动二次定位模板 -------------------------------------- .. code-block:: Func run() LB[1] = 0; While LB[1] <>1 Open Socket("192.168.37.101",11111,2,Single, LB[1]); # 打开端口2 EndWhile; gMasterPort = 2; Get Port[gMasterPort],T[0.2],Goto L[2]; # 获得端口2数据，清空端口2缓存 L[2]: If GetPortState(gMasterPort) <> 1 Halt 3; Goto L[1]; EndIf; Int err_code; gMasterFlag = True; #-------------------------------------------# String flow_name ="cart_repo.t"; # 切换任务 api.xyzSwitchFlow(flow_name, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # io operation # 初始化IO Set Out[1],ON; # Move To Home # 移动到初始位姿 LP[1] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj LP[1], V[30], Z[0], Tool[0]; Int ws_id = 0; String item_codename = "item1"; # 切换工件 api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; Int rough_grasp_pose_token = 0; Double rough_grasp_pose[6] = {0,0,0,0,0,0}; Int rough_grasp_pose_num = 0; Int rough_pipeline_num = 0; Int rough_register_num = 0; String board_name; Int board_grasp_pose_token = 0; Double board_grasp_pose[6] = {0,0,0,0,0,0}; Int board_grasp_pose_num = 0; Int board_pipeline_num = 0; Int board_register_num = 0; Int fine_grasp_pose_token = 0; Double fine_grasp_pose[6] = {0,0,0,0,0,0}; Int fine_grasp_pose_num = 0; Int fine_pipeline_num = 0; Int fine_register_num = 0; rough_grasp_pose_token = 0; api.xyzReqGraspPose(0, err_code, rough_grasp_pose_token); # 请求抓取位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; While True # camera1 get rough grasp pose # 获取粗抓取位姿 api.xyzGetGraspPose(rough_grasp_pose_token, err_code, rough_grasp_pose, rough_grasp_pose_num, rough_pipeline_num, rough_register_num); If err_code <> 0 Halt 3; Goto L[1]; EndIf; # grasp board if no object exists on board # 如果没有物体在板子上，抓取板子 If (rough_grasp_pose_num < 1) # switch board; ws_id = 0; board_name = "board"; # 切换工件 api.xyzSwitchItem(ws_id, board_name, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; api.xyzReqGraspPose(0, err_code, board_grasp_pose_token); # 请求抓取位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; # get board grasp pose # 获取板子抓取位姿 api.xyzGetGraspPose(board_grasp_pose_token, err_code, board_grasp_pose, board_grasp_pose_num, board_pipeline_num, board_register_num); If err_code <> 0 Halt 3; Goto L[1]; EndIf; LP[2] = (board_grasp_pose[0],board_grasp_pose[1],board_grasp_pose[2],board_grasp_pose[3],board_grasp_pose[4],board_grasp_pose[5]),(0,0,0,1),(2,0,0); If (board_grasp_pose_num < 1) Halt 3; Goto L[1]; EndIf; # Move To board grasp pose # 移动到板子抓取位姿 Movj LP[2], V[30], Z[0], Tool[0]; # io operation # 抓取 Set Out[1],OFF; # Move To boardplace pose # 移动到板子放置位姿 LP[3] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj LP[3], V[30], Z[0], Tool[0]; # io operation # 放置 Set Out[1],ON; item_codename = "item1"; # 切换工件 api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; rough_grasp_pose_token = 0; api.xyzReqGraspPose(0, err_code, rough_grasp_pose_token); # 请求粗抓取位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; Continue; EndIf; # move To rough_grasp_pose # 移动到粗抓取位姿 LP[4] = (rough_grasp_pose[0],rough_grasp_pose[1],rough_grasp_pose[2],rough_grasp_pose[3],rough_grasp_pose[4],rough_grasp_pose[5]),(0,0,0,1),(2,0,0); Movj LP[4], V[30], Z[0], Tool[0]; # Send cart pose with eye In hand # 发送当前位姿 api.xyzSendCurrentCartPose(err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; ws_id = 1; item_codename = "item1"; # 切换工件 api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; fine_grasp_pose_token = 0; api.xyzReqGraspPose(0, err_code, fine_grasp_pose_token); # 请求精确抓取位姿 If err_code <> 0 Halt 3; Goto L[1]; EndIf; # get fine grasp pose # 获取精确抓取位姿 api.xyzGetGraspPose(fine_grasp_pose_token, err_code, fine_grasp_pose, fine_grasp_pose_num, fine_pipeline_num, fine_register_num); If err_code <> 0 Halt 3; Goto L[1]; EndIf; If (fine_grasp_pose_num < 1) Halt 3; Goto L[1]; EndIf; LP[5] = (fine_grasp_pose[0],fine_grasp_pose[1],fine_grasp_pose[2],fine_grasp_pose[3],fine_grasp_pose[4],fine_grasp_pose[5]),(0,0,0,1),(2,0,0); # Move To Fine grasp pose # 移动到精确抓取位姿 Movj LP[5], V[30], Z[0], Tool[0]; # io operation # 抓取 Set Out[1],OFF; # Move To place pose # 移动到放置位姿 LP[6] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0); Movj LP[6], V[30], Z[0], Tool[0]; # io operation # 放置 Set Out[1],ON; item_codename = "item1"; # 切换工件 api.xyzSwitchItem(ws_id, item_codename, err_code); If err_code <> 0 Halt 3; Goto L[1]; EndIf; rough_grasp_pose_token = 0; # request next grasp pose # 请求下一次粗抓取位姿 api.xyzReqGraspPose(0, err_code, rough_grasp_pose_token); If err_code <> 0 Halt 3; Goto L[1]; EndIf; EndWhile; L[1]: Close Socket, gMasterPort; EndFunc;