Commit | Line | Data |
---|---|---|
ce9d2bda JM |
1 | #include "DeltaCalibrationStrategy.h" |
2 | #include "Kernel.h" | |
3 | #include "Config.h" | |
4 | #include "Robot.h" | |
5 | #include "StreamOutputPool.h" | |
6 | #include "Gcode.h" | |
7 | #include "checksumm.h" | |
8 | #include "ConfigValue.h" | |
9 | #include "PublicDataRequest.h" | |
10 | #include "EndstopsPublicAccess.h" | |
11 | #include "PublicData.h" | |
12 | #include "Conveyor.h" | |
13 | #include "ZProbe.h" | |
14 | #include "BaseSolution.h" | |
afc5b690 | 15 | #include "StepperMotor.h" |
ce9d2bda JM |
16 | |
17 | #include <tuple> | |
18 | #include <algorithm> | |
19 | ||
119114b4 JM |
20 | #define radius_checksum CHECKSUM("radius") |
21 | #define initial_height_checksum CHECKSUM("initial_height") | |
22 | ||
57e927fa JM |
23 | // deprecated |
24 | #define probe_radius_checksum CHECKSUM("probe_radius") | |
ce9d2bda JM |
25 | |
26 | bool DeltaCalibrationStrategy::handleConfig() | |
27 | { | |
28 | // default is probably wrong | |
57e927fa JM |
29 | float r= THEKERNEL->config->value(leveling_strategy_checksum, delta_calibration_strategy_checksum, radius_checksum)->by_default(-1)->as_number(); |
30 | if(r == -1) { | |
31 | // deprecated config syntax] | |
32 | r = THEKERNEL->config->value(zprobe_checksum, probe_radius_checksum)->by_default(100.0F)->as_number(); | |
33 | } | |
34 | this->probe_radius= r; | |
119114b4 JM |
35 | |
36 | // the initial height above the bed we stop the intial move down after home to find the bed | |
37 | // this should be a height that is enough that the probe will not hit the bed and is an offset from max_z (can be set to 0 if max_z takes into account the probe offset) | |
38 | this->initial_height= THEKERNEL->config->value(leveling_strategy_checksum, delta_calibration_strategy_checksum, initial_height_checksum)->by_default(10)->as_number(); | |
ce9d2bda JM |
39 | return true; |
40 | } | |
41 | ||
42 | bool DeltaCalibrationStrategy::handleGcode(Gcode *gcode) | |
43 | { | |
ce9d2bda JM |
44 | if( gcode->has_g) { |
45 | // G code processing | |
46 | if( gcode->g == 32 ) { // auto calibration for delta, Z bed mapping for cartesian | |
47 | // first wait for an empty queue i.e. no moves left | |
48 | THEKERNEL->conveyor->wait_for_empty_queue(); | |
49 | ||
e9f69a35 JM |
50 | // turn off any compensation transform as it will be invalidated anyway by this |
51 | THEKERNEL->robot->compensationTransform= nullptr; | |
52 | ||
f6efadb0 | 53 | if(!gcode->has_letter('R')) { |
ce9d2bda | 54 | if(!calibrate_delta_endstops(gcode)) { |
1b69657f | 55 | gcode->stream->printf("Calibration failed to complete\n"); |
ce9d2bda JM |
56 | return true; |
57 | } | |
58 | } | |
f6efadb0 | 59 | if(!gcode->has_letter('E')) { |
ce9d2bda | 60 | if(!calibrate_delta_radius(gcode)) { |
1b69657f | 61 | gcode->stream->printf("Calibration failed to complete\n"); |
ce9d2bda JM |
62 | return true; |
63 | } | |
64 | } | |
65 | gcode->stream->printf("Calibration complete, save settings with M500\n"); | |
66 | return true; | |
afc5b690 JM |
67 | |
68 | }else if (gcode->g == 29) { | |
69 | // probe the 7 points | |
70 | if(!probe_delta_points(gcode)) { | |
1b69657f | 71 | gcode->stream->printf("Calibration failed to complete\n"); |
afc5b690 JM |
72 | } |
73 | return true; | |
ce9d2bda JM |
74 | } |
75 | ||
76 | } else if(gcode->has_m) { | |
57e927fa | 77 | // handle mcodes |
ce9d2bda JM |
78 | } |
79 | ||
80 | return false; | |
81 | } | |
82 | ||
ce9d2bda JM |
83 | // calculate the X and Y positions for the three towers given the radius from the center |
84 | static std::tuple<float, float, float, float, float, float> getCoordinates(float radius) | |
85 | { | |
86 | float px = 0.866F * radius; // ~sin(60) | |
87 | float py = 0.5F * radius; // cos(60) | |
88 | float t1x = -px, t1y = -py; // X Tower | |
89 | float t2x = px, t2y = -py; // Y Tower | |
90 | float t3x = 0.0F, t3y = radius; // Z Tower | |
91 | return std::make_tuple(t1x, t1y, t2x, t2y, t3x, t3y); | |
92 | } | |
93 | ||
afc5b690 JM |
94 | |
95 | // Probes the 7 points on a delta can be used for off board calibration | |
96 | bool DeltaCalibrationStrategy::probe_delta_points(Gcode *gcode) | |
97 | { | |
1a207d9c JM |
98 | float bedht= findBed(); |
99 | if(isnan(bedht)) return false; | |
100 | ||
101 | gcode->stream->printf("initial Bed ht is %f mm\n", bedht); | |
102 | ||
d2fb5867 JM |
103 | // check probe ht |
104 | int s; | |
105 | if(!zprobe->doProbeAt(s, 0, 0)) return false; | |
106 | float dz = zprobe->getProbeHeight() - zprobe->zsteps_to_mm(s); | |
107 | gcode->stream->printf("center probe: %1.4f\n", dz); | |
1a207d9c | 108 | |
afc5b690 JM |
109 | // get probe points |
110 | float t1x, t1y, t2x, t2y, t3x, t3y; | |
111 | std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius); | |
112 | ||
113 | // gather probe points | |
114 | float pp[][2] {{t1x, t1y}, {t2x, t2y}, {t3x, t3y}, {0, 0}, {-t1x, -t1y}, {-t2x, -t2y}, {-t3x, -t3y}}; | |
115 | ||
119114b4 JM |
116 | float max_delta= 0; |
117 | float last_z= NAN; | |
aaf0c0ee JM |
118 | float start_z; |
119 | std::tie(std::ignore, std::ignore, start_z)= THEKERNEL->robot->get_axis_position(); | |
120 | ||
afc5b690 JM |
121 | for(auto& i : pp) { |
122 | int s; | |
123 | if(!zprobe->doProbeAt(s, i[0], i[1])) return false; | |
124 | float z = zprobe->zsteps_to_mm(s); | |
aaf0c0ee JM |
125 | if(gcode->subcode == 0) { |
126 | gcode->stream->printf("X:%1.4f Y:%1.4f Z:%1.4f (%d) A:%1.4f B:%1.4f C:%1.4f\n", | |
127 | i[0], i[1], z, s, | |
128 | THEKERNEL->robot->actuators[0]->get_current_position()+z, | |
129 | THEKERNEL->robot->actuators[1]->get_current_position()+z, | |
130 | THEKERNEL->robot->actuators[2]->get_current_position()+z); | |
131 | ||
132 | }else if(gcode->subcode == 1) { | |
133 | // format that can be pasted here http://escher3d.com/pages/wizards/wizarddelta.php | |
134 | gcode->stream->printf("X%1.4f Y%1.4f Z%1.4f\n", i[0], i[1], start_z - z); | |
135 | } | |
119114b4 JM |
136 | |
137 | if(isnan(last_z)) { | |
138 | last_z= z; | |
139 | }else{ | |
140 | max_delta= std::max(max_delta, fabsf(z-last_z)); | |
141 | } | |
afc5b690 JM |
142 | } |
143 | ||
119114b4 JM |
144 | gcode->stream->printf("max delta: %f\n", max_delta); |
145 | ||
afc5b690 JM |
146 | return true; |
147 | } | |
148 | ||
119114b4 JM |
149 | float DeltaCalibrationStrategy::findBed() |
150 | { | |
151 | // home | |
152 | zprobe->home(); | |
153 | ||
154 | // move to an initial position fast so as to not take all day, we move down max_z - initial_height, which is set in config, default 10mm | |
155 | float deltaz= zprobe->getMaxZ() - initial_height; | |
156 | zprobe->coordinated_move(NAN, NAN, -deltaz, zprobe->getFastFeedrate(), true); | |
157 | ||
158 | // find bed, run at slow rate so as to not hit bed hard | |
159 | int s; | |
160 | if(!zprobe->run_probe(s, false)) return NAN; | |
d2fb5867 JM |
161 | zprobe->return_probe(s); |
162 | ||
163 | // leave the probe zprobe->getProbeHeight() above bed | |
164 | float dz= zprobe->getProbeHeight() - zprobe->zsteps_to_mm(s); | |
165 | if(dz >= 0) { | |
166 | // probe was not started above bed | |
167 | return NAN; | |
168 | } | |
169 | zprobe->coordinated_move(NAN, NAN, dz, zprobe->getFastFeedrate(), true); // relative move | |
119114b4 JM |
170 | |
171 | return zprobe->zsteps_to_mm(s) + deltaz - zprobe->getProbeHeight(); // distance to move from home to 5mm above bed | |
172 | } | |
173 | ||
ce9d2bda JM |
174 | /* Run a calibration routine for a delta |
175 | 1. Home | |
176 | 2. probe for z bed | |
177 | 3. probe initial tower positions | |
178 | 4. set initial trims such that trims will be minimal negative values | |
179 | 5. home, probe three towers again | |
180 | 6. calculate trim offset and apply to all trims | |
181 | 7. repeat 5, 6 until it converges on a solution | |
182 | */ | |
183 | ||
184 | bool DeltaCalibrationStrategy::calibrate_delta_endstops(Gcode *gcode) | |
185 | { | |
f6efadb0 JM |
186 | float target = 0.03F; |
187 | if(gcode->has_letter('I')) target = gcode->get_value('I'); // override default target | |
188 | if(gcode->has_letter('J')) this->probe_radius = gcode->get_value('J'); // override default probe radius | |
ce9d2bda | 189 | |
f6efadb0 JM |
190 | bool keep = false; |
191 | if(gcode->has_letter('K')) keep = true; // keep current settings | |
ce9d2bda JM |
192 | |
193 | gcode->stream->printf("Calibrating Endstops: target %fmm, radius %fmm\n", target, this->probe_radius); | |
194 | ||
195 | // get probe points | |
196 | float t1x, t1y, t2x, t2y, t3x, t3y; | |
197 | std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius); | |
198 | ||
f6efadb0 | 199 | float trimx = 0.0F, trimy = 0.0F, trimz = 0.0F; |
ce9d2bda JM |
200 | if(!keep) { |
201 | // zero trim values | |
202 | if(!set_trim(0, 0, 0, gcode->stream)) return false; | |
203 | ||
f6efadb0 | 204 | } else { |
ce9d2bda JM |
205 | // get current trim, and continue from that |
206 | if (get_trim(trimx, trimy, trimz)) { | |
207 | gcode->stream->printf("Current Trim X: %f, Y: %f, Z: %f\r\n", trimx, trimy, trimz); | |
208 | ||
209 | } else { | |
210 | gcode->stream->printf("Could not get current trim, are endstops enabled?\n"); | |
211 | return false; | |
212 | } | |
213 | } | |
214 | ||
119114b4 JM |
215 | // find the bed, as we potentially have a temporary z probe we don't know how low under the nozzle it is |
216 | // so we need to find the initial place that the probe triggers when it hits the bed | |
217 | float bedht= findBed(); | |
218 | if(isnan(bedht)) return false; | |
219 | gcode->stream->printf("initial Bed ht is %f mm\n", bedht); | |
ce9d2bda | 220 | |
d2fb5867 JM |
221 | // check probe ht |
222 | int s; | |
223 | if(!zprobe->doProbeAt(s, 0, 0)) return false; | |
224 | float dz = zprobe->getProbeHeight() - zprobe->zsteps_to_mm(s); | |
225 | gcode->stream->printf("center probe: %1.4f\n", dz); | |
226 | if(fabsf(dz) > target) { | |
227 | gcode->stream->printf("Probe was not repeatable to %f mm, (%f)\n", target, dz); | |
228 | return false; | |
229 | } | |
ce9d2bda JM |
230 | |
231 | // get initial probes | |
232 | // probe the base of the X tower | |
97832d6d | 233 | if(!zprobe->doProbeAt(s, t1x, t1y)) return false; |
57e927fa | 234 | float t1z = zprobe->zsteps_to_mm(s); |
ce9d2bda JM |
235 | gcode->stream->printf("T1-0 Z:%1.4f C:%d\n", t1z, s); |
236 | ||
237 | // probe the base of the Y tower | |
97832d6d | 238 | if(!zprobe->doProbeAt(s, t2x, t2y)) return false; |
57e927fa | 239 | float t2z = zprobe->zsteps_to_mm(s); |
ce9d2bda JM |
240 | gcode->stream->printf("T2-0 Z:%1.4f C:%d\n", t2z, s); |
241 | ||
242 | // probe the base of the Z tower | |
97832d6d | 243 | if(!zprobe->doProbeAt(s, t3x, t3y)) return false; |
57e927fa | 244 | float t3z = zprobe->zsteps_to_mm(s); |
ce9d2bda JM |
245 | gcode->stream->printf("T3-0 Z:%1.4f C:%d\n", t3z, s); |
246 | ||
f6efadb0 | 247 | float trimscale = 1.2522F; // empirically determined |
ce9d2bda | 248 | |
f6efadb0 JM |
249 | auto mm = std::minmax({t1z, t2z, t3z}); |
250 | if((mm.second - mm.first) <= target) { | |
251 | gcode->stream->printf("trim already set within required parameters: delta %f\n", mm.second - mm.first); | |
ce9d2bda JM |
252 | return true; |
253 | } | |
254 | ||
255 | // set trims to worst case so we always have a negative trim | |
f6efadb0 JM |
256 | trimx += (mm.first - t1z) * trimscale; |
257 | trimy += (mm.first - t2z) * trimscale; | |
258 | trimz += (mm.first - t3z) * trimscale; | |
ce9d2bda JM |
259 | |
260 | for (int i = 1; i <= 10; ++i) { | |
261 | // set trim | |
262 | if(!set_trim(trimx, trimy, trimz, gcode->stream)) return false; | |
263 | ||
264 | // home and move probe to start position just above the bed | |
265 | zprobe->home(); | |
266 | zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed | |
267 | ||
268 | // probe the base of the X tower | |
97832d6d | 269 | if(!zprobe->doProbeAt(s, t1x, t1y)) return false; |
57e927fa | 270 | t1z = zprobe->zsteps_to_mm(s); |
ce9d2bda JM |
271 | gcode->stream->printf("T1-%d Z:%1.4f C:%d\n", i, t1z, s); |
272 | ||
273 | // probe the base of the Y tower | |
97832d6d | 274 | if(!zprobe->doProbeAt(s, t2x, t2y)) return false; |
57e927fa | 275 | t2z = zprobe->zsteps_to_mm(s); |
ce9d2bda JM |
276 | gcode->stream->printf("T2-%d Z:%1.4f C:%d\n", i, t2z, s); |
277 | ||
278 | // probe the base of the Z tower | |
97832d6d | 279 | if(!zprobe->doProbeAt(s, t3x, t3y)) return false; |
57e927fa | 280 | t3z = zprobe->zsteps_to_mm(s); |
ce9d2bda JM |
281 | gcode->stream->printf("T3-%d Z:%1.4f C:%d\n", i, t3z, s); |
282 | ||
f6efadb0 JM |
283 | mm = std::minmax({t1z, t2z, t3z}); |
284 | if((mm.second - mm.first) <= target) { | |
285 | gcode->stream->printf("trim set to within required parameters: delta %f\n", mm.second - mm.first); | |
ce9d2bda JM |
286 | break; |
287 | } | |
288 | ||
289 | // set new trim values based on min difference | |
f6efadb0 JM |
290 | trimx += (mm.first - t1z) * trimscale; |
291 | trimy += (mm.first - t2z) * trimscale; | |
292 | trimz += (mm.first - t3z) * trimscale; | |
ce9d2bda JM |
293 | |
294 | // flush the output | |
295 | THEKERNEL->call_event(ON_IDLE); | |
296 | } | |
297 | ||
f6efadb0 JM |
298 | if((mm.second - mm.first) > target) { |
299 | gcode->stream->printf("WARNING: trim did not resolve to within required parameters: delta %f\n", mm.second - mm.first); | |
ce9d2bda JM |
300 | } |
301 | ||
302 | return true; | |
303 | } | |
304 | ||
305 | /* | |
306 | probe edges to get outer positions, then probe center | |
307 | modify the delta radius until center and X converge | |
308 | */ | |
309 | ||
310 | bool DeltaCalibrationStrategy::calibrate_delta_radius(Gcode *gcode) | |
311 | { | |
f6efadb0 JM |
312 | float target = 0.03F; |
313 | if(gcode->has_letter('I')) target = gcode->get_value('I'); // override default target | |
314 | if(gcode->has_letter('J')) this->probe_radius = gcode->get_value('J'); // override default probe radius | |
ce9d2bda JM |
315 | |
316 | gcode->stream->printf("Calibrating delta radius: target %f, radius %f\n", target, this->probe_radius); | |
317 | ||
318 | // get probe points | |
319 | float t1x, t1y, t2x, t2y, t3x, t3y; | |
320 | std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius); | |
321 | ||
119114b4 JM |
322 | // find the bed, as we potentially have a temporary z probe we don't know how low under the nozzle it is |
323 | // so we need to find thr initial place that the probe triggers when it hits the bed | |
324 | float bedht= findBed(); | |
325 | if(isnan(bedht)) return false; | |
326 | gcode->stream->printf("initial Bed ht is %f mm\n", bedht); | |
ce9d2bda | 327 | |
d2fb5867 JM |
328 | // check probe ht |
329 | int s; | |
330 | if(!zprobe->doProbeAt(s, 0, 0)) return false; | |
331 | float dz = zprobe->getProbeHeight() - zprobe->zsteps_to_mm(s); | |
332 | gcode->stream->printf("center probe: %1.4f\n", dz); | |
333 | if(fabsf(dz) > target) { | |
334 | gcode->stream->printf("Probe was not repeatable to %f mm, (%f)\n", target, dz); | |
335 | return false; | |
336 | } | |
ce9d2bda JM |
337 | |
338 | // probe center to get reference point at this Z height | |
339 | int dc; | |
97832d6d | 340 | if(!zprobe->doProbeAt(dc, 0, 0)) return false; |
57e927fa JM |
341 | gcode->stream->printf("CT Z:%1.3f C:%d\n", zprobe->zsteps_to_mm(dc), dc); |
342 | float cmm = zprobe->zsteps_to_mm(dc); | |
ce9d2bda JM |
343 | |
344 | // get current delta radius | |
f6efadb0 | 345 | float delta_radius = 0.0F; |
ce9d2bda JM |
346 | BaseSolution::arm_options_t options; |
347 | if(THEKERNEL->robot->arm_solution->get_optional(options)) { | |
f6efadb0 | 348 | delta_radius = options['R']; |
ce9d2bda JM |
349 | } |
350 | if(delta_radius == 0.0F) { | |
351 | gcode->stream->printf("This appears to not be a delta arm solution\n"); | |
352 | return false; | |
353 | } | |
354 | options.clear(); | |
355 | ||
489cf67d | 356 | bool good= false; |
f6efadb0 | 357 | float drinc = 2.5F; // approx |
ce9d2bda JM |
358 | for (int i = 1; i <= 10; ++i) { |
359 | // probe t1, t2, t3 and get average, but use coordinated moves, probing center won't change | |
360 | int dx, dy, dz; | |
97832d6d | 361 | if(!zprobe->doProbeAt(dx, t1x, t1y)) return false; |
57e927fa | 362 | gcode->stream->printf("T1-%d Z:%1.3f C:%d\n", i, zprobe->zsteps_to_mm(dx), dx); |
97832d6d | 363 | if(!zprobe->doProbeAt(dy, t2x, t2y)) return false; |
57e927fa | 364 | gcode->stream->printf("T2-%d Z:%1.3f C:%d\n", i, zprobe->zsteps_to_mm(dy), dy); |
97832d6d | 365 | if(!zprobe->doProbeAt(dz, t3x, t3y)) return false; |
57e927fa | 366 | gcode->stream->printf("T3-%d Z:%1.3f C:%d\n", i, zprobe->zsteps_to_mm(dz), dz); |
ce9d2bda JM |
367 | |
368 | // now look at the difference and reduce it by adjusting delta radius | |
57e927fa | 369 | float m = zprobe->zsteps_to_mm((dx + dy + dz) / 3.0F); |
f6efadb0 | 370 | float d = cmm - m; |
ce9d2bda JM |
371 | gcode->stream->printf("C-%d Z-ave:%1.4f delta: %1.3f\n", i, m, d); |
372 | ||
8b261cdc | 373 | if(fabsf(d) <= target){ |
489cf67d JM |
374 | good= true; |
375 | break; // resolution of success | |
376 | } | |
ce9d2bda JM |
377 | |
378 | // increase delta radius to adjust for low center | |
379 | // decrease delta radius to adjust for high center | |
f6efadb0 | 380 | delta_radius += (d * drinc); |
ce9d2bda JM |
381 | |
382 | // set the new delta radius | |
f6efadb0 | 383 | options['R'] = delta_radius; |
ce9d2bda JM |
384 | THEKERNEL->robot->arm_solution->set_optional(options); |
385 | gcode->stream->printf("Setting delta radius to: %1.4f\n", delta_radius); | |
386 | ||
387 | zprobe->home(); | |
388 | zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // needs to be a relative coordinated move | |
389 | ||
390 | // flush the output | |
391 | THEKERNEL->call_event(ON_IDLE); | |
392 | } | |
489cf67d JM |
393 | |
394 | if(!good) { | |
395 | gcode->stream->printf("WARNING: delta radius did not resolve to within required parameters: %f\n", target); | |
396 | } | |
397 | ||
ce9d2bda JM |
398 | return true; |
399 | } | |
400 | ||
401 | bool DeltaCalibrationStrategy::set_trim(float x, float y, float z, StreamOutput *stream) | |
402 | { | |
f6efadb0 JM |
403 | float t[3] {x, y, z}; |
404 | bool ok = PublicData::set_value( endstops_checksum, trim_checksum, t); | |
ce9d2bda JM |
405 | |
406 | if (ok) { | |
407 | stream->printf("set trim to X:%f Y:%f Z:%f\n", x, y, z); | |
408 | } else { | |
409 | stream->printf("unable to set trim, is endstops enabled?\n"); | |
410 | } | |
411 | ||
412 | return ok; | |
413 | } | |
414 | ||
f6efadb0 | 415 | bool DeltaCalibrationStrategy::get_trim(float &x, float &y, float &z) |
ce9d2bda JM |
416 | { |
417 | void *returned_data; | |
418 | bool ok = PublicData::get_value( endstops_checksum, trim_checksum, &returned_data ); | |
419 | ||
420 | if (ok) { | |
421 | float *trim = static_cast<float *>(returned_data); | |
f6efadb0 JM |
422 | x = trim[0]; |
423 | y = trim[1]; | |
424 | z = trim[2]; | |
ce9d2bda JM |
425 | return true; |
426 | } | |
427 | return false; | |
428 | } |