案例/模板说明 ============================ 机械臂主控主函数说明 ++++++++++++++++++++++++++++++++ 以下为机械臂主控模板代码，注意对工控机返回的 err_code 进行判断。 .. code-block:: .PROGRAM cartmovebasic() ;------------------------------------------------------------------------ ;cart move 基础模板 ;------------------------------------------------------------------------ ; S1: 初始化参数 CALL set_const CP ON MESSAGE ON CALL close_all_sock ; 相关运动参数需要合理设置 SPEED 100 ALWAYS ACCEL 100 ALWAYS ACCURACY 10 ALWAYS ; 数字输出信号需要合理设置 SIGNAL -1 ; 一些示教点位需要合理设置 POINT #HOME = #PPOINT(0,0,0,0,0,0) POINT cart_wp1 = TRANS(0,0,0,0,0,0) ; scan pose POINT cart_wp2 = TRANS(0,0,0,0,0,0) .eye_on_hand = 0 ; 眼在手上标志位 ; S2: 连接到上位机 CALL xyz_conn(sock_id, IP[], PORT) ; CALL xyz_sw_flw(.error_code, "test.t") ; IF .error_code <> 0 GOTO LABEL_FAIL ; S3: 切换工件 CALL xyz_sw_item(.error_code, 0, "obj1") IF .error_code <> 0 GOTO LABEL_FAIL ; S4: 移动到home点，需要提前设置好home点位置 JMOVE #HOME WHILE 1 DO LABEL_START: ; S5: 眼在手上 IF .eye_on_hand == 1 THEN ; S6: 移动到拍照位 ; you should define scan_pose first LMOVE cart_wp1 POINT sending_pose = cart_wp1 ; S7: 发送拍照位姿给上位机 CALL xyz_s_c_pose(.error_code) IF .error_code <> 0 GOTO LABEL_FAIL END ; S8: 请求抓取位姿 CALL xyz_r_g_pose(.error_code, 0, .token) IF .error_code <> 0 GOTO LABEL_FAIL ; S9: 获取抓取位姿 CALL xyz_g_g_pose(.error_code, .token, .pose_num, .pipe_num, .reg_num) IF .error_code <> 0 GOTO LABEL_FAIL IF .pose_num < 1 THEN TWAIT 5 GOTO LABEL_START END ; S10: 移动到抓取位并抓取 ; 注意，可能需要添加一些过渡点 LMOVE grasp_pose SIGNAL 1 ; S11: 移动到放置位并放置 LMOVE cart_wp2 SIGNAL -1 END LABEL_FAIL: CALL xyz_disconn(.ret, sock_id) .END .PROGRAM cartmoverepo() ;------------------------------------------------------------------------ ;cart move 重定位模板 ;------------------------------------------------------------------------ ; S1: 初始化参数 CALL set_const CP ON MESSAGE ON CALL close_all_sock ; 相关运动参数需要合理设置 SPEED 100 ALWAYS ACCEL 100 ALWAYS ACCURACY 10 ALWAYS ; 数字输出信号需要合理设置 SIGNAL -1 SIGNAL -2 ; 一些示教点位需要合理设置 POINT #HOME = #PPOINT(0,0,0,0,0,0) POINT cart_wp1 = TRANS(0,0,0,0,0,0) ; scan pose POINT cart_wp2 = TRANS(0,0,0,0,0,0) ; S2: 连接到上位机 CALL xyz_conn(sock_id, IP[], PORT) ;CALL xyz_sw_flw(.error_code, "test.t") ;IF .error_code <> 0 GOTO LABEL_FAIL ; S3: 切换工件 CALL xyz_sw_item(.error_code, 0, "obj1") IF .error_code <> 0 GOTO LABEL_FAIL ; S4: 移动到home点，需要提前设置好home点位置 JMOVE #HOME ; S5: 请求抓取位姿 CALL xyz_r_g_pose(.error_code, 0, .token) IF .error_code <> 0 GOTO LABEL_FAIL WHILE 1 DO LABEL_START: ; S6: 获取抓取位姿 CALL xyz_g_g_pose(.error_code, .token, .pose_num, .pipe_num, .reg_num) IF .error_code <> 0 GOTO LABEL_FAIL ; S7: IF .pose_num < 1 THEN ;S15: 上位机切换至识别隔板 ;工作空间中已经没有工件，需要先取走隔板 CALL xyz_sw_item(.error_code, 0, "board") IF .error_code <> 0 GOTO LABEL_FAIL ; S16: 请求隔板抓取位姿 CALL xyz_r_g_pose(.error_code, 0, .token) IF .error_code <> 0 GOTO LABEL_FAIL ; S17: 获取隔板抓取位姿 CALL xyz_g_g_pose(.error_code, .token, .pose_num, .pipe_num, .reg_num) IF (.error_code <> 0) OR (.pose_num < 1) GOTO LABEL_FAIL ; S18: 移动到抓取位并抓取隔板 LMOVE grasp_pose SIGNAL 1 ; S19: 移动到放置位并放置 LMOVE cart_wp2 SIGNAL -1 ;注意，可能需要添加一些过渡点 ;S20: 上位机切换至识别工件 CALL xyz_sw_item(.error_code, 0, "obj1") IF .error_code <> 0 GOTO LABEL_FAIL CALL xyz_r_g_pose(.error_code, 0, .token) IF .error_code <> 0 GOTO LABEL_FAIL GOTO LABEL_START ELSE ; S8: 移动到拍照位，需要先示教该拍照位 LMOVE cart_wp1 POINT sending_pose = cart_wp1 ; S9: 发送拍照位姿 CALL xyz_s_c_pose(.error_code) IF .error_code <> 0 GOTO LABEL_FAIL ; S10: 上位机切换至识别工件 CALL xyz_sw_item(.error_code, 1, "obj1") IF .error_code <> 0 GOTO LABEL_FAIL ; S11: 请求抓取位姿 CALL xyz_r_g_pose(.error_code, 1, .token) IF .error_code <> 0 GOTO LABEL_FAIL ; S12: 获取抓取位姿 CALL xyz_g_g_pose(.error_code, .token, .pose_num, .pipe_num, .reg_num) IF (.error_code <> 0) OR (.pose_num < 1) GOTO LABEL_FAIL ; S13: 移动到抓取位并抓取 LMOVE grasp_pose SIGNAL 1 ; S14: 移动到放置位并放置 ; Attention: Additional waypoints should be added for place obj LMOVE cart_wp2 SIGNAL -1 END CALL xyz_sw_item(.error_code, 0, "obj1") IF .error_code <> 0 GOTO LABEL_FAIL CALL xyz_r_g_pose(.error_code, 0, .token) IF .error_code <> 0 GOTO LABEL_FAIL END LABEL_FAIL: CALL xyz_disconn(.ret, sock_id) .END .PROGRAM trajmovesync() ;------------------------------------------------------------------------ ;traj move 同步模板 ;------------------------------------------------------------------------ ; S1: 初始化参数 CALL set_const CP ON MESSAGE ON CALL close_all_sock ; S2: 连接上位机 CALL xyz_conn(sock_id, IP[], PORT) ; CALL xyz_sw_flw(.error_code, "test.t") ; IF .error_code <> 0 GOTO LABEL_FAIL ; S3: 切换工件 CALL xyz_sw_item(.error_code, 0, "item1") IF .error_code <> 0 GOTO LABEL_FAIL ; 以下参数需要设置合理的初始值 SIGNAL -1 .error_code = 0 .ws_id = 0 ; S4: 移动到home点，需要提前设置好home点位置 POINT #HOME = #PPOINT(0,0,0,0,0,0) JMOVE #HOME WHILE 1 DO ; S5: 请求抓取和放置规划 CALL xyz_r_picpla(.error_code, .ws_id) IF .error_code <> 0 GOTO LABEL_FAIL ; S6: 获取抓取入筐轨迹 CALL xyz_g_picin(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) IF .error_code <> 0 GOTO LABEL_FAIL ; S7: 执行抓取入筐轨迹 CALL xyz_exectraj(2, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) ; S8: 获取抓取出筐轨迹 CALL xyz_g_picout(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) IF .error_code <> 0 GOTO LABEL_FAIL ; S9: 执行抓取出筐轨迹 CALL xyz_exectraj(3, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) ; S10: 获取放置入筐轨迹 CALL xyz_g_plain(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) IF .error_code <> 0 GOTO LABEL_FAIL ; S11: 执行放置入筐轨迹 CALL xyz_exectraj(4, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) ; S12: 获取放置出筐轨迹 CALL xyz_g_plaout(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) IF .error_code <> 0 GOTO LABEL_FAIL ; S13: 执行放置出筐轨迹 CALL xyz_exectraj(5, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) END LABEL_FAIL: CALL xyz_disconn(.ret, sock_id) .END .PROGRAM trajmoveasync() ;------------------------------------------------------------------------ ;traj move 异步模板 ;------------------------------------------------------------------------ ; S1: 初始化参数 CALL set_const CP ON MESSAGE ON CALL close_all_sock ; S2: 连接到上位机 CALL xyz_conn(sock_id, IP[], PORT) ; CALL xyz_sw_flw(.error_code, "test.t") ; IF .error_code <> 0 GOTO LABEL_FAIL ; S3: 切换工件 CALL xyz_sw_item(.error_code, 0, "item1") IF .error_code <> 0 GOTO LABEL_FAIL ; 以下参数需要设置合理的初始值 SIGNAL -1 .error_code = 0 .ws_id = 0 ; S4: 移动到home点，需要提前设置好home点位置 POINT #HOME = #PPOINT(0,0,0,0,0,0) JMOVE #HOME ; S5: 请求抓取和放置规划 CALL xyz_r_picpla(.error_code, .ws_id) IF .error_code <> 0 GOTO LABEL_FAIL WHILE 1 DO ; S6: 获取抓取入筐轨迹 CALL xyz_g_picin(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) IF .error_code <> 0 GOTO LABEL_FAIL ; S7: 执行抓取入筐轨迹 CALL xyz_exectraj(2, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) ; S8: 获取抓取出筐轨迹 CALL xyz_g_picout(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) IF .error_code <> 0 GOTO LABEL_FAIL ; S9: 执行抓取出筐轨迹 CALL xyz_exectraj(3, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) ; S10: 请求下一次的抓取和放置规划 CALL xyz_r_picpla(.error_code, .ws_id) IF .error_code <> 0 GOTO LABEL_FAIL ; S11: 获取放置入框轨迹 CALL xyz_g_plain(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) IF .error_code <> 0 GOTO LABEL_FAIL ; S12: 执行放置入筐轨迹 CALL xyz_exectraj(4, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) ; S13: 获取放置出筐轨迹 CALL xyz_g_plaout(.error_code, .ws_id, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) IF .error_code <> 0 GOTO LABEL_FAIL ; S14: 执行放置出筐轨迹 CALL xyz_exectraj(5, .pose_type, .wps_num, .wps_type[], .#jnts_traj[]) END LABEL_FAIL: CALL xyz_disconn(.ret, sock_id) .END