案例/模板说明
机械臂主控主函数说明
注意对工控机返回的 err_code 进行判断。
轨迹移动同步模板：
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;
轨迹移动异步模板
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;
座标移动基础模板：
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;
座标移动二次定位模板
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;