2 Author: Jim Morris (wolfmanjm@gmail.com)
3 License: GPL3 or better see <http://www.gnu.org/licenses/>
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.
12 The strategy must be enabled in the cofnig as well as zprobe.
14 leveling-strategy.three-point-leveling.enable true
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:-
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)
23 or they may be defined (and saved with M500) using M557 P0 X30 Y40.5 where P is 0,1,2
25 probe offsets from the nozzle or tool head can be defined with
27 leveling-strategy.three-point-leveling.probe_offsets 0,0,0 # probe offsetrs x,y,z
29 they may also be set with M565 X0 Y0 Z0
31 To force homing in X and Y before G32 does the probe the following can be set in config, this is the default
33 leveling-strategy.three-point-leveling.home_first true # disable by setting to false
35 The probe tolerance can be set using the config line
37 leveling-strategy.three-point-leveling.tolerance 0.03 # the probe tolerance in mm, default is 0.03mm
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
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
50 M500 saves the probe points and the probe offsets
51 M503 displays the current settings
54 #include "ThreePointStrategy.h"
58 #include "StreamOutputPool.h"
60 #include "checksumm.h"
61 #include "ConfigValue.h"
62 #include "PublicDataRequest.h"
63 #include "PublicData.h"
67 #include "nuts_bolts.h"
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")
82 ThreePointStrategy::ThreePointStrategy(ZProbe
*zprobe
) : LevelingStrategy(zprobe
)
84 for (int i
= 0; i
< 3; ++i
) {
85 probe_points
[i
] = std::make_tuple(NAN
, NAN
);
90 ThreePointStrategy::~ThreePointStrategy()
95 bool ThreePointStrategy::handleConfig()
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());
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());
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();
115 bool ThreePointStrategy::handleGcode(Gcode
*gcode
)
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");
125 } else if( gcode
->g
== 31 ) { // report status
126 if(this->plane
== nullptr) {
127 gcode
->stream
->printf("Bed leveling plane is not set\n");
129 gcode
->stream
->printf("Bed leveling plane normal= %f, %f, %f\n", plane
->getNormal()[0], plane
->getNormal()[1], plane
->getNormal()[2]);
131 gcode
->stream
->printf("Probe is %s\n", zprobe
->getProbeStatus() ? "Triggered" : "Not triggered");
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();
138 // clear any existing plane and compensation
140 this->plane
= nullptr;
141 setAdjustFunction(false);
143 if(!doProbing(gcode
->stream
)) {
144 gcode
->stream
->printf("Probe failed to complete, probe not triggered or other error\n");
146 gcode
->stream
->printf("Probe completed, bed plane defined\n");
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
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
);
161 gcode
->stream
->printf("only 3 probe points allowed P0-P2\n");
165 } else if(gcode
->m
== 561) { // M561: Set Identity Transform with no parameters, set the saved plane if A B C D are given
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");
173 // smoothie specific way to restore a saved plane
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);
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
);
193 } else if(gcode
->m
== 500 || gcode
->m
== 503) { // M500 save, M503 display
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
);
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
);
204 // encode plane and save if set and M500 and enabled
205 if(this->save
&& this->plane
!= nullptr) {
206 if(gcode
->m
== 500) {
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
);
211 gcode
->stream
->printf(";The bed plane will be saved on M500\n");
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
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
);
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]);
232 if(gcode
->has_letter('X')) x
= gcode
->get_value('X');
233 if(gcode
->has_letter('Y')) y
= gcode
->get_value('Y');
235 gcode
->stream
->printf("z= %f\n", z
);
236 // tell robot to adjust z on each move
237 setAdjustFunction(true);
246 void ThreePointStrategy::homeXY()
248 Gcode
gc("G28 X0 Y0", &(StreamOutput::NullStream
));
249 THEKERNEL
->call_event(ON_GCODE_RECEIVED
, &gc
);
252 bool ThreePointStrategy::doProbing(StreamOutput
*stream
)
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
);
264 // optionally home XY axis first, but allow for manual homing
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());
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
278 // TODO this needs to be configurable to use min z or probe
280 // find bed via probe
282 if(!zprobe
->run_probe(mm
, zprobe
->getSlowFeedrate())) return false;
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
);
287 // move up to specified probe start position
288 zprobe
->coordinated_move(NAN
, NAN
, zprobe
->getProbeHeight(), zprobe
->getSlowFeedrate()); // move to probe start position
290 // probe the three points
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
);
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
);
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);
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);
326 // Probes the 3 points and reports heights
327 bool ThreePointStrategy::test_probe_points(Gcode
*gcode
)
329 // check the probe points have been defined
332 for (int i
= 0; i
< 3; ++i
) {
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
);
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
);
347 max_delta
= std::max(max_delta
, fabsf(z
-last_z
));
351 gcode
->stream
->printf("max delta: %f\n", max_delta
);
356 void ThreePointStrategy::setAdjustFunction(bool 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]); };
363 THEROBOT
->compensationTransform
= nullptr;
367 // find the Z offset for the point on the plane at x, y
368 float ThreePointStrategy::getZOffset(float x
, float y
)
370 if(this->plane
== nullptr) return NAN
;
371 return this->plane
->getz(x
, y
);
374 // parse a "X,Y" string return x,y
375 std::tuple
<float, float> ThreePointStrategy::parseXY(const char *str
)
377 float x
= NAN
, y
= NAN
;
380 if(p
+ 1 < str
+ strlen(str
)) {
381 y
= strtof(p
+ 1, nullptr);
383 return std::make_tuple(x
, y
);
386 // parse a "X,Y,Z" string return x,y,z tuple
387 std::tuple
<float, float, float> ThreePointStrategy::parseXYZ(const char *str
)
389 float x
= 0, y
= 0, z
= 0;
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);
398 return std::make_tuple(x
, y
, z
);