案例/模板说明
机械臂主控程序说明
以下为机械臂主控模板代码，注意对工控机返回的 err_code 进行判断。
座标移动基础模板
DEF xyz_CartMove_template()
; S1: Initialization
; S1: 初始化
INT err_code ; 错误码
INT grasp_pose_token ; 抓取位姿 token
E6POS grasp_pose ; 抓取位姿
E6POS home_pose ; home 位姿
E6POS scan_pose ; 拍照位姿
E6POS place_pose ; 放置位姿
INT grasp_pose_num ; 抓取位姿数量
BOOL is_eye_in_hand ; 是否为眼在手上
INT obj_pipeline_num ; 物体 pipeline 编号
INT obj_register_num ；物体注册编号
grasp_pose_token = 0
gMasterFlag = TRUE
err_code = 0
grasp_pose_num = 0
obj_pipeline_num = 0
obj_register_num = 0
; 以下为中断处理等代码，不能删除
GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
INTERRUPT ON 3
BAS (#INITMOV,0 )
; The first movement for Kuka must be a PTP movement
$BWDSTART = FALSE
PDAT_ACT = PPDAT_1
FDAT_ACT = FPDAT_2
BAS(#PTP_PARAMS, PPDAT_1.vel)
; Comment out this line if there is an error
SET_CD_PARAMS (0)
PTP $AXIS_ACT
; S2: Connect to IPC
; S2: 连接 IPC
xyzOpenSocket("MasterClient") ; 连接 IPC
; change the flow name here!
; err_code = xyzSwitchFlow("cart_move.t") ; 切换任务
; IF (err_code <> 0) THEN ;
; GOTO END_PROGRAM;
; ENDIF
; S3: Switch object
; S3: 切换物体
; 0: ws_id, "item1": item_codename
; change the item_codename here!
err_code = xyzSwitchItem(0,"item1") ; 切换物体
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
; S4: Move to home pose
; S4: 移动到 home 位姿
; you should use WorkVisual to set the home_pose,
; or use pendant to set the home_pose
; 使用 WorkVisual 设置 home 位姿，或者使用示教器设置 home 位姿
; home_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
; xyzSetMoveJParameters(10,20,0) ; 设置关节移动参数: 速度、加速度、减速度
; PTP home_pose ; 关节移动到 home 位姿
; xyzSetMoveLParameters(10,20,0) ; 设置直线移动参数: 速度、加速度、减速度
; LIN home_pose ; 直线移动到 home 位姿
; $OUT[500] = TRUE; IO 操作
; if application is eye_on_hand, please make eye_on_hand := true;
; 如果是眼在手上的应用，请将 eye_on_hand := true;
is_eye_in_hand = false;
BEGIN_WHILE:
WHILE TRUE
; S5: eye on hand application
; S5: 眼在手上的应用
IF is_eye_in_hand THEN
; S6: Move to scan pose
; S6: 移动到拍照位姿
; you should use WorkVisual to set the scan_pose,
; or use pendant to set the scan_pose
; 使用 WorkVisual 设置拍照位姿，或者使用示教器设置拍照位姿
; set movel or moveJ parameters(vel, acc, zone)
; scan_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
; xyzSetMoveJParameters(10,20,0) ; 设置关节移动参数: 速度、加速度、减速度
; PTP scan_pose ; 关节移动到拍照位姿
; xyzSetMoveLParameters(10,20,0) ; 设置直线移动参数: 速度、加速度、减速度
; LIN scan_pose ; 直线移动到拍照位姿
; S7: Send scan pose
; S7: 发送拍照位姿
err_code = xyzSendCurrentCartPose()
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
ENDIF
; S8: Request grasp pose(including cap image)
; S8: 请求抓取位姿
err_code = xyzReqGraspPose(0,grasp_pose_token)
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
; S9: Get grasp pose
; S9: 获取抓取位姿
err_code = xyzGetGraspPose(grasp_pose_token, grasp_pose, grasp_pose_num, obj_pipeline_num, obj_register_num)
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
; 如果没有可以抓取的物体
IF (grasp_pose_num < 1) THEN
WAIT SEC 5
GOTO BEGIN_WHILE;
ENDIF
; S10: Move to grasp pose and grasp object
; S10: 移动到抓取位姿并抓取物体
; Attention: Additional waypoints maybe need added
; 注意：可能需要添加额外的路径点
; xyzSetMoveJParameters(10,20,0) ; 设置关节移动参数: 速度、加速度、减速度
; PTP grasp_pose ; 关节移动到抓取位姿
; xyzSetMoveLParameters(10,20,0) ; 设置直线移动参数: 速度、加速度、减速度
; LIN grasp_pose ; 直线移动到抓取位姿
; $OUT[500] = TRUE; grasp object 抓取物体
; WAIT SEC 0.5
; S11: Move to place pose and place object
; S11: 移动到放置位姿并放置物体
; you should use WorkVisual to set the place_pose,
; or use pendant to set the place_pose
; 使用 WorkVisual 设置放置位姿，或者使用示教器设置放置位姿
; place_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
; xyzSetMoveJParameters(10,20,0) ; 设置关节移动参数: 速度、加速度、减速度
; PTP place_pose ; 关节移动到放置位姿
; xyzSetMoveLParameters(10,20,0) ; 设置直线移动参数: 速度、加速度、减速度
; LIN place_pose ; 直线移动到放置位姿
; Attention: Additional waypoints should be added
; 注意：需要添加额外的路径点
; after move to place pose
; 在移动到放置位姿之后
; $OUT[500] = FALSE; place object 放置物体
ENDWHILE
END_PROGRAM:
END
座标移动二次定位模板
DEF xyz_CartMove_reposition()
; S1: Initialization
; S1: 初始化
INT err_code ; 错误码
INT rough_grasp_pose_token ; 粗定位抓取位姿 token
E6POS rough_grasp_pose ; 粗定位抓取位姿
INT rough_grasp_pose_num ; 粗定位抓取位姿数量
INT board_grasp_pose_token ; 精定位抓取位姿 token
E6POS board_grasp_pose ; 精定位抓取位姿
INT board_grasp_pose_num ; 精定位抓取位姿数量
INT fine_grasp_pose_token ; 精定位抓取位姿 token
E6POS fine_grasp_pose ; 精定位抓取位姿
INT fine_grasp_pose_num ; 精定位抓取位姿数量
E6POS home_pose ; home 位姿
INT obj_pipeline_num ; 物体 pipeline 编号
INT obj_register_num ; 物体注册编号
INT board_pipeline_num ; board pipeline 编号
INT board_register_num ; board 注册编号
gMasterFlag = TRUE
err_code = 0
obj_pipeline_num = 0
obj_register_num = 0
board_pipeline_num = 0
board_register_num = 0
fine_grasp_pose_token = 0
; 以下为中断处理等代码，不能删除
GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
INTERRUPT ON 3
BAS (#INITMOV,0 )
; The first movement for Kuka must be a PTP movement
$BWDSTART = FALSE
PDAT_ACT = PPDAT_1
FDAT_ACT = FPDAT_2
BAS(#PTP_PARAMS, PPDAT_1.vel)
; Comment out this line if there is an error
SET_CD_PARAMS (0)
PTP $AXIS_ACT
; $OUT[500] = TRUE; io initialization IO 初始化
; S2: Connect to IPC
; S2: 连接 IPC
xyzOpenSocket("MasterClient") ; 连接 IPC
; change the flow name here! The name should be less than 13 characters.
; err_code = xyzSwitchFlow("cart_repo.t") ; 切换任务
; IF (err_code <> 0) THEN ;
; HALT;
; GOTO END_PROGRAM;
; ENDIF
; S3: The camera outside the robotic arm is switched to recognize the current workpiece
; S3: 切换机械臂外部的相机，识别当前工件
; camera 1 rough location, 0: ws_id, "item1": item_codename
; change item_codename here!
err_code = xyzSwitchItem(0,"item1") ; 切换工件
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
; S4: Move to home pose
; S4: 移动到 home 位姿
; Additional: should define home_pose first,
; you can modify home_pose with WorkVisual,
; or use pendant to set the home_pose
; 需要先定义 home 位姿，
; 可以使用 WorkVisual 修改 home 位姿，或者使用示教器设置 home 位姿
; home_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
; xyzSetMoveJParameters(10,20,0) ; vel, acc, zone 设置关节移动参数：速度、加速度、减速度
; PTP home_pose ; 关节移动到 home 位姿
; xyzSetMoveLParameters(10,20,0) ; vel, acc, zone 设置直线移动参数：速度、加速度、减速度
; LIN home_pose ; 直线移动到 home 位姿
; S5: Request grasp pose
; S5: 请求抓取位姿
err_code = xyzReqGraspPose(0,rough_grasp_pose_token) ; 请求抓取位姿
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
BEGIN_WHILE:
WHILE TRUE
; S6: get grasp pose
; S6: 获取抓取位姿
err_code = xyzGetGraspPose(rough_grasp_pose_token, rough_grasp_pose, rough_grasp_pose_num, obj_pipeline_num, obj_register_num)
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
; S7: grasp board if no object exists on board
; S7: 如果board上没有物体，就抓取board
IF (rough_grasp_pose_num < 1) THEN
; S15: The camera outside the robotic arm is switched to recognize the board
; S15: 切换机械臂外部的相机，识别board
; no object in workspace, should remove board first
; 没有物体在工作区，应该先移除board
; switch board; O:ws_id, board: board_name
err_code = xyzSwitchItem(0,"board") ; 切换工件为 board
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
; S16: Request board grasp pose
; S16: 请求 board 抓取位姿
err_code = xyzReqGraspPose(0,board_grasp_pose_token)
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
; S17: Get board grasp pose
; S17: 获取 board 抓取位姿
err_code = xyzGetGraspPose(board_grasp_pose_token, board_grasp_pose, board_grasp_pose_num, board_pipeline_num, board_register_num)
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
; 如果没有可以抓取的 board
IF (board_grasp_pose_num < 1) THEN
; no board exists, you can change tote
HALT;
GOTO END_PROGRAM;
ENDIF
; S18: Move to board grasp pose and grasp board
; S18: 移动到 board 抓取位姿并抓取 board
; xyzSetMoveJParameters(10,20,0) ; vel, acc, zone 设置关节移动参数：速度、加速度、减速度
; PTP grasp_pose ; 关节移动到抓取位姿
; xyzSetMoveLParameters(10,20,0) ; vel, acc, zone 设置直线移动参数：速度、加速度、减速度
; LIN grasp_pose ; 直线移动到抓取位姿
; $OUT[500] = TRUE; grasp board ; 抓取 board
WAIT SEC 0.5
; S19: Move to board place pose
; S19: 移动到 board 放置位姿
; you should use WorkVisual to set the place_pose,
; or use pendant to set the place_pose
; 需要先定义 place 位姿，使用 WorkVisual 或者示教器设置
; place_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
; xyzSetMoveJParameters(10,20,0) ; vel, acc, zone 设置关节移动参数：速度、加速度、减速度
; PTP place_pose ; 关节移动到放置位姿
; xyzSetMoveLParameters(10,20,0) ; vel, acc, zone 设置直线移动参数：速度、加速度、减速度
; LIN place_pose ; 直线移动到放置位姿
; $OUT[500] = FALSE; place board ; 放置 board
; S20: The camera outside the robotic arm is switched to recognize the current workpiece
; S20: 切换机械臂外部的相机，识别当前工件
; 0: ws_id, "item1": item_codename
; change the item_codename here!
err_code = xyzSwitchItem(0,"item1") ; 切换工件为 item1
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
GOTO BEGIN_WHILE;
ELSE
; S8: Move to scan pose
; S8: 移动到拍照位姿
; Additional: should define scan_pose first,
; youcan modify scan_pose with WorkVisual or
; use pendant to set scan_pose
; 需要先定义 scan 位姿，使用 WorkVisual 或者示教器设置
; scan_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
; xyzSetMoveJParameters(10,20,0) ; 设置关节移动参数：速度、加速度、减速度
; PTP scan_pose ; 关节移动到拍照位姿
; xyzSetMoveLParameters(10,20,0) ; 设置直线移动参数：速度、加速度、减速度
; LIN scan_pose ; 直线移动到拍照位姿
; S9: Send scan pose
; S9: 发送拍照位姿
err_code = xyzSendCurrentCartPose()
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
; S10: The camera on the robotic arm is switched to recognize the current workpiece
; S10: 切换机械臂上的相机，识别当前工件
; camera2 cap precise image, 1: ws_id, "item1": item_codename
; change the item_codename here!
err_code = xyzSwitchItem(1,"item1") ; 切换工件为 item1
IF (err_code <> 0) THEN ;
GOTO END_PROGRAM;
ENDIF
; S11: Request grasp pose
; S11: 请求抓取位姿
err_code = xyzReqGraspPose(1,fine_grasp_pose_token)
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
; S12: Get grasp pose
; S12: 获取抓取位姿
err_code = xyzGetGraspPose(fine_grasp_pose_token ,fine_grasp_pose, fine_grasp_pose_num, obj_pipeline_num, obj_register_num)
IF (err_code <> 0) THEN ;
HALT;
GOTO END_PROGRAM;
ENDIF
; 如果没有可以抓取的工件
IF (fine_grasp_pose_num < 1) THEN
HALT;
GOTO END_PROGRAM;
ENDIF
; S13: Move to grasp pose and grasp workpiece
; S13: 移动到抓取位姿并抓取工件
; xyzSetMoveJParameters(10,20,0) ; vel, acc, zone 设置关节移动参数：速度、加速度、减速度
; PTP fine_grasp_pose ; 关节移动到抓取位姿
; xyzSetMoveLParameters(10,20,0) ; vel, acc, zone 设置直线移动参数：速度、加速度、减速度
; LIN fine_grasp_pose ; 直线移动到抓取位姿
; $OUT[500] = TRUE; grasp workpiece ; 抓取工件
WAIT SEC 0.5
; S14: Move to place pose and place workpiece
; S14: 移动到放置位姿并放置工件
; you should use WorkVisual to define place_pose,
; or use pendant to set place_pose
; 需要先定义 place 位姿，使用 WorkVisual 或者示教器设置
; place_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
; xyzSetMoveJParameters(10,20,0) ; vel, acc, zone 设置关节移动参数：速度、加速度、减速度
; PTP place_pose ; 关节移动到放置位姿
; xyzSetMoveLParameters(10,20,0) ; vel, acc, zone 设置直线移动参数：速度、加速度、减速度
; LIN place_pose ; 直线移动到放置位姿
; $OUT[500] = FALSE; place workpiece ; 放置工件
ENDIF
ENDWHILE
END_PROGRAM:
END
3D轨迹移动同步程序
DEF xyz_TrajMove_Sync()
; S1: Initialization
; S1: 初始化
INT err_code
CHAR flow_name[20]
INT ws_id
CHAR item_codename[20]
E6POS home_pose
INT pipeline_num
INT register_num
INT way_point_num
INT wp_type[30]
E6AXIS joints[50]
E6POS carts[50]
gMasterFlag = TRUE
GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
INTERRUPT ON 3
BAS (#INITMOV,0 )
; The first movement for Kuka must be a PTP movement
$BWDSTART = FALSE
PDAT_ACT = PPDAT_1
FDAT_ACT = FPDAT_2
BAS(#PTP_PARAMS, PPDAT_1.vel)
; Comment out this line if there is an error
SET_CD_PARAMS (0)
PTP $AXIS_ACT
; S2: Connect to IPC
; S2: 连接工控机
xyzOpenSocket("MasterClient")
gStrOpSuccess = STRCLEAR(gSendgMotMsg[])
; flow_name[] = "traj_sync.t"
; err_code = xyzSwitchFlow(flow_name[])
; IF (err_code <> 0) THEN ;
; HALT;
; GOTO err_exit;
; ENDIF
; S3: Switch to the current item
; S3: 切换到当前工件
ws_id = 0
item_codename[] = "item1"
err_code = xyzSwitchItem(ws_id, item_codename[])
IF (err_code <> 0) THEN ;
HALT;
GOTO err_exit;
ENDIF
; IO operation
; $OUT[500] = False
WAIT SEC 0.5
; S4: Move to home pose
; S4: 移动到 home 位姿
; you should use WorkVisual to set the home pose,
; or use pendant to set the home pose
; home_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
; xyzSetMoveJParameters(10,20,0) ; vel, acc, zone
; PTP home_pose
; xyzSetMoveLParameters(10,20,0)
; LIN home_pose
WHILE True
; S5: Request Pick and place trajectory
; S5: 请求抓取放置轨迹
ws_id = 0
err_code = xyzReqPickPlace(ws_id)
IF (err_code <> 0) THEN ;
HALT;
GOTO err_exit;
ENDIF
; S6: Get Pick in trajectory
; S6: 获取抓取入筐轨迹
err_code = xyzGetPickin(0, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
IF (err_code <> 0) THEN
; S15
HALT;
GOTO err_exit;
ENDIF
; if tote cleared, exit
; 如果没有可以抓取的物体，退出
IF (way_point_num < 1) THEN
GOTO clear_tore_exit
ENDIF
; S8 Execute Pick in trajectory
; S8: 执行抓取入筐轨迹
xyzExecutePickInTraj(way_point_num, wp_type[], joints[], carts[])
; IO operation IO 操作
; $OUT[500] = False
; S9: Get Pick out trajectory
; S9: 获取抓取出筐轨迹
err_code = xyzGetPickout(0, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
IF (err_code <> 0) THEN ;
HALT;
GOTO err_exit;
ENDIF
; S10: Execute Pick out trajectory
; S10: 执行抓取出筐轨迹
xyzExecutePickOutTraj(way_point_num, wp_type[], joints[], carts[])
; S11: Get Place in trajectory
; S11: 获取放置入筐轨迹
err_code = xyzGetPlacein(0, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
IF (err_code <> 0) THEN ;
HALT;
GOTO err_exit;
ENDIF
; S12: Execute Place in trajectory
; S12: 执行放置入筐轨迹
xyzExecuteTraj(way_point_num, wp_type[], joints[], carts[])
; IO operation IO 操作
; $OUT[500] = True
; S13: Get Place out trajectory
; S13: 获取放置出筐轨迹
err_code = xyzGetPlaceout(0, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
IF (err_code <> 0) THEN ;
HALT;
GOTO err_exit;
ENDIF
; S14: Execute Place out trajectory
; S14: 执行放置出筐轨迹
xyzExecuteTraj(way_point_num, wp_type[], joints[], carts[])
ENDWHILE
err_exit:
clear_tore_exit:
END
3D轨迹移动异步运行程序
DEF xyz_TrajMove_Async()
; S1: Initialization
; S1: 初始化
INT err_code
CHAR flow_name[20]
INT ws_id
CHAR item_codename[20]
E6POS home_pose
INT pipeline_num
INT register_num
INT way_point_num
INT wp_type[30]
E6AXIS joints[50]
E6POS carts[50]
gMasterFlag = TRUE ; 表示正在运行机械臂主控
; 以下为中断处理等代码，不能删除
GLOBAL INTERRUPT DECL 3 WHEN $STOPMESS==TRUE DO IR_STOPM ( )
INTERRUPT ON 3
BAS (#INITMOV,0 )
; The first movement for Kuka must be a PTP movement
$BWDSTART = FALSE
PDAT_ACT = PPDAT_1
FDAT_ACT = FPDAT_2
BAS(#PTP_PARAMS, PPDAT_1.vel)
; Comment out this line if there is an error
SET_CD_PARAMS (0)
PTP $AXIS_ACT
; S2: Connect to IPC
; S2: 连接工控机
xyzOpenSocket("MasterClient")
gStrOpSuccess = STRCLEAR(gSendgMotMsg[])
; flow_name[] = "traj_async.t"
; err_code = xyzSwitchFlow(flow_name[])
; IF (err_code <> 0) THEN ;
; HALT;
; GOTO err_exit;
; ENDIF
; S3: Switch to the current item
; S3: 切换到当前工件
ws_id = 0
item_codename[] = "item1"
err_code = xyzSwitchItem(ws_id, item_codename[])
IF (err_code <> 0) THEN ;
HALT;
GOTO err_exit;
ENDIF
; IO operation IO 操作
; $OUT[500] = False
; S4: Move to home pose
; S4: 移动到 home 位姿
; you should use WorkVisual to set the home_pose,
; or use pendant to set the home_pose
; home_pose = {X 578.80, Y -9.12, Z 541.47, A 173.34,B -4.38,C -177.88}
; xyzSetMoveJParameters(10,20,0) ; vel, acc, zone
; PTP home_pose
; xyzSetMoveLParameters(10,20,0)
; LIN home_pose
WHILE True
; S5: Request Pick and place trajectory
; S5: 请求抓取放置轨迹
ws_id = 0
err_code = xyzReqPickPlace(ws_id)
IF (err_code <> 0) THEN ;
HALT;
GOTO err_exit;
ENDIF
; S6: Get Pick in trajectory
; S6: 获取抓取入筐轨迹
ws_id = 0
err_code = xyzGetPickin(ws_id, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
IF (err_code <> 0) THEN ;
HALT;
GOTO err_exit;
ENDIF
; if tote cleared, exit
; 如果没有可以抓取的物体，退出
IF (way_point_num < 1) THEN
GOTO clear_tore_exit
ENDIF
; S8 Execute Pick in trajectory
; S8: 执行抓取入筐轨迹
xyzExecutePickInTraj(way_point_num, wp_type[], joints[], carts[])
; IO operation IO 操作
; $OUT[500] = False
; S9: Get Pick out trajectory
; S9: 获取抓取出筐轨迹
ws_id = 0
err_code = xyzGetPickout(ws_id, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
IF (err_code <> 0) THEN ;
HALT;
GOTO err_exit;
ENDIF
; S10: Execute Pick out trajectory
; S10: 执行抓取出筐轨迹
xyzExecutePickOutTraj(way_point_num, wp_type[], joints[], carts[])
; S11: request next pick and place plan in advance
; S11: 提前请求下一次抓取放置轨迹
ws_id = 0
err_code = xyzReqPickPlace(ws_id)
IF (err_code <> 0) THEN ;
HALT;
GOTO err_exit;
ENDIF
; S12: Get Place in trajectory
; S12: 获取放置入筐轨迹
ws_id = 0
err_code = xyzGetPlacein(ws_id, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
IF (err_code <> 0) THEN ;
HALT;
GOTO err_exit;
ENDIF
; S13: Execute Place in trajectory
; S13: 执行放置入筐轨迹
xyzExecuteTraj(way_point_num, wp_type[], joints[], carts[])
; IO operation IO 操作
; $OUT[500] = True
; S14: Get Place out trajectory
; S14: 获取放置出筐轨迹
ws_id = 0
err_code = xyzGetPlaceout(ws_id, pipeline_num, register_num, way_point_num, wp_type[], joints[], carts[])
IF (err_code <> 0) THEN ;
HALT;
GOTO err_exit;
ENDIF
; S15: Execute Place out trajectory
; S15: 执行放置出筐轨迹
xyzExecuteTraj(way_point_num, wp_type[], joints[], carts[])
ENDWHILE
err_exit:
clear_tore_exit:
END