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
|
component sim_axis_hardware "A component to simulate home and limit switches";
description
"""
This component creates simulated home and limit switches based on the current position.
.br
It currently can supply simulation for X, tandem X, Y, tandem Y, Z, U, V, and A axes.
""";
pin in float Xcurrent_pos "The current position on the axis - eg connect to joint.0.motor-pos-fb";
pin in float X2current_pos;
pin in float Ycurrent_pos;
pin in float Y2current_pos;
pin in float Zcurrent_pos;
pin in float Acurrent_pos;
pin in float Ucurrent_pos;
pin in float Vcurrent_pos;
pin in float Xhomesw_pos =1 "The position of the home switch";
pin in float X2homesw_pos =1;
pin in float Yhomesw_pos =1;
pin in float Y2homesw_pos =1;
pin in float Zhomesw_pos =1;
pin in float Ahomesw_pos =1;
pin in float Uhomesw_pos =1;
pin in float Vhomesw_pos =1;
pin in float Xmaxsw_upper "The upper range of the maximum limit switch, above this is false.";
pin in float X2maxsw_upper;
pin in float Ymaxsw_upper;
pin in float Y2maxsw_upper;
pin in float Zmaxsw_upper;
pin in float Amaxsw_upper;
pin in float Umaxsw_upper;
pin in float Vmaxsw_upper;
pin in float Xmaxsw_lower "The lower range of the maximum limit switch, below this is false.";
pin in float X2maxsw_lower;
pin in float Ymaxsw_lower;
pin in float Y2maxsw_lower;
pin in float Zmaxsw_lower;
pin in float Amaxsw_lower;
pin in float Umaxsw_lower;
pin in float Vmaxsw_lower;
pin in float Xminsw_upper "The upper range of the minimum limit switch, above this is false.";
pin in float X2minsw_upper;
pin in float Yminsw_upper;
pin in float Y2minsw_upper;
pin in float Zminsw_upper;
pin in float Aminsw_upper;
pin in float Uminsw_upper;
pin in float Vminsw_upper;
pin in float Xminsw_lower "The lower range of the minimum limit switch, below this is false.";
pin in float X2minsw_lower;
pin in float Yminsw_lower;
pin in float Y2minsw_lower;
pin in float Zminsw_lower;
pin in float Aminsw_lower;
pin in float Uminsw_lower;
pin in float Vminsw_lower;
pin in float Xhomesw_hyst =.025 "range that home switch will be true +- half this to the home position";
pin in float X2homesw_hyst =.025;
pin in float Yhomesw_hyst =.025;
pin in float Y2homesw_hyst =.025;
pin in float Zhomesw_hyst =.025;
pin in float Ahomesw_hyst =.025;
pin in float Uhomesw_hyst =.025;
pin in float Vhomesw_hyst =.025;
pin in bit Xhoming "True is homing in progress";
pin in bit X2homing;
pin in bit Yhoming;
pin in bit Y2homing;
pin in bit Zhoming;
pin in bit Ahoming;
pin in bit Uhoming;
pin in bit Vhoming;
pin out bit Xhomesw_out "Home switch for the X axis";
pin out bit X2homesw_out;
pin out bit Yhomesw_out;
pin out bit Y2homesw_out;
pin out bit Zhomesw_out;
pin out bit Ahomesw_out;
pin out bit Uhomesw_out;
pin out bit Vhomesw_out;
pin out bit homesw_all;
pin out bit Xmaxsw_out "Max limit switch";
pin out bit Xminsw_out "min limit switch";
pin out bit Xbothsw_out "True for both max and min limit switch";
pin out bit X2maxsw_out;
pin out bit X2minsw_out;
pin out bit X2bothsw_out;
pin out bit Ymaxsw_out;
pin out bit Yminsw_out;
pin out bit Ybothsw_out;
pin out bit Y2maxsw_out;
pin out bit Y2minsw_out;
pin out bit Y2bothsw_out;
pin out bit Zmaxsw_out;
pin out bit Zminsw_out;
pin out bit Zbothsw_out;
pin out bit Amaxsw_out;
pin out bit Aminsw_out;
pin out bit Abothsw_out;
pin out bit Umaxsw_out;
pin out bit Uminsw_out;
pin out bit Ubothsw_out;
pin out bit Vmaxsw_out;
pin out bit Vminsw_out;
pin out bit Vbothsw_out;
pin out bit limitsw_all;
pin out bit limitsw_homesw_all "True for all limits and all home.";
pin out bit Xmaxsw_homesw_out;
pin out bit Xminsw_homesw_out;
pin out bit Xbothsw_homesw_out;
pin out bit X2maxsw_homesw_out;
pin out bit X2minsw_homesw_out;
pin out bit X2bothsw_homesw_out;
pin out bit Ymaxsw_homesw_out;
pin out bit Yminsw_homesw_out;
pin out bit Ybothsw_homesw_out;
pin out bit Y2maxsw_homesw_out;
pin out bit Y2minsw_homesw_out;
pin out bit Y2bothsw_homesw_out;
pin out bit Zmaxsw_homesw_out;
pin out bit Zminsw_homesw_out;
pin out bit Zbothsw_homesw_out;
pin out bit Amaxsw_homesw_out;
pin out bit Aminsw_homesw_out;
pin out bit Abothsw_homesw_out;
pin out bit Umaxsw_homesw_out;
pin out bit Uminsw_homesw_out;
pin out bit Ubothsw_homesw_out;
pin out bit Vmaxsw_homesw_out;
pin out bit Vminsw_homesw_out;
pin out bit Vbothsw_homesw_out;
pin in float limit_offset =.01 "how much the limit switches are offset from inputted position. added to max, subtracted from min";
function update fp;
license "GPL";
author "Chris S Morley";
;;
/* private comparator function */
int comp(double in0, double in1, double hyst) {
double tmp;
double halfhyst;
tmp = in1 - in0;
halfhyst = 0.5 * hyst;
if(tmp < -halfhyst) {
return false;
} else if(tmp > halfhyst){
return false;
} else {
return true;
}
}
/* private window comparator function */
int wcomp (double in, double max_, double min_){
return !((in >= max_)|| (in <= min_));
}
FUNCTION(update) {
/*set home switches */
Xhomesw_out = comp(Xhomesw_pos,Xcurrent_pos,Xhomesw_hyst);
X2homesw_out = comp(X2homesw_pos,X2current_pos,X2homesw_hyst);
Yhomesw_out = comp(Yhomesw_pos,Ycurrent_pos,Yhomesw_hyst);
Y2homesw_out = comp(Y2homesw_pos,Y2current_pos,Y2homesw_hyst);
Zhomesw_out = comp(Zhomesw_pos,Zcurrent_pos,Zhomesw_hyst);
Ahomesw_out = comp(Ahomesw_pos,Acurrent_pos,Ahomesw_hyst);
Uhomesw_out = comp(Uhomesw_pos,Ucurrent_pos,Uhomesw_hyst);
Vhomesw_out = comp(Vhomesw_pos,Vcurrent_pos,Vhomesw_hyst);
int homing = Xhoming || X2homing || Yhoming || Y2homing ||
Zhoming || Ahoming || Uhoming || Vhoming;
homesw_all = (Xhomesw_out || X2homesw_out || Yhomesw_out || Y2homesw_out ||
Zhomesw_out || Ahomesw_out || Uhomesw_out || Vhomesw_out) && homing;
/* set limit switches */
Xmaxsw_out = wcomp(Xcurrent_pos,Xmaxsw_upper,Xmaxsw_lower+limit_offset);
Xminsw_out = wcomp(Xcurrent_pos,Xminsw_upper-limit_offset,Xminsw_lower);
Xbothsw_out = (Xmaxsw_out || Xminsw_out) && !homing;
X2maxsw_out = wcomp(X2current_pos,X2maxsw_upper,X2maxsw_lower+limit_offset);
X2minsw_out = wcomp(X2current_pos,X2minsw_upper-limit_offset,X2minsw_lower);
X2bothsw_out = (X2maxsw_out || X2minsw_out) && !homing;
Ymaxsw_out = wcomp(Ycurrent_pos,Ymaxsw_upper,Ymaxsw_lower+limit_offset);
Yminsw_out = wcomp(Ycurrent_pos,Yminsw_upper-limit_offset,Yminsw_lower);
Ybothsw_out = (Ymaxsw_out || Yminsw_out) && !homing;
Y2maxsw_out = wcomp(Y2current_pos,Y2maxsw_upper,Y2maxsw_lower+limit_offset);
Y2minsw_out = wcomp(Y2current_pos,Y2minsw_upper-limit_offset,Y2minsw_lower);
Y2bothsw_out = (Y2maxsw_out || Y2minsw_out) && !homing;
Zmaxsw_out = wcomp(Zcurrent_pos,Zmaxsw_upper,Zmaxsw_lower+limit_offset);
Zminsw_out = wcomp(Zcurrent_pos,Zminsw_upper-limit_offset,Zminsw_lower);
Zbothsw_out = (Zmaxsw_out || Zminsw_out) && !homing;
Amaxsw_out = wcomp(Acurrent_pos,Amaxsw_upper,Amaxsw_lower+limit_offset);
Aminsw_out = wcomp(Acurrent_pos,Aminsw_upper-limit_offset,Aminsw_lower);
Abothsw_out = (Amaxsw_out || Aminsw_out) && !homing;
Umaxsw_out = wcomp(Ucurrent_pos,Umaxsw_upper,Umaxsw_lower+limit_offset);
Uminsw_out = wcomp(Ucurrent_pos,Uminsw_upper-limit_offset,Uminsw_lower);
Ubothsw_out = (Umaxsw_out || Uminsw_out) && !homing;
Vmaxsw_out = wcomp(Vcurrent_pos,Vmaxsw_upper,Vmaxsw_lower+limit_offset);
Vminsw_out = wcomp(Vcurrent_pos,Vminsw_upper-limit_offset,Vminsw_lower);
Vbothsw_out = (Vmaxsw_out || Vminsw_out) && !homing;
limitsw_all = (Xbothsw_out || X2bothsw_out || Ybothsw_out || Y2bothsw_out
|| Zbothsw_out || Abothsw_out || Ubothsw_out || Vbothsw_out);
/* set limit and home switches */
limitsw_homesw_all = (homesw_all || limitsw_all);
Xmaxsw_homesw_out = (Xmaxsw_out || Xhomesw_out);
Xminsw_homesw_out = (Xminsw_out || Xhomesw_out);
Xbothsw_homesw_out = (Xbothsw_out || Xhomesw_out);
X2maxsw_homesw_out = (X2maxsw_out || X2homesw_out);
X2minsw_homesw_out = (X2minsw_out || X2homesw_out);
X2bothsw_homesw_out = (X2bothsw_out || X2homesw_out);
Ymaxsw_homesw_out = (Ymaxsw_out || Yhomesw_out);
Yminsw_homesw_out = (Yminsw_out || Yhomesw_out);
Ybothsw_homesw_out = (Ybothsw_out || Yhomesw_out);
Y2maxsw_homesw_out = (Y2maxsw_out || Y2homesw_out);
Y2minsw_homesw_out = (Y2minsw_out || Y2homesw_out);
Y2bothsw_homesw_out = (Y2bothsw_out || Y2homesw_out);
Zmaxsw_homesw_out = (Zmaxsw_out || Zhomesw_out);
Zminsw_homesw_out = (Zminsw_out || Zhomesw_out);
Zbothsw_homesw_out = (Zbothsw_out || Zhomesw_out);
Umaxsw_homesw_out = (Umaxsw_out || Uhomesw_out);
Uminsw_homesw_out = (Uminsw_out || Uhomesw_out);
Ubothsw_homesw_out = (Ubothsw_out || Uhomesw_out);
Vmaxsw_homesw_out = (Vmaxsw_out || Vhomesw_out);
Vminsw_homesw_out = (Vminsw_out || Vhomesw_out);
Vbothsw_homesw_out = (Vbothsw_out || Vhomesw_out);
}
|
