案例/模板说明
机械臂主控主函数说明
注意对工控机返回的 err_code 进行判断。
座标移动基础模板：
Include "api.pro";
Func run()
# S1, S2 : Init and connect To IPC 初始化以及连接工控机
LB[1] = 0;
While LB[1] <>1
Open Socket("192.168.37.101",11111,2,Single, LB[1]);
EndWhile;
gMasterPort = 2;
Get Port[gMasterPort],T[0.2],Goto L[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;
# S3 切换工件
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
Set Out[1],ON;
# S4: 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];
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
# S5
If is_eye_in_hand
# S6: 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];
# S7 发送当前位姿
api.xyzSendCurrentCartPose(err_code);
If err_code <> 0
Halt 3;
Goto L[1];
EndIf;
EndIf;
# S8: req grasp pose(including cap image) 请求抓取位姿
grasp_pose_token = 0;
ws_id = 0;
api.xyzReqGraspPose(ws_id, err_code, grasp_pose_token);
If err_code <> 0
Halt 3;
Goto L[1];
EndIf;
# S9 获取抓取位姿
grasp_pose_num = 0;
pipeline_num = 0;
register_num = 0;
Int ints[6];
api.xyzGetGraspPose(grasp_pose_token, err_code, grasp_pose, grasp_pose_num, pipeline_num, register_num, ints);
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),(3,0,0);
If grasp_pose_num < 1
Wait T[5];
Continue;
EndIf;
# S10: Move To grasp pose 移动到抓取位姿
# add more path pose, modify the offset PR[], unit: mm
# 添加额外路径点
PR[1] = (0,0,100,0,0,0);
Movj Offset(LP[2],PR[1]),V[30],Z[0],Tool[0];
Movl LP[2], V[30], Z[0], Tool[0];
Movj Offset(LP[2],PR[1]),V[30],Z[0],Tool[0];
# io operation
Set Out[1],OFF;
# S11: Move To place pose 移动到放置位姿
# add more path pose, modify the offset PR[], unit: mm
# 添加额外路径点
PR[2] = (0,0,100,0,0,0);
LP[3] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(3,0,0);
Movj Offset(LP[2],PR[2]),V[30],Z[0],Tool[0];
Movl LP[3], V[30], Z[0], Tool[0];
Movj Offset(LP[2],PR[2]),V[30],Z[0],Tool[0];
# io operation
Set Out[1],ON;
EndWhile;
L[1]:
Close Socket, gMasterPort;
EndFunc;
座标移动二次定位模板
Include "api.pro";
Func run()
# S1, S2 : Init and connect To IPC 初始化以及连接工控机
LB[1] = 0;
While LB[1] <>1
Open Socket("192.168.37.101",11111,2,Single, LB[1]);
EndWhile;
gMasterPort = 2;
Get Port[gMasterPort],T[0.2],Goto L[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;
# S3 切换工件
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;
# S4: 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];
# io operation
Set Out[1],ON;
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;
Int ints[6];
While True
# S5 请求抓取位姿
rough_grasp_pose_token = 0;
ws_id = 0;
api.xyzReqGraspPose(ws_id, err_code, rough_grasp_pose_token);
If err_code <> 0
Halt 3;
Goto L[1];
EndIf;
# S6: 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, ints);
If err_code <> 0
Halt 3;
Goto L[1];
EndIf;
# S7: grasp board if no object exists on board 如果没有物体在隔板上，抓取隔板
If (rough_grasp_pose_num < 1)
# S15: 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;
# S16 请求隔板抓取位姿
ws_id = 0;
api.xyzReqGraspPose(ws_id, err_code, board_grasp_pose_token);
If err_code <> 0
Halt 3;
Goto L[1];
EndIf;
# S17 获取隔板抓取位姿
api.xyzGetGraspPose(board_grasp_pose_token, err_code, board_grasp_pose, board_grasp_pose_num, board_pipeline_num, board_register_num, ints);
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),(3,0,0);
If (board_grasp_pose_num < 1)
Halt 3;
Goto L[1];
EndIf;
# S18: Move To board grasp pose 移动到隔板抓取位姿
PR[1] = (0,0,100,0,0,0);
# add more path pose, modify the offset PR[], unit: mm
# 添加额外路径点
Movj Offset(LP[2],PR[1]),V[30],Z[0],Tool[0];
Movl LP[2], V[30], Z[0], Tool[0];
Movj Offset(LP[2],PR[1]),V[30],Z[0],Tool[0];
# io operation
Set Out[1],OFF;
# S19: Move To board place pose 移动到隔板放置位姿
# add more path pose, modify the offset PR[], unit: mm
# 添加额外路径点
PR[2] = (0,0,100,0,0,0);
LP[3] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(3,0,0);
Movj Offset(LP[3],PR[2]),V[30],Z[0],Tool[0];
Movl LP[3], V[30], Z[0], Tool[0];
Movj Offset(LP[3],PR[2]),V[30],Z[0],Tool[0];
# io operation
Set Out[1],ON;
# S20 切换工件
item_codename = "item1";
ws_id = 0;
api.xyzSwitchItem(ws_id, item_codename, err_code);
If err_code <> 0
Halt 3;
Goto L[1];
EndIf;
Continue;
EndIf;
# S9: 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];
# S9: Send cart pose with eye In hand
api.xyzSendCurrentCartPose(err_code);
If err_code <> 0
Halt 3;
Goto L[1];
EndIf;
# S10 使用机械臂上相机识别工件
ws_id = 1;
item_codename = "item1";
api.xyzSwitchItem(ws_id, item_codename, err_code);
If err_code <> 0
Halt 3;
Goto L[1];
EndIf;
# S11 请求精确抓取位姿
fine_grasp_pose_token = 0;
ws_id = 0;
api.xyzReqGraspPose(ws_id, err_code, fine_grasp_pose_token);
If err_code <> 0
Halt 3;
Goto L[1];
EndIf;
# S12 获取精确抓取位姿
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),(3,0,0);
# S13: Move To Fine grasp pose 移动到精确抓取位姿
# add more path pose, modify the offset PR[], unit: mm
# 添加额外路径点
PR[3] = (0,0,100,0,0,0);
Movj Offset(LP[5],PR[3]),V[30],Z[0],Tool[0];
Movj LP[5], V[30], Z[0], Tool[0];
Movj Offset(LP[5],PR[3]),V[30],Z[0],Tool[0];
# io operation
Set Out[1],OFF;
# S14: Move To place pose 移动到放置位姿
# add more path pose, modify the offset PR[], unit: mm
# 添加额外路径点
PR[4] = (0,0,100,0,0,0);
LP[6] = (0, 0, 0, 0, -90, 0),(0,0,0,1),(1,0,0);
Movj Offset(LP[6],PR[4]),V[30],Z[0],Tool[0];
Movj LP[6], V[30], Z[0], Tool[0];
Movj Offset(LP[6],PR[4]),V[30],Z[0],Tool[0];
# io operation
Set Out[1],ON;
EndWhile;
L[1]:
Close Socket, gMasterPort;
EndFunc;
轨迹移动同步模板：
Include "api.pro";
Func run()
# S1,S2: Init And connect To IPC 初始化以及连接工控机
LB[1] = 0;
While LB[1] <>1
Open Socket("192.168.37.101",11111,2,Single, LB[1]);
EndWhile;
gMasterPort = 2;
Get Port[gMasterPort],T[0.2],Goto L[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;
# S3
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
Set Out[1],ON;
# S4: 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
# S5 请求计算抓取放置
ws_id = 0;
api.xyzReqPickPlace(ws_id,err_code);
If err_code <> 0
Halt 3;
Goto L[1];
EndIf;
# S6 获取抓取入筐轨迹
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;
# S7
If num < 1
# S15 如果没有可以抓取的工件
Goto L[1];
EndIf;
# S8 执行抓取入筐轨迹
api.xyzExecutePickInTraj(num, wp_type, pos);
# io operation
Set Out[1],OFF;
# S9 获取抓取出筐轨迹
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;
# S10 执行抓取出筐轨迹
api.xyzExecutePickOutTraj(num, wp_type, pos);
# S11 获取放置入筐轨迹
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;
# S12 执行放置入筐轨迹
api.xyzExecuteTraj(num, wp_type, pos);
# io operation
Set Out[1],ON;
# S13 获取放置出筐轨迹
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;
# S14 执行放置出筐轨迹
api.xyzExecuteTraj(num, wp_type, pos);
EndWhile;
L[1]:
Close Socket, gMasterPort;
EndFunc;
轨迹移动异步模板
Include "api.pro";
Func run()
# S1,S2: Init And connect To IPC 初始化以及连接工控机
LB[1] = 0;
While LB[1] <>1
Open Socket("192.168.37.101",11111,2,Single, LB[1]);
EndWhile;
gMasterPort = 2;
Get Port[gMasterPort],T[0.2],Goto L[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;
# S3 切换工件
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
Set Out[1],ON;
# S4: 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
# S5 请求计算抓取放置
ws_id = 0;
api.xyzReqPickPlace(ws_id,err_code);
If err_code <> 0
Halt 3;
Goto L[1];
EndIf;
# S6 获取抓取入筐轨迹
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;
# S7
If num < 1
# S16 如果没有可以抓取的工件
Goto L[1];
EndIf;
# S8 执行抓取入筐轨迹
api.xyzExecutePickInTraj(num, wp_type, pos);
# io operation
Set Out[1],OFF;
# S9 请求抓取出筐轨迹
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;
# S10 执行抓取出筐轨迹
api.xyzExecutePickOutTraj(num, wp_type, pos);
# S11: 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;
# S12 获取放置入筐轨迹
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;
# S13 执行放置入筐轨迹
api.xyzExecuteTraj(num, wp_type, pos);
# io operation
Set Out[1],ON;
# S14 获取放置出筐轨迹
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;
# S15 执行放置出筐轨迹
api.xyzExecuteTraj(num, wp_type, pos);
EndWhile;
L[1]:
Close Socket, gMasterPort;
EndFunc;