HCoop / clinton / Smoothieware.git / blob
? search:


65390df12daf0e5397e2f8ea4eb56e66ef1e1e37
[clinton/Smoothieware.git] / src / modules / tools / zprobe / ThreePointStrategy.cpp
1 /*
2 Author: Jim Morris (wolfmanjm@gmail.com)
3 License: GPL3 or better see <http://www.gnu.org/licenses/>
4
5 Summary
6 -------
7 Probes three user specified points on the bed and determines the plane of the bed relative to the probe.
8 as the head moves in X and Y it will adjust Z to keep the head tram with the bed.
9
10 Configuration
11 -------------
12 The strategy must be enabled in the cofnig as well as zprobe.
13
14 leveling-strategy.three-point-leveling.enable true
15
16 Three probe points must be defined, these are best if they are the three points of an equilateral triangle, as far apart as possible.
17 They can be defined in the config file as:-
18
19 leveling-strategy.three-point-leveling.point1 100.0,0.0 # the first probe point (x,y)
20 leveling-strategy.three-point-leveling.point2 200.0,200.0 # the second probe point (x,y)
21 leveling-strategy.three-point-leveling.point3 0.0,200.0 # the third probe point (x,y)
22
23 or they may be defined (and saved with M500) using M557 P0 X30 Y40.5 where P is 0,1,2
24
25 probe offsets from the nozzle or tool head can be defined with
26
27 leveling-strategy.three-point-leveling.probe_offsets 0,0,0 # probe offsetrs x,y,z
28
29 they may also be set with M565 X0 Y0 Z0
30
31 To force homing in X and Y before G32 does the probe the following can be set in config, this is the default
32
33 leveling-strategy.three-point-leveling.home_first true # disable by setting to false
34
35 The probe tolerance can be set using the config line
36
37 leveling-strategy.three-point-leveling.tolerance 0.03 # the probe tolerance in mm, default is 0.03mm
38
39
40 Usage
41 -----
42 G29 probes the three probe points and reports the Z at each point, if a plane is active it will be used to level the probe.
43 G32 probes the three probe points and defines the bed plane, this will remain in effect until reset or M561
44 G31 reports the status
45
46 M557 defines the probe points
47 M561 clears the plane and the bed leveling is disabled until G32 is run again
48 M565 defines the probe offsets from the nozzle or tool head
49
50 M500 saves the probe points and the probe offsets
51 M503 displays the current settings
52 */
53
54 #include "ThreePointStrategy.h"
55 #include "Kernel.h"
56 #include "Config.h"
57 #include "Robot.h"
58 #include "StreamOutputPool.h"
59 #include "Gcode.h"
60 #include "checksumm.h"
61 #include "ConfigValue.h"
62 #include "PublicDataRequest.h"
63 #include "PublicData.h"
64 #include "Conveyor.h"
65 #include "ZProbe.h"
66 #include "Plane3D.h"
67 #include "nuts_bolts.h"
68
69 #include <string>
70 #include <algorithm>
71 #include <cstdlib>
72 #include <cmath>
73
74 #define probe_point_1_checksum CHECKSUM("point1")
75 #define probe_point_2_checksum CHECKSUM("point2")
76 #define probe_point_3_checksum CHECKSUM("point3")
77 #define probe_offsets_checksum CHECKSUM("probe_offsets")
78 #define home_checksum CHECKSUM("home_first")
79 #define tolerance_checksum CHECKSUM("tolerance")
80 #define save_plane_checksum CHECKSUM("save_plane")
81
82 ThreePointStrategy::ThreePointStrategy(ZProbe *zprobe) : LevelingStrategy(zprobe)
83 {
84 for (int i = 0; i < 3; ++i) {
85 probe_points[i] = std::make_tuple(NAN, NAN);
86 }
87 plane = nullptr;
88 }
89
90 ThreePointStrategy::~ThreePointStrategy()
91 {
92 delete plane;
93 }
94
95 bool ThreePointStrategy::handleConfig()
96 {
97 // format is xxx,yyy for the probe points
98 std::string p1 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_1_checksum)->by_default("")->as_string();
99 std::string p2 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_2_checksum)->by_default("")->as_string();
100 std::string p3 = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_point_3_checksum)->by_default("")->as_string();
101 if(!p1.empty()) probe_points[0] = parseXY(p1.c_str());
102 if(!p2.empty()) probe_points[1] = parseXY(p2.c_str());
103 if(!p3.empty()) probe_points[2] = parseXY(p3.c_str());
104
105 // Probe offsets xxx,yyy,zzz
106 std::string po = THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, probe_offsets_checksum)->by_default("0,0,0")->as_string();
107 this->probe_offsets= parseXYZ(po.c_str());
108
109 this->home= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, home_checksum)->by_default(true)->as_bool();
110 this->tolerance= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, tolerance_checksum)->by_default(0.03F)->as_number();
111 this->save= THEKERNEL->config->value(leveling_strategy_checksum, three_point_leveling_strategy_checksum, save_plane_checksum)->by_default(false)->as_bool();
112 return true;
113 }
114
115 bool ThreePointStrategy::handleGcode(Gcode *gcode)
116 {
117 if(gcode->has_g) {
118 // G code processing
119 if(gcode->g == 29) { // test probe points for level
120 if(!test_probe_points(gcode)) {
121 gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
122 }
123 return true;
124
125 } else if( gcode->g == 31 ) { // report status
126 if(this->plane == nullptr) {
127 gcode->stream->printf("Bed leveling plane is not set\n");
128 }else{
129 gcode->stream->printf("Bed leveling plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
130 }
131 gcode->stream->printf("Probe is %s\n", zprobe->getProbeStatus() ? "Triggered" : "Not triggered");
132 return true;
133
134 } else if( gcode->g == 32 ) { // three point probe
135 // first wait for an empty queue i.e. no moves left
136 THEKERNEL->conveyor->wait_for_idle();
137
138 // clear any existing plane and compensation
139 delete this->plane;
140 this->plane= nullptr;
141 setAdjustFunction(false);
142
143 if(!doProbing(gcode->stream)) {
144 gcode->stream->printf("Probe failed to complete, probe not triggered or other error\n");
145 } else {
146 gcode->stream->printf("Probe completed, bed plane defined\n");
147 }
148 return true;
149 }
150
151 } else if(gcode->has_m) {
152 if(gcode->m == 557) { // M557 - set probe points eg M557 P0 X30 Y40.5 where P is 0,1,2
153 int idx = 0;
154 float x = NAN, y = NAN;
155 if(gcode->has_letter('P')) idx = gcode->get_value('P');
156 if(gcode->has_letter('X')) x = gcode->get_value('X');
157 if(gcode->has_letter('Y')) y = gcode->get_value('Y');
158 if(idx >= 0 && idx <= 2) {
159 probe_points[idx] = std::make_tuple(x, y);
160 }else{
161 gcode->stream->printf("only 3 probe points allowed P0-P2\n");
162 }
163 return true;
164
165 } else if(gcode->m == 561) { // M561: Set Identity Transform with no parameters, set the saved plane if A B C D are given
166 delete this->plane;
167 if(gcode->get_num_args() == 0) {
168 this->plane= nullptr;
169 // delete the compensationTransform in robot
170 setAdjustFunction(false);
171 gcode->stream->printf("saved plane cleared\n");
172 }else{
173 // smoothie specific way to restore a saved plane
174 uint32_t a,b,c,d;
175 a=b=c=d= 0;
176 if(gcode->has_letter('A')) a = gcode->get_uint('A');
177 if(gcode->has_letter('B')) b = gcode->get_uint('B');
178 if(gcode->has_letter('C')) c = gcode->get_uint('C');
179 if(gcode->has_letter('D')) d = gcode->get_uint('D');
180 this->plane= new Plane3D(a, b, c, d);
181 setAdjustFunction(true);
182 }
183 return true;
184
185 } else if(gcode->m == 565) { // M565: Set Z probe offsets
186 float x= 0, y= 0, z= 0;
187 if(gcode->has_letter('X')) x = gcode->get_value('X');
188 if(gcode->has_letter('Y')) y = gcode->get_value('Y');
189 if(gcode->has_letter('Z')) z = gcode->get_value('Z');
190 probe_offsets = std::make_tuple(x, y, z);
191 return true;
192
193 } else if(gcode->m == 500 || gcode->m == 503) { // M500 save, M503 display
194 float x, y, z;
195 gcode->stream->printf(";Probe points:\n");
196 for (int i = 0; i < 3; ++i) {
197 std::tie(x, y) = probe_points[i];
198 gcode->stream->printf("M557 P%d X%1.5f Y%1.5f\n", i, x, y);
199 }
200 gcode->stream->printf(";Probe offsets:\n");
201 std::tie(x, y, z) = probe_offsets;
202 gcode->stream->printf("M565 X%1.5f Y%1.5f Z%1.5f\n", x, y, z);
203
204 // encode plane and save if set and M500 and enabled
205 if(this->save && this->plane != nullptr) {
206 if(gcode->m == 500) {
207 uint32_t a, b, c, d;
208 this->plane->encode(a, b, c, d);
209 gcode->stream->printf(";Saved bed plane:\nM561 A%lu B%lu C%lu D%lu \n", a, b, c, d);
210 }else{
211 gcode->stream->printf(";The bed plane will be saved on M500\n");
212 }
213 }
214 return true;
215
216 }
217 #if 0
218 else if(gcode->m == 9999) {
219 // DEBUG run a test M9999 A B C X Y set Z to A B C and test for point at X Y
220 Vector3 v[3];
221 float x, y, z, a= 0, b= 0, c= 0;
222 if(gcode->has_letter('A')) a = gcode->get_value('A');
223 if(gcode->has_letter('B')) b = gcode->get_value('B');
224 if(gcode->has_letter('C')) c = gcode->get_value('C');
225 std::tie(x, y) = probe_points[0]; v[0].set(x, y, a);
226 std::tie(x, y) = probe_points[1]; v[1].set(x, y, b);
227 std::tie(x, y) = probe_points[2]; v[2].set(x, y, c);
228 delete this->plane;
229 this->plane = new Plane3D(v[0], v[1], v[2]);
230 gcode->stream->printf("plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
231 x= 0; y=0;
232 if(gcode->has_letter('X')) x = gcode->get_value('X');
233 if(gcode->has_letter('Y')) y = gcode->get_value('Y');
234 z= getZOffset(x, y);
235 gcode->stream->printf("z= %f\n", z);
236 // tell robot to adjust z on each move
237 setAdjustFunction(true);
238 return true;
239 }
240 #endif
241 }
242
243 return false;
244 }
245
246 void ThreePointStrategy::homeXY()
247 {
248 Gcode gc("G28 X0 Y0", &(StreamOutput::NullStream));
249 THEKERNEL->call_event(ON_GCODE_RECEIVED, &gc);
250 }
251
252 bool ThreePointStrategy::doProbing(StreamOutput *stream)
253 {
254 float x, y;
255 // check the probe points have been defined
256 for (int i = 0; i < 3; ++i) {
257 std::tie(x, y) = probe_points[i];
258 if(isnan(x) || isnan(y)) {
259 stream->printf("Probe point P%d has not been defined, use M557 P%d Xnnn Ynnn to define it\n", i, i);
260 return false;
261 }
262 }
263
264 // optionally home XY axis first, but allow for manual homing
265 if(this->home)
266 homeXY();
267
268 // move to the first probe point
269 std::tie(x, y) = probe_points[0];
270 // offset by the probe XY offset
271 x -= std::get<X_AXIS>(this->probe_offsets);
272 y -= std::get<Y_AXIS>(this->probe_offsets);
273 zprobe->coordinated_move(x, y, NAN, zprobe->getFastFeedrate());
274
275 // for now we use probe to find bed and not the Z min endstop
276 // the first probe point becomes Z == 0 effectively so if we home Z or manually set z after this, it needs to be at the first probe point
277
278 // TODO this needs to be configurable to use min z or probe
279
280 // find bed via probe
281 float mm;
282 if(!zprobe->run_probe(mm)) return false;
283
284 // TODO if using probe then we probably need to set Z to 0 at first probe point, but take into account probe offset from head
285 THEROBOT->reset_axis_position(std::get<Z_AXIS>(this->probe_offsets), Z_AXIS);
286
287 // move up to specified probe start position
288 zprobe->coordinated_move(NAN, NAN, zprobe->getProbeHeight(), zprobe->getSlowFeedrate()); // move to probe start position
289
290 // probe the three points
291 Vector3 v[3];
292 for (int i = 0; i < 3; ++i) {
293 std::tie(x, y) = probe_points[i];
294 // offset moves by the probe XY offset
295 float z = zprobe->probeDistance(x-std::get<X_AXIS>(this->probe_offsets), y-std::get<Y_AXIS>(this->probe_offsets));
296 if(isnan(z)) return false; // probe failed
297 z= zprobe->getProbeHeight() - z; // relative distance between the probe points, lower is negative z
298 stream->printf("DEBUG: P%d:%1.4f\n", i, z);
299 v[i] = Vector3(x, y, z);
300 }
301
302 // if first point is not within tolerance report it, it should ideally be 0
303 if(fabsf(v[0][2]) > this->tolerance) {
304 stream->printf("WARNING: probe is not within tolerance: %f > %f\n", fabsf(v[0][2]), this->tolerance);
305 }
306
307 // define the plane
308 delete this->plane;
309 // check tolerance level here default 0.03mm
310 auto mmx = std::minmax({v[0][2], v[1][2], v[2][2]});
311 if((mmx.second - mmx.first) <= this->tolerance) {
312 this->plane= nullptr; // plane is flat no need to do anything
313 stream->printf("DEBUG: flat plane\n");
314 // clear the compensationTransform in robot
315 setAdjustFunction(false);
316
317 }else{
318 this->plane = new Plane3D(v[0], v[1], v[2]);
319 stream->printf("DEBUG: plane normal= %f, %f, %f\n", plane->getNormal()[0], plane->getNormal()[1], plane->getNormal()[2]);
320 setAdjustFunction(true);
321 }
322
323 return true;
324 }
325
326 // Probes the 3 points and reports heights
327 bool ThreePointStrategy::test_probe_points(Gcode *gcode)
328 {
329 // check the probe points have been defined
330 float max_delta= 0;
331 float last_z= NAN;
332 for (int i = 0; i < 3; ++i) {
333 float x, y;
334 std::tie(x, y) = probe_points[i];
335 if(isnan(x) || isnan(y)) {
336 gcode->stream->printf("Probe point P%d has not been defined, use M557 P%d Xnnn Ynnn to define it\n", i, i);
337 return false;
338 }
339
340 float z = zprobe->probeDistance(x-std::get<X_AXIS>(this->probe_offsets), y-std::get<Y_AXIS>(this->probe_offsets));
341 if(isnan(z)) return false; // probe failed
342 gcode->stream->printf("X:%1.4f Y:%1.4f Z:%1.4f\n", x, y, z);
343
344 if(isnan(last_z)) {
345 last_z= z;
346 }else{
347 max_delta= std::max(max_delta, fabsf(z-last_z));
348 }
349 }
350
351 gcode->stream->printf("max delta: %f\n", max_delta);
352
353 return true;
354 }
355
356 void ThreePointStrategy::setAdjustFunction(bool on)
357 {
358 if(on) {
359 // set the compensationTransform in robot
360 THEROBOT->compensationTransform= [this](float *target, bool inverse) { if(inverse) target[2] -= this->plane->getz(target[0], target[1]); else target[2] += this->plane->getz(target[0], target[1]); };
361 }else{
362 // clear it
363 THEROBOT->compensationTransform= nullptr;
364 }
365 }
366
367 // find the Z offset for the point on the plane at x, y
368 float ThreePointStrategy::getZOffset(float x, float y)
369 {
370 if(this->plane == nullptr) return NAN;
371 return this->plane->getz(x, y);
372 }
373
374 // parse a "X,Y" string return x,y
375 std::tuple<float, float> ThreePointStrategy::parseXY(const char *str)
376 {
377 float x = NAN, y = NAN;
378 char *p;
379 x = strtof(str, &p);
380 if(p + 1 < str + strlen(str)) {
381 y = strtof(p + 1, nullptr);
382 }
383 return std::make_tuple(x, y);
384 }
385
386 // parse a "X,Y,Z" string return x,y,z tuple
387 std::tuple<float, float, float> ThreePointStrategy::parseXYZ(const char *str)
388 {
389 float x = 0, y = 0, z= 0;
390 char *p;
391 x = strtof(str, &p);
392 if(p + 1 < str + strlen(str)) {
393 y = strtof(p + 1, &p);
394 if(p + 1 < str + strlen(str)) {
395 z = strtof(p + 1, nullptr);
396 }
397 }
398 return std::make_tuple(x, y, z);
399 }
Unnamed repository; edit this file 'description' to name the repository.