52程序.txt
42.3 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
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
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.17,-7.50,-145.81],[0.995108,-0.00529605,-0.00620484,0.0984543],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p2:=[[73.77,1.66,-146.98],[0.995104,-0.00528212,-0.00619488,0.0985003],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p3:=[[124.21,10.00,-145.62],[0.995108,-0.00527065,-0.00618692,0.0984586],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p4:=[[161.35,15.98,-145.85],[0.995111,-0.00526213,-0.00612356,0.0984314],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p5:=[[-229.91,346.95,459.70],[4.83347E-05,-0.362215,0.932095,-0.000395109],[-1,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget p6:=[[-227.57,512.43,459.54],[1.03816E-05,0.362197,-0.932102,0.000179463],[-1,0,0,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 Home_Wobj1:=[[-172.69,-38.62,-160.40],[0.995107,-0.00528568,-0.00629137,0.0984558],[-1,-3,-3,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Home_Wobj2:=[[-203,23,6],[1,0,0,0],[-1,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget Temp_Wobj2:=[[-301.05,-391.76,438.17],[0.998911,-0.00076512,-0.00303646,-0.046547],[-1,0,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
!*****12 matrix put position
CONST robtarget cp1:=[[345.43,238.53,499.39],[0.999961,-0.00767894,-0.000719497,0.00437209],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp2:=[[349.88,235.10,397.10],[0.999749,-0.0214884,-0.00250497,0.00580575],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp3:=[[350.72,235.69,290.74],[0.999749,-0.0214991,-0.00250919,0.00579997],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp4:=[[351.55,236.29,185.26],[0.999748,-0.0215145,-0.0025124,0.00581933],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp5:=[[352.37,236.88,81.67],[0.999748,-0.021528,-0.00250979,0.00583732],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp6:=[[353.17,237.48,-20.55],[0.999747,-0.0215573,-0.00250887,0.00589586],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp7:=[[345.92,763.75,505.22],[0.999942,-0.00939056,-0.00510156,-0.00166134],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp8:=[[346.13,766.75,398.91],[0.999985,-0.00433806,-0.00329039,-0.000584401],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp9:=[[357.63,767.27,292.29],[0.999985,-0.00432659,-0.00328373,-0.000531956],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp10:=[[358.48,769.82,188.67],[0.999985,-0.00431653,-0.00326409,-0.000525566],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp11:=[[359.30,770.40,84.78],[0.999985,-0.00431388,-0.003262,-0.000523857],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget cp12:=[[355.12,769.98,-21.95],[0.999748,-0.0215194,-0.00249739,0.00590012],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp1:=[[337.69,134.02,632.11],[0.999877,-0.0121344,-0.00469915,0.0087367],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp2:=[[340.63,134.20,600.92],[0.999877,-0.0121303,-0.00472418,0.00876584],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp3:=[[351.21,134.23,570.70],[0.99988,-0.0121422,-0.00401846,0.00875981],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp4:=[[350.81,135.46,539.88],[0.999909,-0.00969493,-0.00224496,0.00912136],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp5:=[[351.05,135.65,508.92],[0.999909,-0.00969575,-0.00225711,0.00914035],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp6:=[[351.28,135.83,479.10],[0.999908,-0.00969568,-0.00226564,0.00916041],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp7:=[[351.59,136.43,447.25],[0.999833,-0.0155925,-0.00229068,0.00929],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp8:=[[351.83,136.60,417.50],[0.999833,-0.0155932,-0.00228097,0.00929406],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp9:=[[352.08,136.79,386.38],[0.999833,-0.0155771,-0.00227957,0.00931142],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp10:=[[352.32,136.97,356.47],[0.999832,-0.0155882,-0.00227877,0.00933308],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp11:=[[352.56,137.14,326.76],[0.999832,-0.015614,-0.00227344,0.00934752],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp12:=[[353.44,137.35,295.65],[0.999826,-0.0155961,-0.00405226,0.00935448],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp13:=[[353.68,137.53,265.23],[0.999826,-0.0155994,-0.00405334,0.00937555],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp14:=[[353.93,137.72,234.23],[0.999826,-0.0155997,-0.00405075,0.00939318],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp15:=[[354.16,137.90,203.81],[0.999826,-0.0156103,-0.0040453,0.00941586],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp16:=[[354.40,138.09,173.24],[0.999825,-0.015628,-0.00403922,0.00945458],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp17:=[[354.63,138.27,143.31],[0.999825,-0.0156403,-0.00403433,0.00948092],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp18:=[[354.86,138.46,113.22],[0.999824,-0.0156569,-0.00402251,0.00951363],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp19:=[[355.10,138.64,82.50],[0.999824,-0.0156514,-0.00402062,0.00953432],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp20:=[[353.17,138.90,50.63],[0.999818,-0.0156433,-0.00533198,0.00956749],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp21:=[[353.40,139.08,20.17],[0.999817,-0.015663,-0.00532834,0.00960587],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp22:=[[353.63,139.27,-9.71],[0.999816,-0.0156909,-0.00532695,0.00965102],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp23:=[[353.87,139.47,-40.78],[0.999815,-0.0157141,-0.0053249,0.00970075],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp24:=[[331.47,381.41,633.39],[0.999811,-0.0147978,-0.0054917,0.0113349],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp25:=[[337.93,380.75,602.91],[0.999915,-0.0107735,-0.00427597,0.00601987],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp26:=[[354.75,380.77,572.96],[0.999915,-0.0107634,-0.00427666,0.00606778],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp27:=[[353.19,380.41,541.74],[0.999906,-0.0122162,0.000789537,0.00613096],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp28:=[[353.96,380.76,511.16],[0.999911,-0.011798,-0.000693003,0.00613811],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp29:=[[354.22,381.58,480.86],[0.999911,-0.0117871,-0.000705914,0.00613887],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp30:=[[354.09,381.64,449.94],[0.999908,-0.0120991,0.000366658,0.00615926],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp31:=[[354.33,381.82,418.58],[0.999908,-0.0120977,0.00036414,0.00615959],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp32:=[[354.57,382.57,388.85],[0.999908,-0.0120948,0.000374984,0.0061522],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp33:=[[354.43,382.37,358.11],[0.999786,-0.0193647,0.00157683,0.00713599],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp34:=[[354.66,382.53,328.90],[0.999785,-0.0193752,0.00157953,0.00714703],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp35:=[[355.63,384.01,296.42],[0.999885,-0.0139007,-0.000440376,0.00602468],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp36:=[[355.86,384.19,266.52],[0.999885,-0.0138995,-0.000438155,0.00604482],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp37:=[[356.11,384.36,236.68],[0.999885,-0.013909,-0.000428451,0.00605974],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp38:=[[356.75,384.67,205.23],[0.999888,-0.0135918,-0.00153155,0.00606301],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp39:=[[356.99,384.82,176.04],[0.999888,-0.01362,-0.00152927,0.00606686],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp40:=[[357.23,385.00,144.80],[0.999887,-0.013632,-0.00151192,0.00608646],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp41:=[[356.28,385.76,113.96],[0.999887,-0.0122202,-0.00632451,0.00608557],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp42:=[[356.09,385.81,83.10],[0.999889,-0.012618,-0.00506072,0.00613563],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp43:=[[355.86,385.85,52.24],[0.999889,-0.0130289,-0.00371186,0.00616507],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp44:=[[356.10,386.03,21.94],[0.999889,-0.0130492,-0.00371514,0.0062008],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp45:=[[356.33,386.22,-8.88],[0.999888,-0.0130702,-0.00371638,0.00623696],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp46:=[[356.34,386.32,-39.17],[0.999887,-0.0132707,-0.003094,0.00627156],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp47:=[[326.74,623.25,635.32],[0.999914,-0.0107986,-0.00426971,0.0061048],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp48:=[[333.58,623.38,603.69],[0.999914,-0.0107812,-0.00427024,0.00612283],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp49:=[[357.46,625.47,574.49],[0.999952,-0.00305262,-0.00516121,0.00776183],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp50:=[[356.93,625.22,543.81],[0.999956,-0.00426976,-0.002983,0.00780496],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp51:=[[356.58,625.09,513.44],[0.9999,-0.0122717,-0.00117482,0.00686932],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp52:=[[356.14,624.55,482.99],[0.999933,-0.00900366,0.000623937,0.00731607],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp53:=[[357.13,625.16,451.50],[0.999942,-0.00782208,-0.00149072,0.00730679],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp54:=[[357.38,626.77,421.91],[0.999942,-0.00781879,-0.00148705,0.0073069],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp55:=[[357.64,626.96,390.75],[0.999941,-0.00782284,-0.00148208,0.00732235],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp56:=[[358.28,627.46,360.18],[0.999906,-0.0110666,-0.00254013,0.0076485],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp57:=[[358.00,627.62,329.58],[0.999937,-0.00846024,-0.00107243,0.00723399],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp58:=[[358.24,627.80,298.84],[0.999937,-0.00847453,-0.00105525,0.00724562],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp59:=[[359.05,627.49,269.18],[0.999901,-0.0112571,-0.00261928,0.00804665],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp60:=[[358.59,627.27,239.25],[0.999891,-0.0123736,-0.000660279,0.00808572],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp61:=[[358.84,627.46,207.23],[0.99989,-0.0123907,-0.000652971,0.00810959],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp62:=[[359.01,627.59,177.60],[0.999888,-0.0125182,-0.000456755,0.00813404],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp63:=[[359.72,629.59,146.82],[0.999896,-0.0118021,-0.00175152,0.0081358],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp64:=[[359.96,629.77,115.89],[0.999896,-0.0118023,-0.00174636,0.00816421],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp65:=[[360.20,629.95,85.60],[0.999895,-0.0118218,-0.00175258,0.00818474],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp66:=[[360.44,630.12,55.55],[0.999895,-0.0118344,-0.00175363,0.00819404],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp67:=[[359.69,629.72,24.76],[0.999876,-0.0134108,0.0010085,0.00822693],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp68:=[[359.93,629.90,-6.08],[0.999875,-0.0134332,0.00101617,0.00825616],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp69:=[[360.49,630.27,-37.18],[0.999882,-0.0129373,0.000115525,0.00827929],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp70:=[[117.47,871.62,635.17],[0.999952,-0.00307383,-0.00516145,0.00772701],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp71:=[[224.70,870.71,605.54],[0.999952,-0.00306043,-0.005148,0.00775771],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp72:=[[287.65,870.22,576.13],[0.999952,-0.00309618,-0.00515484,0.00776251],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp73:=[[354.95,869.73,546.40],[0.999952,-0.00308384,-0.00515957,0.00778448],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp74:=[[355.19,869.90,515.92],[0.999952,-0.00309032,-0.00516533,0.00778619],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp75:=[[360.55,868.83,484.41],[0.999948,-0.00647655,-0.00111027,0.00783676],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp76:=[[361.11,869.86,453.38],[0.999941,-0.00749554,-0.001949,0.00757678],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp77:=[[361.34,870.03,423.27],[0.999941,-0.00749537,-0.00194667,0.0075812],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp78:=[[361.79,869.92,392.32],[0.999935,-0.00818481,-0.00252626,0.00751235],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp79:=[[362.02,870.10,362.37],[0.999935,-0.00818043,-0.00252846,0.00753182],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp80:=[[362.28,870.95,331.56],[0.999935,-0.00819508,-0.00251152,0.00754881],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp81:=[[362.52,871.13,301.41],[0.999935,-0.00820144,-0.0025054,0.00755881],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp82:=[[361.80,871.31,270.44],[0.999935,-0.00820105,-0.00251058,0.00756183],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp83:=[[361.14,872.81,240.26],[0.999951,-0.00682655,5.30802E-05,0.00712324],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp84:=[[361.39,872.99,209.38],[0.999951,-0.00684082,4.53594E-05,0.00713508],[0,0,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp85:=[[360.10,872.95,180.45],[0.999935,-0.00836053,-0.00122894,0.00763245],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp86:=[[360.35,874.09,148.83],[0.999935,-0.00837051,-0.00121695,0.00764644],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp87:=[[361.05,874.16,118.36],[0.999913,-0.00989276,-0.00248368,0.0083394],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp88:=[[361.29,874.33,87.72],[0.999913,-0.00990059,-0.00248569,0.00835244],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp89:=[[360.72,873.81,56.73],[0.999895,-0.011825,-0.000197653,0.00836898],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp90:=[[360.96,875.20,26.53],[0.999895,-0.0118284,-0.000191439,0.00838305],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp91:=[[361.21,875.39,-4.54],[0.999895,-0.0118345,-0.000187378,0.00840726],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget dp92:=[[361.44,875.55,-34.39],[0.999894,-0.01184,-0.000180527,0.00841147],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
!*****12 matrix put position end
CONST robtarget p23:=[[325,777,-30],[1,0,0,0],[0,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
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]];
CONST robtarget p31:=[[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 p32:=[[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 p33:=[[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 p34:=[[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 p35:=[[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 p36:=[[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 p37:=[[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 p38:=[[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 p39:=[[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 p40:=[[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]];
CONST robtarget p41:=[[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 p42:=[[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 p43:=[[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 p44:=[[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 p45:=[[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 p46:=[[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 p47:=[[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 p48:=[[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 p49:=[[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 p50:=[[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]];
CONST robtarget pbackhomepos:=[[409.48,-496.58,1005.14],[0.999432,-0.0284694,0.00852407,0.0158778],[-1,-1,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget pbackhomepos10:=[[409.48,-496.58,1005.13],[0.999432,-0.0284685,0.00852428,0.0158778],[-1,-1,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
CONST robtarget jbackhomepos:=[[409.48,-496.58,1005.14],[0.999432,-0.0284698,0.00852399,0.0158778],[-1,-1,-3,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget PNowPos10:=[[582.05,356.05,1058.28],[0.0622783,0.000912002,-0.0132789,-0.99797],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS speeddata speed1:=[1000,600,600,600];
PERS speeddata speed2:=[350,350,350,300];
PERS speeddata speed3:=[200,200,200,200];
VAR socketdev socket_server;
VAR socketdev socket_client;
PERS string str_address:="192.168.103.12";
!PERS string str_address:="192.168.201.88";
PERS num n_port:=10021;
Var pos now_pos:=[0,0,0];
VAR robtarget PNowPos;
VAR string strReceive:="p1,L,100";
VAR string resultStr_FINISHED:=",FINISHED;";
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 shelfType:="p";
PERS string revice_Str1_Cmd:="moveget";
PERS string revice_Str2_pName:="p1";
PERS string revice_Str3_moveType:="L";
PERS string revice_Str4_speed:="1000";
VAR bool flag1:=false;
!PERS num value_1:=700;
!PERS num value_2:=50;
VAR robtarget pTarget:=[[16.70,-8.84,-146.71],[0.995232,-0.0069405,-0.00673188,0.0970544],[-1,-3,-3,1],[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;
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.73995,-351.15,465.56],[0.300709,-0.919332,0.248148,0.0531501],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
PERS bool pCheckResult:=TRUE;
PERS tooldata tooltest:=[TRUE,[[-208.798,-217.638,93.8838],[1,0,0,0]],[1.3,[-31.7,-112.8,55.6],[1,0,0,0],0,0.045,0.029]];
PERS wobjdata wobjtest:=[FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[1033.18,-257.207,479.665],[2.98351E-05,0.707094,0.707119,6.49066E-05]]];
PERS tooldata tool1:=[TRUE,[[209.468,-213.3,114.724],[0.0139655,0.916976,-0.398699,-0.000349054]],[1.7,[123.1,-86.1,47.4],[1,0,0,0],0.028,0,0.035]];
PERS wobjdata wobj1:=[FALSE,TRUE,"",[[137,-964,808.5],[0.00791395,0.00160744,0.0045071,0.999957]],[[0,0,0],[1,0,0,0]]];
PERS wobjdata wobj2:=[FALSE,TRUE,"",[[823,-121,536],[0.99997,-0.00283,0.00395,0.00521]],[[0,0,0],[1,0,0,0]]];
CONST robtarget p17:=[[232.66,0.00,29.24],[1,-1.36004E-05,1.18275E-06,-4.95038E-06],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
VAR robtarget PNowPos20:=[[647.80,-254.06,1005.12],[0.999435,-0.0284313,0.00857599,0.0157199],[-1,-1,-2,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
TASK PERS tooldata tool3:=[TRUE,[[0,0,0],[1,0,0,0]],[-1,[0,0,0],[1,0,0,0],0,0,0]];
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 cp1,speed1,z50,tool1\WObj:=wobj2;
MoveL cp2,speed1,z50,tool1\WObj:=wobj2;
MoveL cp3,speed1,z50,tool1\WObj:=wobj2;
MoveL cp4,speed1,z50,tool1\WObj:=wobj2;
MoveL cp5,speed1,z50,tool1\WObj:=wobj2;
MoveL cp6,speed1,z50,tool1\WObj:=wobj2;
MoveL cp7,speed1,z50,tool1\WObj:=wobj2;
MoveL cp8,speed1,z50,tool1\WObj:=wobj2;
MoveL cp9,speed1,z50,tool1\WObj:=wobj2;
MoveL cp10,speed1,z50,tool1\WObj:=wobj2;
MoveL cp11,speed1,z50,tool1\WObj:=wobj2;
MoveL cp12,speed1,z50,tool1\WObj:=wobj2;
MoveL dp1,speed1,z50,tool1\WObj:=wobj2;
MoveL dp2,speed1,z50,tool1\WObj:=wobj2;
MoveL dp3,speed1,z50,tool1\WObj:=wobj2;
MoveL dp4,speed1,z50,tool1\WObj:=wobj2;
MoveL dp5,speed1,z50,tool1\WObj:=wobj2;
MoveL dp6,speed1,z50,tool1\WObj:=wobj2;
MoveL dp7,speed1,z50,tool1\WObj:=wobj2;
MoveL dp8,speed1,z50,tool1\WObj:=wobj2;
MoveL dp9,speed1,z50,tool1\WObj:=wobj2;
MoveL dp10,speed1,z50,tool1\WObj:=wobj2;
MoveL dp11,speed1,z50,tool1\WObj:=wobj2;
MoveL dp12,speed1,z50,tool1\WObj:=wobj2;
MoveL dp13,speed1,z50,tool1\WObj:=wobj2;
MoveL dp14,speed1,z50,tool1\WObj:=wobj2;
MoveL dp15,speed1,z50,tool1\WObj:=wobj2;
MoveL dp16,speed1,z50,tool1\WObj:=wobj2;
MoveL dp17,speed1,z50,tool1\WObj:=wobj2;
MoveL dp18,speed1,z50,tool1\WObj:=wobj2;
MoveL dp19,speed1,z50,tool1\WObj:=wobj2;
MoveL dp20,speed1,z50,tool1\WObj:=wobj2;
MoveL dp21,speed1,z50,tool1\WObj:=wobj2;
MoveL dp22,speed1,z50,tool1\WObj:=wobj2;
MoveL dp23,speed1,z50,tool1\WObj:=wobj2;
MoveL dp24,speed1,z50,tool1\WObj:=wobj2;
MoveL dp25,speed1,z50,tool1\WObj:=wobj2;
MoveL dp26,speed1,z50,tool1\WObj:=wobj2;
MoveL dp27,speed1,z50,tool1\WObj:=wobj2;
MoveL dp28,speed1,z50,tool1\WObj:=wobj2;
MoveL dp29,speed1,z50,tool1\WObj:=wobj2;
MoveL dp30,speed1,z50,tool1\WObj:=wobj2;
MoveL dp31,speed1,z50,tool1\WObj:=wobj2;
MoveL dp32,speed1,z50,tool1\WObj:=wobj2;
MoveL dp33,speed1,z50,tool1\WObj:=wobj2;
MoveL dp34,speed1,z50,tool1\WObj:=wobj2;
MoveL dp35,speed1,z50,tool1\WObj:=wobj2;
MoveL dp36,speed1,z50,tool1\WObj:=wobj2;
MoveL dp37,speed1,z50,tool1\WObj:=wobj2;
MoveL dp38,speed1,z50,tool1\WObj:=wobj2;
MoveL dp39,speed1,z50,tool1\WObj:=wobj2;
MoveL dp40,speed1,z50,tool1\WObj:=wobj2;
MoveL dp41,speed1,z50,tool1\WObj:=wobj2;
MoveL dp42,speed1,z50,tool1\WObj:=wobj2;
MoveL dp43,speed1,z50,tool1\WObj:=wobj2;
MoveL dp44,speed1,z50,tool1\WObj:=wobj2;
MoveL dp45,speed1,z50,tool1\WObj:=wobj2;
MoveL dp46,speed1,z50,tool1\WObj:=wobj2;
MoveL dp47,speed1,z50,tool1\WObj:=wobj2;
MoveL dp48,speed1,z50,tool1\WObj:=wobj2;
MoveL dp49,speed1,z50,tool1\WObj:=wobj2;
MoveL dp50,speed1,z50,tool1\WObj:=wobj2;
MoveL dp51,speed1,z50,tool1\WObj:=wobj2;
MoveL dp52,speed1,z50,tool1\WObj:=wobj2;
MoveL dp53,speed1,z50,tool1\WObj:=wobj2;
MoveL dp54,speed1,z50,tool1\WObj:=wobj2;
MoveL dp55,speed1,z50,tool1\WObj:=wobj2;
MoveL dp56,speed1,z50,tool1\WObj:=wobj2;
MoveL dp57,speed1,z50,tool1\WObj:=wobj2;
MoveL dp58,speed1,z50,tool1\WObj:=wobj2;
MoveL dp59,speed1,z50,tool1\WObj:=wobj2;
MoveL dp60,speed1,z50,tool1\WObj:=wobj2;
MoveL dp61,speed1,z50,tool1\WObj:=wobj2;
MoveL dp62,speed1,z50,tool1\WObj:=wobj2;
MoveL dp63,speed1,z50,tool1\WObj:=wobj2;
MoveL dp64,speed1,z50,tool1\WObj:=wobj2;
MoveL dp65,speed1,z50,tool1\WObj:=wobj2;
MoveL dp66,speed1,z50,tool1\WObj:=wobj2;
MoveL dp67,speed1,z50,tool1\WObj:=wobj2;
MoveL dp68,speed1,z50,tool1\WObj:=wobj2;
MoveL dp69,speed1,z50,tool1\WObj:=wobj2;
MoveL dp70,speed1,z50,tool1\WObj:=wobj2;
MoveL dp71,speed1,z50,tool1\WObj:=wobj2;
MoveL dp72,speed1,z50,tool1\WObj:=wobj2;
MoveL dp73,speed1,z50,tool1\WObj:=wobj2;
MoveL dp74,speed1,z50,tool1\WObj:=wobj2;
MoveL dp75,speed1,z50,tool1\WObj:=wobj2;
MoveL dp76,speed1,z50,tool1\WObj:=wobj2;
MoveL dp77,speed1,z50,tool1\WObj:=wobj2;
MoveL dp78,speed1,z50,tool1\WObj:=wobj2;
MoveL dp79,speed1,z50,tool1\WObj:=wobj2;
MoveL dp80,speed1,z50,tool1\WObj:=wobj2;
MoveL dp81,speed1,z50,tool1\WObj:=wobj2;
MoveL dp82,speed1,z50,tool1\WObj:=wobj2;
MoveL dp83,speed1,z50,tool1\WObj:=wobj2;
MoveL dp84,speed1,z50,tool1\WObj:=wobj2;
MoveL dp85,speed1,z50,tool1\WObj:=wobj2;
MoveL dp86,speed1,z50,tool1\WObj:=wobj2;
MoveL dp87,speed1,z50,tool1\WObj:=wobj2;
MoveL dp88,speed1,z50,tool1\WObj:=wobj2;
MoveL dp89,speed1,z50,tool1\WObj:=wobj2;
MoveL dp90,speed1,z50,tool1\WObj:=wobj2;
MoveL dp91,speed1,z50,tool1\WObj:=wobj2;
MoveL dp92,speed1,z50,tool1\WObj:=wobj2;
MoveL p23,speed1,z50,tool1\WObj:=wobj2;
MoveL p24,speed1,z50,tool1\WObj:=wobj2;
MoveL p25,speed1,z50,tool1\WObj:=wobj2;
MoveL p26,speed1,z50,tool1\WObj:=wobj2;
MoveL p27,speed1,z50,tool1\WObj:=wobj2;
MoveL p28,speed1,z50,tool1\WObj:=wobj2;
MoveL p29,speed1,z50,tool1\WObj:=wobj2;
MoveL p30,speed1,z50,tool1\WObj:=wobj2;
MoveL p31,speed1,z50,tool1\WObj:=wobj2;
MoveL p32,speed1,z50,tool1\WObj:=wobj2;
MoveL p33,speed1,z50,tool1\WObj:=wobj2;
MoveL p34,speed1,z50,tool1\WObj:=wobj2;
MoveL p35,speed1,z50,tool1\WObj:=wobj2;
MoveL p36,speed1,z50,tool1\WObj:=wobj2;
MoveL p37,speed1,z50,tool1\WObj:=wobj2;
MoveL p38,speed1,z50,tool1\WObj:=wobj2;
MoveL p39,speed1,z50,tool1\WObj:=wobj2;
MoveL p40,speed1,z50,tool1\WObj:=wobj2;
MoveL p41,speed1,z50,tool1\WObj:=wobj2;
MoveL p42,speed1,z50,tool1\WObj:=wobj2;
MoveL p43,speed1,z50,tool1\WObj:=wobj2;
MoveL p44,speed1,z50,tool1\WObj:=wobj2;
MoveL p45,speed1,z50,tool1\WObj:=wobj2;
MoveL p46,speed1,z50,tool1\WObj:=wobj2;
MoveL p47,speed1,z50,tool1\WObj:=wobj2;
MoveL p48,speed1,z50,tool1\WObj:=wobj2;
MoveL p49,speed1,z50,tool1\WObj:=wobj2;
MoveL p50,speed1,z50,tool1\WObj:=wobj2;
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="movehome" THEN
rBackHome;
SocketSend socket_client\Str:=strReceive+resultStr_OK;
!!!!!!!!!!!!!!!
ELSEIF revice_Str1_Cmd="moveget" THEN
MoveL Home_Wobj1, speed1, z50, tool1\WObj:=wobj1;
IF booLineMove THEN
! MoveL Offs(pTarget,0,0,10), speed1, zone1, tool1\WObj:=wobj1;
!MoveL Home_Wobj1,speed1,fine,tool1\WObj:=wobj1;
! GripLoad load0;
MoveL Offs(pTarget,0,0,-30), speed1, z80, tool1\WObj:=wobj1;
MoveL Offs(pTarget,0,0,80), speed3, z80, tool1\WObj:=wobj1;
!Set do_02;
!waittime 1;
MoveL RelTool(pTarget,-400,0,100\Rz:=90), speed2, z80, tool1\WObj:=wobj1;
MoveL RelTool(pTarget,-500,-100,100\Rz:=135), speed2, z80, 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
!MoveJ Home_Wobj2,speed1,fine,tool1\WObj:=wobj1;
IF booLineMove AND shelfType="c" THEN
MoveL Temp_Wobj2, speed2, z100, tool1\WObj:=wobj2;
! GripLoad load1;
MoveL Offs(pTarget,-450,0,20), speed2, zone1, tool1\WObj:=wobj2;
MoveL Offs(pTarget,0,0,20), speed2, zone1, tool1\WObj:=wobj2;
!reset do_02;
MoveL Offs(pTarget,0,0,-20), speed2, zone1, tool1\WObj:=wobj2;
SocketSend socket_client\Str:=strReceive+resultStr_FINISHED;
MoveL Offs(pTarget,-450,0,-20), speed2, zone1, tool1\WObj:=wobj2;
ELSEIF booLineMove AND shelfType="d" THEN
MoveL Temp_Wobj2, speed2, z100, tool1\WObj:=wobj2;
! GripLoad load1;
MoveL Offs(pTarget,-350,0,8), speed2, zone1, tool1\WObj:=wobj2;
MoveL Offs(pTarget,0,0,8), speed2, zone1, tool1\WObj:=wobj2;
MoveL Offs(pTarget,0,0,-9), speed2, zone1, tool1\WObj:=wobj2;
SocketSend socket_client\Str:=strReceive+resultStr_FINISHED;
MoveL Offs(pTarget,-350,0,-9), speed2, zone1, tool1\WObj:=wobj2;
!MoveJ pTarget,speed1,fine,tool1\WObj:=wobj2;
ENDIF
! SocketSend socket_client\Str:=strReceive+resultStr_OK;
MoveL Home_Wobj1, speed1, z100, tool1\WObj:=wobj1;
SocketSend socket_client\Str:=strReceive+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);
shelfType:= StrPart(revice_Str2_pName,1,1);
!capture target
GetDataVal revice_Str2_pName,ptemp;
!Stop;
!Stop;
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>1000 THEN
revice_speed:=1000;
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 rBackHome()
PNowPos:=CRobT(\Tool:=tool1\Wobj:=wobj0);
IF PNowPos.trans.y<=-620 and PNowPos.trans.z>820 THEN
IF PNowPos.trans.z< pbackhomepos.trans.z then
PNowPos.trans.Z:=pbackhomepos.trans.Z;
endif
MOVEL PNowPos,V100,Z10,TOOL1;
MOVEj jbackhomepos,v100,fine, tool1;
ELSEIF PNowPos.trans.y<=-620 and PNowPos.trans.z<=820 AND PNowPos.trans.X<=300 THEN
PNowPos.trans.x:=300;
MOVEL PNowPos,V100,Z10,TOOL1;
PNowPos.trans.z:=900;
MOVEL PNowPos,V100,Z10,TOOL1;
MOVEj jbackhomepos,v100,fine, tool1;
ELSEIF PNowPos.trans.y<=-620 and PNowPos.trans.z<=820 AND PNowPos.trans.X>300 THEN
IF PNowPos.trans.z< pbackhomepos.trans.z then
PNowPos.trans.Z:=pbackhomepos.trans.Z;
endif
MOVEL PNowPos, v100, Z10, TOOL1;
MOVEj jbackhomepos,v100,fine, tool1;
!TPWrite "chang to manual module";
!stop;
ELSeIF (-620<PNowPos.trans.y and PNowPos.trans.y<=550) AND PNowPos.trans.X<=715 then
IF PNowPos.trans.z< pbackhomepos.trans.z then
PNowPos.trans.Z:=pbackhomepos.trans.Z;
ENDIF
MOVEL PNowPos,V100,Z10,TOOL1;
MOVEj jbackhomepos,v100,fine, tool1;
ELSeIF (-620<PNowPos.trans.y and PNowPos.trans.y<=550) AND PNowPos.trans.X>715 then
PNowPos.trans.X:=715;
MOVEL PNowPos,V100,Z10,TOOL1;
IF PNowPos.trans.z< pbackhomepos.trans.z then
PNowPos.trans.Z:=pbackhomepos.trans.Z;
ENDIF
MOVEL PNowPos,V100,Z10,TOOL1;
MoveL PNowPos20, v100, z10, tool1;
MOVEj jbackhomepos,v100,fine, tool1;
ELSeIF PNowPos.trans.y> 550 then
TPWrite"robot is to far";
TPWrite"move the robot to safty posion by manule module";
stop;
ENDIF
ENDPROC
PROC Routine1()
MoveL [[-1.74,-351.15,465.56],[0.300707,-0.919334,0.248145,0.0531493],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v100, z50, tool1\WObj:=wobj1;
MoveL [[-4.66,-300.07,453.63],[0.282192,-0.925597,0.248225,0.0449703],[0,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v100, z10, tool1\WObj:=wobj1;
MoveL [[79.15,-300.86,453.64],[0.282202,-0.925593,0.248224,0.0449831],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v100, z10, tool1\WObj:=wobj1;
MoveL [[78.71,-347.14,453.64],[0.282192,-0.92559,0.248244,0.0450091],[-1,-1,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]], v100, z10, tool1\WObj:=wobj1;
ENDPROC
PROC Routine2()
MoveAbsJ [[0,0,0,0,90,-45],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]]\NoEOffs,v1000,fine,tool0;
ENDPROC
ENDMODULE