机器人程序.txt
12.2 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
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:=[[811.5983,-418.1001,718.5829],[0.05370732,1.526351E-08,-0.9985567,2.109476E-08],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p2:=[[825.4576,0.000000156,533.4979],[0.03072789,-0.000000002,-0.9995278,-0.000000001],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p3:=[[825.4576,0.000001046,609.3],[0.03072784,-0.000000013,-0.9995278,-0.00000001],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p4:=[[825.4576,-0.000000111,675.4476],[0.03072794,0.000000001,-0.9995278,0.000000001],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p5:=[[825.4576,0.000000831,745.9818],[0.03072797,-0.000000011,-0.9995278,-0.000000008],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p6:=[[825.4576,0.000000659,815.8895],[0.03072799,-0.000000008,-0.9995278,-0.000000006],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p7:=[[825.4576,-0.000000347,875.2359],[0.03072807,0.000000004,-0.9995278,0.000000003],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p8:=[[825.4576,0.000001138,914.6687],[0.03072812,-0.000000014,-0.9995278,-0.000000011],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p9:=[[825.4576,0.000000481,954.2989],[0.03072822,-0.000000006,-0.9995278,-0.000000004],[0,0,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p10:=[[825.4576,-0.00000035,996.6124],[0.03072831,0.000000004,-0.9995278,0.000000003],[0,0,-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:=[345,50,400,100];
VAR socketdev socket_server;
VAR socketdev socket_client;
PERS string str_address:="127.0.0.1";
PERS num n_port:=21;
VAR string strReceive:="p1,L,100";
VAR string strSend:="OK";
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 s1_1:="movep";
PERS string s1_2:="p27";
PERS string s1_3:="l";
PERS string s1_4:="345";
VAR bool flag1:=false;
PERS num value_1:=700;
PERS num value_2:=50;
PERS num value_3:=345;
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 bLinear:=TRUE;
PERS bool dataOk:=TRUE;
VAR num len:=0;
PROC modifyPoint()
MoveL p1,speed1,z50,MyTool\WObj:=wobj0;
MoveL p2,speed1,z50,MyTool\WObj:=wobj0;
MoveL p3,speed1,z50,MyTool\WObj:=wobj0;
MoveL p4,speed1,z50,MyTool\WObj:=wobj0;
MoveL p5,speed1,z50,MyTool\WObj:=wobj0;
MoveL p6,speed1,z50,MyTool\WObj:=wobj0;
MoveL p7,speed1,z50,MyTool\WObj:=wobj0;
MoveL p8,speed1,z50,MyTool\WObj:=wobj0;
MoveL p9,speed1,z50,MyTool\WObj:=wobj0;
MoveL p10,speed1,z50,MyTool\WObj:=wobj0;
MoveL p11,speed1,z50,MyTool\WObj:=wobj0;
MoveL p12,speed1,z50,MyTool\WObj:=wobj0;
MoveL p13,speed1,z50,MyTool\WObj:=wobj0;
MoveL p14,speed1,z50,MyTool\WObj:=wobj0;
MoveL p15,speed1,z50,MyTool\WObj:=wobj0;
MoveL p16,speed1,z50,MyTool\WObj:=wobj0;
MoveL p17,speed1,z50,MyTool\WObj:=wobj0;
MoveL p18,speed1,z50,MyTool\WObj:=wobj0;
MoveL p19,speed1,z50,MyTool\WObj:=wobj0;
MoveL p20,speed1,z50,MyTool\WObj:=wobj0;
MoveL p21, speed1, z50, MyTool\WObj:=wobj0;
MoveL p22,speed1,z50,MyTool\WObj:=wobj0;
MoveL p23,speed1,z50,MyTool\WObj:=wobj0;
MoveL p24,speed1,z50,MyTool\WObj:=wobj0;
MoveL p25,speed1,z50,MyTool\WObj:=wobj0;
MoveL p26,speed1,z50,MyTool\WObj:=wobj0;
MoveL p27,speed1,z50,MyTool\WObj:=wobj0;
MoveL p28,speed1,z50,MyTool\WObj:=wobj0;
MoveL p29,speed1,z50,MyTool\WObj:=wobj0;
MoveL p30,speed1,z50,MyTool\WObj:=wobj0;
ENDPROC
PROC main()
SocketPro;
error
IF ERRNO=ERR_SOCK_TIMEOUT THEN
! socketclose socket_client;
! socketcreate socket_client;
! SocketConnect socket_client,str_address,n_port\Time:=5;
! ResetRetrycount;
! RETRY;
! ELSEIF ERRNO=ERR_SOCK_CLOSED THEN
! socketclose socket_client;
! socketcreate socket_client;
! SocketConnect socket_client,str_address,n_port\Time:=5;
! ResetRetrycount;
! RETRY;
ELSE
SocketPro;
RETURN ;
ENDIF
ENDPROC
! PROC Reconnect()
! TPErase;
! SocketCreate socket_client;
! SocketConnect socket_client,str_address,n_port\Time:=2;
! WHILE TRUE DO
! WaitTime 1;
! SocketReceive socket_client\Str:=strReceive;
! SocketSend socket_client\Str:=strSend;
! strSend:=strSend+"a";
! ENDWHILE
! ERROR
! IF ERRNO=ERR_SOCK_TIMEOUT THEN
! ResetRetryCount;
! RETRY;
! ELSEIF ERRNO=ERR_SOCK_CLOSED THEN
! socketRecover;
! ResetRetryCount;
! RETRY;
! ENDIF
! 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:=MyTool);
ptemp:=pTarget;
!receive instruction
label1:
SocketReceive socket_client\Str:=strReceive;
dataOk:=FALSE;
strParse strReceive;
IF dataOk THEN
! GOTO label1;
IF s1_1="movep" THEN
IF bLinear THEN
MoveL pTarget,speed1,fine,MyTool\WObj:=wobj0;
ELSE
MoveJ pTarget,speed1,fine,MyTool\WObj:=wobj0;
ENDIF
SocketSend socket_client\Str:=strReceive+strSend;
SocketSend socket_client\Str:=s1_1+","+s1_2+","+strSend;
ELSEIF s1_1="move2" THEN
IF bLinear THEN
MoveL Offs(pTarget,0,0,10),speed1,fine,MyTool\WObj:=wobj0;
MoveL pTarget,speed1,fine,MyTool\WObj:=wobj0;
MoveL Offs(pTarget,-50,0,0),speed1,fine,MyTool\WObj:=wobj0;
MoveL Offs(pTarget,0,60,0),speed1,fine,MyTool\WObj:=wobj0;
ELSE
MoveJ pTarget,speed1,fine,MyTool\WObj:=wobj0;
ENDIF
SocketSend socket_client\Str:=strReceive+strSend;
SocketSend socket_client\Str:=s1_1+","+s1_2+","+strSend;
ENDIF
ELSE
SocketSend socket_client\Str:=strReceive+" DATA ERROR ";
ENDIF
ENDWHILE
ERROR
IF ERRNO=ERR_SOCK_TIMEOUT THEN
ResetRetryCount;
RETRY;
ELSEIF ERRNO=ERR_SOCK_CLOSED THEN
socketRecover;
ResetRetryCount;
RETRY;
ENDIF
ENDPROC
PROC strParse(string string1)
lable2:
len:=StrLen(string1);
starBit1:=1;
endBit1:=StrFind(string1,starBit1,",");
LengBit1:=endBit1-starBit1;
starBit2:=endBit1+1;
IF starBit2>len THEN
RETURN ;
ENDIF
endBit2:=StrFind(string1,starBit2,",");
LengBit2:=endBit2-starBit2;
starBit3:=endBit2+1;
IF starBit3>len THEN
RETURN ;
ENDIF
endBit3:=StrFind(string1,starBit3,",");
LengBit3:=endBit3-starBit3;
starBit4:=endBit3+1;
IF starBit4>len THEN
RETURN ;
ENDIF
endBit4:=StrFind(string1,starBit4,",");
LengBit4:=endBit4-starBit4;
s1_1:=StrPart(string1,starBit1,LengBit1);
s1_2:=StrPart(string1,starBit2,LengBit2);
s1_3:=StrPart(string1,starBit3,LengBit3);
s1_4:=StrPart(string1,starBit4,LengBit4);
!capture target
GetDataVal s1_2,ptemp;
pTarget:=ptemp;
!determine L or J
IF s1_3="L" THEN
bLinear:=TRUE;
ELSEIF s1_3="J" THEN
bLinear:=FALSE;
ENDIF
! get speed
flag1:=StrToVal(s1_4,value_3);
speed1.v_tcp:=value_3;
dataOk:=TRUE;
ERROR
IF errno=ERR_SOCK_CLOSED THEN
ELSE
dataOk:=FALSE;
RETURN ;
ENDIF
ENDPROC
PROC Path_10()
! MoveL Target_10,v1000,z100,MyTool\WObj:=wobj0;
! MoveL Target_20,v1000,z100,MyTool\WObj:=wobj0;
ENDPROC
ENDMODULE