-
Notifications
You must be signed in to change notification settings - Fork 6
/
RAPIDcode5_23.txt
244 lines (217 loc) · 7.81 KB
/
RAPIDcode5_23.txt
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
MODULE ArcProgram
VAR socketdev socket1;
VAR string CMDString;
VAR string PosString;
VAR num retry_no:=0;
VAR string flagString;
VAR string toolString;
VAR string tansString;
VAR string rotString;
VAR string robconfString;
VAR string extaxString;
VAR string moveArcString;
VAR string SpeedString;
VAR robtarget target_POS;
VAR speeddata spd;
VAR zonedata path0 := [ false, 5, 10, 10, 2, 15, 2 ];
CONST jointtarget p50:=[[0,0,0,0,90,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
VAR robtarget Scan_target_POS{100};
VAR num a;
VAR pos xyzPos;
VAR bool convertOk1;
VAR bool convertOk2;
VAR bool convertOk3;
VAR bool convertOk4;
VAR bool convertOk5;
VAR intnum timeint;
VAR clock myclock;
VAR num time1;
VAR robtarget p1;
VAR num anglex;
VAR num angley;
VAR num anglez;
TASK PERS tooldata tool1:=[TRUE,[[-66.081,1.70513,429.57],[1,0,0,0]],[3,[250,0,50],[1,0,0,0],0,0,0]];
PROC main()
ClkReset myclock;
SocketCreate socket1;
SocketConnect socket1,"192.168.125.16",4999,\Time:=2;
WHILE TRUE DO
SocketReceive socket1,\Str:=CMDString;
cmdExplain(CMDString);
ENDWHILE
ERROR
IF ERRNO=ERR_SOCK_TIMEOUT THEN
IF retry_no<200 THEN
ResetRetryCount;
WaitTime 1;
retry_no:=retry_no+1;
RETRY;
ELSE
RAISE ;
ENDIF
ENDIF
ENDPROC
PROC cmdExplain(string cmd)
TEST cmd
CASE "CurPos":
CurPos;
CASE "PointPos":
PointPos;
CASE "EndPos":
EndPos;
CASE "RecPos":
RecPos;
CASE "Scan":
ClkStart myclock;
Scan;
CASE "ToZero":
ToZero;
CASE "Calibration":
Calibration;
DEFAULT:
Stop;
ENDTEST
ENDPROC
PROC CurPos()
p1:=CRobT(\Tool:=tool0\WObj:=wobj0);
PosString:=ValToStr(p1.trans);
PosString:=PosString+ValToStr(p1.rot);
SocketSend socket1\Str:=PosString;
ENDPROC
PROC PointPos()
p1:=CRobT(\Tool:=tool1\WObj:=wobj0);
PosString:=ValToStr(p1.trans);
SocketSend socket1\Str:=PosString;
ENDPROC
PROC EndPos()
p1:=CRobT(\Tool:=tool0\WObj:=wobj0);
anglex:=EulerZYX(\X,p1.rot);
angley:=EulerZYX(\Y,p1.rot);
anglez:=EulerZYX(\Z,p1.rot);
PosString:=ValToStr(p1.trans);
PosString:=PosString+"["+ValToStr(anglex)+","+ValToStr(angley)+","+ValToStr(anglez)+"]";
SocketSend socket1\Str:=PosString;
ENDPROC
PROC Scan()
a:=0;
SocketSend socket1\Str:="ready";
SocketReceive socket1\Str:=flagString,\Time:=5;
SocketSend socket1\Str:="flagString received";
WHILE flagString="Start" DO
SocketReceive socket1\Str:=tansString;
SocketSend socket1\Str:="tansString received";
SocketReceive socket1\Str:=rotString;
SocketSend socket1\Str:="rotString received";
SocketReceive socket1\Str:=robconfString;
SocketSend socket1\Str:="robconfString received";
SocketReceive socket1\Str:=extaxString;
SocketSend socket1\Str:="extaxString received";
convertOk1:=StrToVal(tansString,target_POS.trans);
convertOk2:=StrToVal(rotString,target_POS.rot);
convertOk3:=StrToVal(robconfString,target_POS.robconf);
convertOk4:=StrToVal(extaxString,target_POS.extax);
IF convertOk1 AND convertOk2 AND convertOk3 AND convertOk4=TRUE then
a:=a+1;
Scan_target_POS{a}:=target_POS;
tansString:="";
rotString:="";
robconfString:="";
extaxString:="";
ELSE
Break;
ENDIF
SocketSend socket1\Str:="ready";
SocketReceive socket1\Str:=flagString,\Time:=5;
SocketSend socket1\Str:="flagString received";
ENDWHILE
ConfJ\On;
CONNECT timeint WITH SendTarget;
ITimer 0.1,timeint;
FOR i FROM 1 TO a DO
MoveJ Scan_target_POS{i},v10,path0,tool0;
ConfJ\Off;
ENDFOR
ClkStop myclock;
IDelete timeint;
ENDPROC
PROC RecPos()
ConfJ\On;
SocketSend socket1\Str:="wait for tool flag";
SocketReceive socket1\Str:=toolString;
SocketSend socket1\Str:="ready";
SocketReceive socket1\Str:=flagString;
SocketSend socket1\Str:="flagString received";
WHILE flagString="Start" DO
SocketReceive socket1\Str:=tansString;
SocketSend socket1\Str:="tansString received";
SocketReceive socket1\Str:=rotString;
SocketSend socket1\Str:="rotString received";
SocketReceive socket1\Str:=robconfString;
SocketSend socket1\Str:="robconfString received";
SocketReceive socket1\Str:=extaxString;
SocketSend socket1\Str:="extaxString received";
SocketReceive socket1\Str:=moveArcString;
SocketSend socket1\Str:="moveArcString received";
SocketReceive socket1\Str:=SpeedString;
SocketSend socket1\Str:="SpeedString received";
convertOk1:=StrToVal(tansString,target_POS.trans);
convertOk2:=StrToVal(rotString,target_POS.rot);
convertOk3:=StrToVal(robconfString,target_POS.robconf);
convertOk4:=StrToVal(extaxString,target_POS.extax);
convertOk5:=StrToVal(SpeedString,spd);!AND convertOk5=TRUE
IF (convertOk1 AND convertOk2 AND convertOk3 AND convertOk4)=TRUE then
IF toolString="tool0" THEN
IF moveArcString="MoveL" THEN
MoveL target_POS,v50,path0,tool0;
ELSEIF moveArcString="ArcL" THEN
MoveL target_POS,v50,path0,tool0;
ELSEIF moveArcString="ArcLStart" THEN
MoveL target_POS,v50,path0,tool0;
ENDIF
ELSE
IF moveArcString="MoveL" THEN
MoveL target_POS,v50,path0,tool1;
ELSEIF moveArcString="ArcL" THEN
MoveL target_POS,v50,path0,tool1;
ENDIF
ENDIF
tansString:="";
rotString:="";
robconfString:="";
extaxString:="";
ELSE
Break;
ENDIF
ConfJ\Off;
SocketSend socket1\Str:="ready";
SocketReceive socket1\Str:=flagString;
SocketSend socket1\Str:="flagString received";
ENDWHILE
ENDPROC
PROC ToZero()
MoveAbsJ p50,v50,z50,tool0;
ENDPROC
PROC Calibration()
!VAR num anglex;
!VAR num angley;
!VAR num anglez;
!VAR pose object;
!anglex := EulerZYX(\X, object.rot);
!angley := EulerZYX(\Y, object.rot);
!anglez := EulerZYX(\Z, object.rot);
ENDPROC
TRAP SendTarget
p1:=CRobT(\Tool:=tool0\WObj:=wobj0);
time1:=ClkRead(myclock,\HighRes);
PosString:=ValToStr(time1);
PosString:=PosString+ValToStr(p1.trans);
!PosString:=PosString+ValToStr(p1.rot);
!anglex := EulerZYX(\X, p1.rot);
!angley := EulerZYX(\Y, p1.rot);
!anglez := EulerZYX(\Z, p1.rot);
PosString:=PosString+"["+ValToStr(EulerZYX(\Z, p1.rot))+",";
PosString:=PosString+ValToStr(EulerZYX(\Y, p1.rot))+",";
PosString:=PosString+ValToStr(EulerZYX(\X, p1.rot))+"]";
SocketSend socket1\Str:=PosString;
ENDTRAP
ENDMODULE