妗堜緥/妯℃澘璇存槑
============================
鏈烘鑷備富鎺т富鍑芥暟璇存槑
++++++++++++++++++++++++++++++++
娉ㄦ剰瀵瑰伐鎺ф満杩斿洖鐨� err_code 杩涜鍒ゆ柇銆�
搴ф爣绉诲姩鍩虹妯℃澘锛�
-------------------------------------
.. code-block::
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;
搴ф爣绉诲姩浜屾瀹氫綅妯℃澘
--------------------------------------
.. code-block::
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;
杞ㄨ抗绉诲姩鍚屾妯℃澘锛�
----------------------------------
.. code-block::
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;
杞ㄨ抗绉诲姩寮傛妯℃澘
-------------------------------------
.. code-block::
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;