ABB机器人程序.txt
15.5 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
MODULE aaa
PERS tooldata MyTool:=[TRUE,[[31.792631019,0,229.638935148],[0.945518576,0,0.325568154,0]],[1,[0,0,1],[1,0,0,0],0,0,0]];
CONST robtarget p1:=[[-16.11,-347.15,474.83],[0.294464,-0.883154,0.297088,0.212292],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p2:=[[-1.74,-351.15,465.56],[0.300708,-0.919333,0.248148,0.0531494],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p3:=[[6.19,-346.97,472.32],[0.376015,-0.914531,0.114921,-0.0950728],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p4:=[[-7.80,-337.93,495.03],[0.505132,-0.860161,0.065858,-0.0250314],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p5:=[[-8.57,-332.96,417.21],[0.448707,-0.890922,0.0589738,-0.0379704],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p6:=[[2.57,-332.92,410.45],[0.443783,-0.892313,0.0696654,-0.0445195],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p7:=[[1.82,-327.19,440.61],[0.463316,-0.882346,0.071927,-0.0403786],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p8:=[[8.10,-375.52,278.06],[0.352443,-0.931844,0.0590098,-0.0629934],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p9:=[[173.02,-380.60,301.74],[0.34939,-0.892928,0.250084,-0.134405],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p10:=[[-107.17,-393.26,299.48],[0.370443,-0.925911,-0.0735457,-0.00716374],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p11:=[[753.764,243.6931,840.0185],[0.2205171,0.3584949,-0.9055654,0.05296053],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p12:=[[753.764,243.6931,775.1548],[0.2205171,0.3584949,-0.9055655,0.05296054],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p13:=[[753.764,243.6931,695.0086],[0.2205171,0.3584949,-0.9055654,0.05296058],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p14:=[[753.764,243.6932,621.1562],[0.2205171,0.3584949,-0.9055654,0.0529606],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p15:=[[753.764,243.6932,509.4409],[0.2205171,0.3584949,-0.9055654,0.05296064],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p16:=[[753.764,243.6931,436.6886],[0.2205173,0.3584949,-0.9055654,0.05296063],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p17:=[[980.7636,243.6931,777.0938],[0.2205173,0.3584949,-0.9055654,0.05296072],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p18:=[[980.7637,243.6931,676.0527],[0.2205175,0.3584949,-0.9055654,0.05296073],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p19:=[[980.7637,243.6931,547.9425],[0.2205175,0.3584949,-0.9055654,0.05296074],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p20:=[[980.7636,243.693,422.2361],[0.2205177,0.3584948,-0.9055653,0.05296072],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p21:=[[943.39,251.39,232.51],[0.325692,0.363748,-0.871897,0.0375142],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p22:=[[933.30,286.58,232.51],[0.324931,0.380032,-0.864924,0.043614],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p23:=[[943.52,289.36,269.92],[0.303826,0.380911,-0.872016,0.0467343],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p24:=[[951.67,291.58,305.65],[0.283752,0.381565,-0.878309,0.0496596],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p25:=[[957.63,293.21,337.45],[0.265933,0.381999,-0.883532,0.0522222],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p26:=[[961.87,294.37,365.18],[0.250414,0.382266,-0.887808,0.0544283],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p27:=[[966.30,295.57,403.79],[0.228818,0.38247,-0.893341,0.057459],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p28:=[[968.46,296.16,431.77],[0.213148,0.382496,-0.897056,0.0596298],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p29:=[[969.95,296.57,468.46],[0.192573,0.382378,-0.901556,0.0624448],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
CONST robtarget p30:=[[969.94,296.56,505.18],[0.17191,0.382088,-0.905649,0.0652315],[0,0,-1,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS speeddata speed1:=[10,50,100,100];
VAR socketdev socket_server;
VAR socketdev socket_client;
PERS string str_address:="192.168.201.88";
!PERS string str_address:="192.168.201.88";
PERS num n_port:=21;
VAR string strReceive:="p1,L,100";
VAR string resultStr_OK:=",OK;";
VAR string resultStr_NG:=",NG;";
VAR socketstatus status;
VAR num starBit1:=0;
VAR num endBit1:=0;
VAR num LengBit1:=0;
VAR num starBit2:=0;
VAR num endBit2:=0;
VAR num LengBit2:=0;
VAR num starBit3:=0;
VAR num endBit3:=0;
VAR num LengBit3:=0;
VAR num endBit4:=0;
VAR num LengBit4:=0;
VAR num starBit4:=0;
PERS string revice_Str1_Cmd:="validateP";
PERS string revice_Str2_pName:="p2";
PERS string revice_Str3_moveType:="L";
PERS string revice_Str4_speed:="10";
VAR bool flag1:=false;
!PERS num value_1:=700;
!PERS num value_2:=50;
VAR robtarget pTarget:=[[700,50,700],[0.0258621,-0.000364211,-0.999665,0.000003063],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget pTemp:=[[700,50,700],[0.0258621,-0.000364211,-0.999665,3.06266E-6],[0,-1,0,0],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
PERS bool booLineMove:=TRUE;
PERS bool dataOk:=TRUE;
VAR num len:=0;
TASK PERS tooldata tool1:=[TRUE,[[-19.2067,-21.2174,16.2824],[1,0,0,0]],[1,[0,0,1],[1,0,0,0],0,0,0]];
TASK PERS wobjdata wobj1:=[FALSE,TRUE,"",[[767.868,61.732,519.513],[0.710458,6.744E-06,-2.16356E-05,-0.703739]],[[0,0,0],[1,0,0,0]]];
PERS zonedata zone1:=[FALSE,5,8,8,0.8,8,0.8];
PERS loaddata load1:=[4,[0,0,0],[1,0,0,0],0,0,0];
PERS robtarget pCheckTest:=[[-1.73959,-351.15,465.559],[0.300709,-0.919332,0.248149,0.0531482],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS bool pCheckResult:=TRUE;
PROC modifyPoint()
MoveL p1,speed1,z50,tool1\WObj:=wobj1;
MoveL p2,speed1,z50,tool1\WObj:=wobj1;
MoveL p3,speed1,z50,tool1\WObj:=wobj1;
MoveL p4,speed1,z50,tool1\WObj:=wobj1;
MoveL p5,speed1,z50,tool1\WObj:=wobj1;
MoveL p6,speed1,z50,tool1\WObj:=wobj1;
MoveL p7,speed1,z50,tool1\WObj:=wobj1;
MoveL p8,speed1,z50,tool1\WObj:=wobj1;
MoveL p9,speed1,z50,tool1\WObj:=wobj1;
MoveL p10,speed1,z50,tool1\WObj:=wobj1;
MoveL p11,speed1,z50,tool1\WObj:=wobj1;
MoveL p12,speed1,z50,tool1\WObj:=wobj1;
MoveL p13,speed1,z50,tool1\WObj:=wobj1;
MoveL p14,speed1,z50,tool1\WObj:=wobj1;
MoveL p15,speed1,z50,tool1\WObj:=wobj1;
MoveL p16,speed1,z50,tool1\WObj:=wobj1;
MoveL p17,speed1,z50,tool1\WObj:=wobj1;
MoveL p18,speed1,z50,tool1\WObj:=wobj1;
MoveL p19,speed1,z50,tool1\WObj:=wobj1;
MoveL p20,speed1,z50,tool1\WObj:=wobj1;
MoveL p21,speed1,z50,tool1\WObj:=wobj1;
MoveL p22,speed1,z50,tool1\WObj:=wobj1;
MoveL p23,speed1,z50,tool1\WObj:=wobj1;
MoveL p24,speed1,z50,tool1\WObj:=wobj1;
MoveL p25,speed1,z50,tool1\WObj:=wobj1;
MoveL p26,speed1,z50,tool1\WObj:=wobj1;
MoveL p27,speed1,z50,tool1\WObj:=wobj1;
MoveL p28,speed1,z50,tool1\WObj:=wobj1;
MoveL p29,speed1,z50,tool1\WObj:=wobj1;
MoveL p30,speed1,z50,tool1\WObj:=wobj1;
ENDPROC
PROC main()
zone1:=z5;
SocketPro;
error
IF ERRNO=ERR_SOCK_TIMEOUT THEN
ELSE
SocketPro;
RETURN ;
ENDIF
ENDPROC
PROC loadexample()
VAR robtarget ptemp:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
VAR robtarget pPlace:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
MoveL Offs(pTemp,0,0,10),speed1,z50,tool1\WObj:=wobj1;
MoveL pTemp,speed1,fine,tool1\WObj:=wobj1;
GripLoad load1;
MoveL Offs(pTemp,0,0,10),speed1,z50,tool1\WObj:=wobj1;
MoveL Offs(pPlace,0,0,10),speed1,z50,tool1\WObj:=wobj1;
MoveL pPlace,speed1,fine,tool1\WObj:=wobj1;
GripLoad load0;
MoveL Offs(pPlace,0,0,10),speed1,z50,tool1\WObj:=wobj1;
ENDPROC
PROC socketRecover()
SocketClose socket_client;
SocketCreate socket_client;
SocketConnect socket_client,str_address,n_port\Time:=5;
ERROR
IF ERRNO=ERR_SOCK_TIMEOUT THEN
ResetRetryCount;
RETRY;
ENDIF
ENDPROC
PROC SocketPro()
TPErase;
SocketClose socket_client;
!create client
SocketCreate socket_client;
!connect server
SocketConnect socket_client,str_address,n_port\Time:=WAIT_MAX;
WHILE TRUE DO
pTarget:=CRobT(\Tool:=tool1);
ptemp:=pTarget;
!receive instruction
label1:
SocketReceive socket_client\Str:=strReceive;
dataOk:=FALSE;
strParse strReceive;
IF dataOk THEN
! GOTO label1;
IF revice_Str1_Cmd="movep" THEN
IF booLineMove THEN
MoveL pTarget,speed1,fine,tool1\WObj:=wobj1;
ELSE
MoveJ pTarget,speed1,fine,tool1\WObj:=wobj1;
ENDIF
SocketSend socket_client\Str:=strReceive+resultStr_OK;
!SocketSend socket_client\Str:=revice_Str1_Cmd+","+revice_Str2_pName+","+resultStr_OK+";";
ELSEIF revice_Str1_Cmd="moveget" THEN
IF booLineMove THEN
! MoveL Offs(pTarget,0,0,10), speed1, zone1, tool1\WObj:=wobj1;
MoveL pTarget,speed1,fine,tool1\WObj:=wobj1;
! GripLoad load0;
MoveL Offs(pTarget,0,0,-50),speed1,zone1,tool1\WObj:=wobj1;
MoveL Offs(pTarget,0,-60,0),speed1,zone1,tool1\WObj:=wobj1;
ELSE
MoveJ pTarget,speed1,fine,tool1\WObj:=wobj1;
ENDIF
SocketSend socket_client\Str:=strReceive+resultStr_OK ;
! SocketSend socket_client\Str:=revice_Str1_Cmd+","+revice_Str2_pName+","+resultStr_OK+";";
ELSEIF revice_Str1_Cmd="moveput" THEN
IF booLineMove THEN
MoveL pTarget,speed1,fine,tool1\WObj:=wobj1;
! GripLoad load1;
MoveL Offs(pTarget,0,0,50),speed1,zone1,tool1\WObj:=wobj1;
MoveL Offs(pTarget,0,-60,0),speed1,zone1,tool1\WObj:=wobj1;
ELSE
MoveJ pTarget,speed1,fine,tool1\WObj:=wobj1;
ENDIF
SocketSend socket_client\Str:=strReceive+resultStr_OK ;
! SocketSend socket_client\Str:=revice_Str1_Cmd+","+revice_Str2_pName+","+resultStr_OK+";";
ELSEIF revice_Str1_Cmd="validateP" THEN
!WaitRob\ZeroSpeed;
!wait robot stop;
pCheckTest:=CRobT(\Tool:=tool1\WObj:=wobj1);
!get current position
pCheckResult:=false;
pCheckResult:=validateP(pTarget,pCheckTest,0.1);
IF pCheckResult THEN
SocketSend socket_client\Str:=strReceive+resultStr_OK ;
ELSE
SocketSend socket_client\Str:=strReceive+resultStr_NG ;
ENDIF
ELSE
SocketSend socket_client\Str:=strReceive+" DATA ERROR ;";
ENDIF
ENDIF
ENDWHILE
ERROR
IF ERRNO=ERR_SOCK_TIMEOUT THEN
ResetRetryCount;
RETRY;
ELSEIF ERRNO=ERR_SOCK_CLOSED THEN
socketRecover;
ResetRetryCount;
RETRY;
ENDIF
ENDPROC
FUNC bool validateP(robtarget ptest,robtarget pCompare,num threshold)
VAR jointtarget joinTest:=[[-12.1057,1.3138,-1.32846,-3.7545E-7,90.0147,-12.1057],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
VAR jointtarget joinCompare:=[[-12.143,1.32099,-1.33096,-0.000317427,90.0147,-12.1215],[9E+9,9E+9,9E+9,9E+9,9E+9,9E+9]];
VAR num counter;
joinTest:=CalcJointT(pTest,tool1\WObj:=wobj1);
joinCompare:=CalcJointT(pCompare,tool1\WObj:=wobj1);
IF abs(joinTest.robax.rax_1-joinCompare.robax.rax_1)<threshold incr counter;
IF abs(joinTest.robax.rax_2-joinCompare.robax.rax_2)<threshold incr counter;
IF abs(joinTest.robax.rax_3-joinCompare.robax.rax_3)<threshold incr counter;
IF abs(joinTest.robax.rax_4-joinCompare.robax.rax_4)<threshold incr counter;
IF abs(joinTest.robax.rax_5-joinCompare.robax.rax_5)<threshold incr counter;
IF abs(joinTest.robax.rax_6-joinCompare.robax.rax_6)<threshold incr counter;
IF counter=6 THEN
RETURN TRUE;
ELSE
RETURN FALSE;
ENDIF
ENDFUNC
PROC strParse(string reviceStr)
VAR num revice_speed:=10;
lable2:
len:=StrLen(reviceStr);
starBit1:=1;
endBit1:=StrFind(reviceStr,starBit1,",");
LengBit1:=endBit1-starBit1;
starBit2:=endBit1+1;
IF starBit2>len THEN
RETURN ;
ENDIF
endBit2:=StrFind(reviceStr,starBit2,",");
LengBit2:=endBit2-starBit2;
starBit3:=endBit2+1;
IF starBit3>len THEN
RETURN ;
ENDIF
endBit3:=StrFind(reviceStr,starBit3,",");
LengBit3:=endBit3-starBit3;
starBit4:=endBit3+1;
IF starBit4>len THEN
RETURN ;
ENDIF
endBit4:=StrFind(reviceStr,starBit4,",");
LengBit4:=endBit4-starBit4;
revice_Str1_Cmd:=StrPart(reviceStr,starBit1,LengBit1);
revice_Str2_pName:=StrPart(reviceStr,starBit2,LengBit2);
revice_Str3_moveType:=StrPart(reviceStr,starBit3,LengBit3);
revice_Str4_speed:=StrPart(reviceStr,starBit4,LengBit4);
!capture target
GetDataVal revice_Str2_pName,ptemp;
pTarget:=ptemp;
!determine L or J
IF revice_Str3_moveType="L" THEN
booLineMove:=TRUE;
ELSEIF revice_Str3_moveType="J" THEN
booLineMove:=FALSE;
ENDIF
! get speed
flag1:=StrToVal(revice_Str4_speed,revice_speed);
IF revice_speed>100 THEN
revice_speed:=100;
ENDIF
speed1.v_tcp:=revice_speed;
dataOk:=TRUE;
ERROR
IF errno=ERR_SOCK_CLOSED THEN
ELSE
dataOk:=FALSE;
RETURN ;
ENDIF
ENDPROC
!PROC Path_10()
! MoveL Target_10,v1000,z100,tool1\WObj:=wobj1;
! MoveL Target_20,v1000,z100,tool1\WObj:=wobj1;
!ENDPROC
PROC Routine1()
MoveL [[-59.00,-191.91,358.37],[0.718539,-0.637964,0.152099,0.231451],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v100,z50,tool1\WObj:=wobj1;
MoveL [[117.05,-193.58,358.36],[0.718537,-0.63796,0.1521,0.231467],[-1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v100,z10,tool1\WObj:=wobj1;
MoveL [[117.05,-193.58,473.42],[0.718538,-0.637961,0.152095,0.231465],[-1,-1,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v100,z10,tool1\WObj:=wobj1;
MoveL [[-16.00,-192.31,473.42],[0.718545,-0.637953,0.152097,0.231464],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]],v100,z10,tool1\WObj:=wobj1;
ENDPROC
ENDMODULE