|
@@ -65,8 +65,7 @@
|
65
|
65
|
unified_bed_leveling::g29_phase_value,
|
66
|
66
|
unified_bed_leveling::g29_repetition_cnt,
|
67
|
67
|
unified_bed_leveling::g29_storage_slot = 0,
|
68
|
|
- unified_bed_leveling::g29_map_type,
|
69
|
|
- unified_bed_leveling::g29_grid_size;
|
|
68
|
+ unified_bed_leveling::g29_map_type;
|
70
|
69
|
bool unified_bed_leveling::g29_c_flag,
|
71
|
70
|
unified_bed_leveling::g29_x_flag,
|
72
|
71
|
unified_bed_leveling::g29_y_flag;
|
|
@@ -75,6 +74,10 @@
|
75
|
74
|
unified_bed_leveling::g29_card_thickness = 0.0,
|
76
|
75
|
unified_bed_leveling::g29_constant = 0.0;
|
77
|
76
|
|
|
77
|
+ #if HAS_BED_PROBE
|
|
78
|
+ int unified_bed_leveling::g29_grid_size;
|
|
79
|
+ #endif
|
|
80
|
+
|
78
|
81
|
/**
|
79
|
82
|
* G29: Unified Bed Leveling by Roxy
|
80
|
83
|
*
|
|
@@ -310,6 +313,8 @@
|
310
|
313
|
return;
|
311
|
314
|
}
|
312
|
315
|
|
|
316
|
+ if (g29_parameter_parsing()) return; // abort if parsing the simple parameters causes a problem,
|
|
317
|
+
|
313
|
318
|
// Check for commands that require the printer to be homed
|
314
|
319
|
if (axis_unhomed_error()) {
|
315
|
320
|
const int8_t p_val = parser.intval('P', -1);
|
|
@@ -317,8 +322,6 @@
|
317
|
322
|
gcode.home_all_axes();
|
318
|
323
|
}
|
319
|
324
|
|
320
|
|
- if (g29_parameter_parsing()) return; // abort if parsing the simple parameters causes a problem,
|
321
|
|
-
|
322
|
325
|
// Invalidate Mesh Points. This command is a little bit asymmetrical because
|
323
|
326
|
// it directly specifies the repetition count and does not use the 'R' parameter.
|
324
|
327
|
if (parser.seen('I')) {
|
|
@@ -381,40 +384,44 @@
|
381
|
384
|
}
|
382
|
385
|
}
|
383
|
386
|
|
384
|
|
- if (parser.seen('J')) {
|
385
|
|
- if (g29_grid_size) { // if not 0 it is a normal n x n grid being probed
|
386
|
|
- save_ubl_active_state_and_disable();
|
387
|
|
- tilt_mesh_based_on_probed_grid(parser.seen('T'));
|
388
|
|
- restore_ubl_active_state_and_leave();
|
389
|
|
- }
|
390
|
|
- else { // grid_size == 0 : A 3-Point leveling has been requested
|
391
|
|
- float z3, z2, z1 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level);
|
392
|
|
- if (!isnan(z1)) {
|
393
|
|
- z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level);
|
394
|
|
- if (!isnan(z2))
|
395
|
|
- z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level);
|
396
|
|
- }
|
|
387
|
+ #if HAS_BED_PROBE
|
397
|
388
|
|
398
|
|
- if (isnan(z1) || isnan(z2) || isnan(z3)) { // probe_pt will return NAN if unreachable
|
399
|
|
- SERIAL_ERROR_START();
|
400
|
|
- SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
|
401
|
|
- goto LEAVE;
|
|
389
|
+ if (parser.seen('J')) {
|
|
390
|
+ if (g29_grid_size) { // if not 0 it is a normal n x n grid being probed
|
|
391
|
+ save_ubl_active_state_and_disable();
|
|
392
|
+ tilt_mesh_based_on_probed_grid(parser.seen('T'));
|
|
393
|
+ restore_ubl_active_state_and_leave();
|
402
|
394
|
}
|
|
395
|
+ else { // grid_size == 0 : A 3-Point leveling has been requested
|
|
396
|
+ float z3, z2, z1 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y), false, g29_verbose_level);
|
|
397
|
+ if (!isnan(z1)) {
|
|
398
|
+ z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level);
|
|
399
|
+ if (!isnan(z2))
|
|
400
|
+ z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level);
|
|
401
|
+ }
|
403
|
402
|
|
404
|
|
- // Adjust z1, z2, z3 by the Mesh Height at these points. Just because they're non-zero
|
405
|
|
- // doesn't mean the Mesh is tilted! (Compensate each probe point by what the Mesh says
|
406
|
|
- // its height is.)
|
|
403
|
+ if (isnan(z1) || isnan(z2) || isnan(z3)) { // probe_pt will return NAN if unreachable
|
|
404
|
+ SERIAL_ERROR_START();
|
|
405
|
+ SERIAL_ERRORLNPGM("Attempt to probe off the bed.");
|
|
406
|
+ goto LEAVE;
|
|
407
|
+ }
|
407
|
408
|
|
408
|
|
- save_ubl_active_state_and_disable();
|
409
|
|
- z1 -= get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y)) /* + zprobe_zoffset */ ;
|
410
|
|
- z2 -= get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y)) /* + zprobe_zoffset */ ;
|
411
|
|
- z3 -= get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y)) /* + zprobe_zoffset */ ;
|
|
409
|
+ // Adjust z1, z2, z3 by the Mesh Height at these points. Just because they're non-zero
|
|
410
|
+ // doesn't mean the Mesh is tilted! (Compensate each probe point by what the Mesh says
|
|
411
|
+ // its height is.)
|
412
|
412
|
|
413
|
|
- do_blocking_move_to_xy(0.5 * (UBL_MESH_MAX_X - (UBL_MESH_MIN_X)), 0.5 * (UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)));
|
414
|
|
- tilt_mesh_based_on_3pts(z1, z2, z3);
|
415
|
|
- restore_ubl_active_state_and_leave();
|
|
413
|
+ save_ubl_active_state_and_disable();
|
|
414
|
+ z1 -= get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_1_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_1_Y)) /* + zprobe_zoffset */ ;
|
|
415
|
+ z2 -= get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y)) /* + zprobe_zoffset */ ;
|
|
416
|
+ z3 -= get_z_correction(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y)) /* + zprobe_zoffset */ ;
|
|
417
|
+
|
|
418
|
+ do_blocking_move_to_xy(0.5 * (UBL_MESH_MAX_X - (UBL_MESH_MIN_X)), 0.5 * (UBL_MESH_MAX_Y - (UBL_MESH_MIN_Y)));
|
|
419
|
+ tilt_mesh_based_on_3pts(z1, z2, z3);
|
|
420
|
+ restore_ubl_active_state_and_leave();
|
|
421
|
+ }
|
416
|
422
|
}
|
417
|
|
- }
|
|
423
|
+
|
|
424
|
+ #endif // HAS_BED_PROBE
|
418
|
425
|
|
419
|
426
|
if (parser.seen('P')) {
|
420
|
427
|
if (WITHIN(g29_phase_value, 0, 1) && state.storage_slot == -1) {
|
|
@@ -431,23 +438,27 @@
|
431
|
438
|
SERIAL_PROTOCOLLNPGM("Mesh zeroed.");
|
432
|
439
|
break;
|
433
|
440
|
|
434
|
|
- case 1:
|
435
|
|
- //
|
436
|
|
- // Invalidate Entire Mesh and Automatically Probe Mesh in areas that can be reached by the probe
|
437
|
|
- //
|
438
|
|
- if (!parser.seen('C')) {
|
439
|
|
- invalidate();
|
440
|
|
- SERIAL_PROTOCOLLNPGM("Mesh invalidated. Probing mesh.");
|
441
|
|
- }
|
442
|
|
- if (g29_verbose_level > 1) {
|
443
|
|
- SERIAL_PROTOCOLPAIR("Probing Mesh Points Closest to (", g29_x_pos);
|
444
|
|
- SERIAL_PROTOCOLCHAR(',');
|
445
|
|
- SERIAL_PROTOCOL(g29_y_pos);
|
446
|
|
- SERIAL_PROTOCOLLNPGM(").\n");
|
447
|
|
- }
|
448
|
|
- probe_entire_mesh(g29_x_pos + X_PROBE_OFFSET_FROM_EXTRUDER, g29_y_pos + Y_PROBE_OFFSET_FROM_EXTRUDER,
|
449
|
|
- parser.seen('T'), parser.seen('E'), parser.seen('U'));
|
450
|
|
- break;
|
|
441
|
+ #if HAS_BED_PROBE
|
|
442
|
+
|
|
443
|
+ case 1:
|
|
444
|
+ //
|
|
445
|
+ // Invalidate Entire Mesh and Automatically Probe Mesh in areas that can be reached by the probe
|
|
446
|
+ //
|
|
447
|
+ if (!parser.seen('C')) {
|
|
448
|
+ invalidate();
|
|
449
|
+ SERIAL_PROTOCOLLNPGM("Mesh invalidated. Probing mesh.");
|
|
450
|
+ }
|
|
451
|
+ if (g29_verbose_level > 1) {
|
|
452
|
+ SERIAL_PROTOCOLPAIR("Probing Mesh Points Closest to (", g29_x_pos);
|
|
453
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
454
|
+ SERIAL_PROTOCOL(g29_y_pos);
|
|
455
|
+ SERIAL_PROTOCOLLNPGM(").\n");
|
|
456
|
+ }
|
|
457
|
+ probe_entire_mesh(g29_x_pos + X_PROBE_OFFSET_FROM_EXTRUDER, g29_y_pos + Y_PROBE_OFFSET_FROM_EXTRUDER,
|
|
458
|
+ parser.seen('T'), parser.seen('E'), parser.seen('U'));
|
|
459
|
+ break;
|
|
460
|
+
|
|
461
|
+ #endif
|
451
|
462
|
|
452
|
463
|
case 2: {
|
453
|
464
|
#if ENABLED(NEWPANEL)
|
|
@@ -776,161 +787,166 @@
|
776
|
787
|
z_values[x][y] += g29_constant;
|
777
|
788
|
}
|
778
|
789
|
|
779
|
|
- /**
|
780
|
|
- * Probe all invalidated locations of the mesh that can be reached by the probe.
|
781
|
|
- * This attempts to fill in locations closest to the nozzle's start location first.
|
782
|
|
- */
|
783
|
|
- void unified_bed_leveling::probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool close_or_far) {
|
784
|
|
- mesh_index_pair location;
|
785
|
|
-
|
786
|
|
- has_control_of_lcd_panel = true;
|
787
|
|
- save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe
|
788
|
|
- DEPLOY_PROBE();
|
789
|
|
-
|
790
|
|
- uint16_t max_iterations = GRID_MAX_POINTS;
|
|
790
|
+ #if HAS_BED_PROBE
|
791
|
791
|
|
792
|
|
- do {
|
793
|
|
- #if ENABLED(NEWPANEL)
|
794
|
|
- if (ubl_lcd_clicked()) {
|
795
|
|
- SERIAL_PROTOCOLLNPGM("\nMesh only partially populated.\n");
|
796
|
|
- lcd_quick_feedback();
|
797
|
|
- STOW_PROBE();
|
798
|
|
- while (ubl_lcd_clicked()) idle();
|
799
|
|
- has_control_of_lcd_panel = false;
|
800
|
|
- restore_ubl_active_state_and_leave();
|
801
|
|
- safe_delay(50); // Debounce the Encoder wheel
|
802
|
|
- return;
|
803
|
|
- }
|
804
|
|
- #endif
|
805
|
|
-
|
806
|
|
- location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_PROBE_AS_REFERENCE, NULL, close_or_far);
|
807
|
|
-
|
808
|
|
- if (location.x_index >= 0) { // mesh point found and is reachable by probe
|
809
|
|
- const float rawx = mesh_index_to_xpos(location.x_index),
|
810
|
|
- rawy = mesh_index_to_ypos(location.y_index);
|
|
792
|
+ /**
|
|
793
|
+ * Probe all invalidated locations of the mesh that can be reached by the probe.
|
|
794
|
+ * This attempts to fill in locations closest to the nozzle's start location first.
|
|
795
|
+ */
|
|
796
|
+ void unified_bed_leveling::probe_entire_mesh(const float &lx, const float &ly, const bool do_ubl_mesh_map, const bool stow_probe, bool close_or_far) {
|
|
797
|
+ mesh_index_pair location;
|
811
|
798
|
|
812
|
|
- const float measured_z = probe_pt(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy), stow_probe, g29_verbose_level); // TODO: Needs error handling
|
813
|
|
- z_values[location.x_index][location.y_index] = measured_z;
|
814
|
|
- }
|
|
799
|
+ has_control_of_lcd_panel = true;
|
|
800
|
+ save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe
|
|
801
|
+ DEPLOY_PROBE();
|
815
|
802
|
|
816
|
|
- if (do_ubl_mesh_map) display_map(g29_map_type);
|
|
803
|
+ uint16_t max_iterations = GRID_MAX_POINTS;
|
817
|
804
|
|
818
|
|
- } while (location.x_index >= 0 && --max_iterations);
|
|
805
|
+ do {
|
|
806
|
+ #if ENABLED(NEWPANEL)
|
|
807
|
+ if (ubl_lcd_clicked()) {
|
|
808
|
+ SERIAL_PROTOCOLLNPGM("\nMesh only partially populated.\n");
|
|
809
|
+ lcd_quick_feedback();
|
|
810
|
+ STOW_PROBE();
|
|
811
|
+ while (ubl_lcd_clicked()) idle();
|
|
812
|
+ has_control_of_lcd_panel = false;
|
|
813
|
+ restore_ubl_active_state_and_leave();
|
|
814
|
+ safe_delay(50); // Debounce the Encoder wheel
|
|
815
|
+ return;
|
|
816
|
+ }
|
|
817
|
+ #endif
|
819
|
818
|
|
820
|
|
- STOW_PROBE();
|
821
|
|
- restore_ubl_active_state_and_leave();
|
|
819
|
+ location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_PROBE_AS_REFERENCE, NULL, close_or_far);
|
822
|
820
|
|
823
|
|
- do_blocking_move_to_xy(
|
824
|
|
- constrain(lx - (X_PROBE_OFFSET_FROM_EXTRUDER), UBL_MESH_MIN_X, UBL_MESH_MAX_X),
|
825
|
|
- constrain(ly - (Y_PROBE_OFFSET_FROM_EXTRUDER), UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)
|
826
|
|
- );
|
827
|
|
- }
|
|
821
|
+ if (location.x_index >= 0) { // mesh point found and is reachable by probe
|
|
822
|
+ const float rawx = mesh_index_to_xpos(location.x_index),
|
|
823
|
+ rawy = mesh_index_to_ypos(location.y_index);
|
828
|
824
|
|
829
|
|
- void unified_bed_leveling::tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3) {
|
830
|
|
- matrix_3x3 rotation;
|
831
|
|
- vector_3 v1 = vector_3( (UBL_PROBE_PT_1_X - UBL_PROBE_PT_2_X),
|
832
|
|
- (UBL_PROBE_PT_1_Y - UBL_PROBE_PT_2_Y),
|
833
|
|
- (z1 - z2) ),
|
|
825
|
+ const float measured_z = probe_pt(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy), stow_probe, g29_verbose_level); // TODO: Needs error handling
|
|
826
|
+ z_values[location.x_index][location.y_index] = measured_z;
|
|
827
|
+ }
|
834
|
828
|
|
835
|
|
- v2 = vector_3( (UBL_PROBE_PT_3_X - UBL_PROBE_PT_2_X),
|
836
|
|
- (UBL_PROBE_PT_3_Y - UBL_PROBE_PT_2_Y),
|
837
|
|
- (z3 - z2) ),
|
|
829
|
+ if (do_ubl_mesh_map) display_map(g29_map_type);
|
838
|
830
|
|
839
|
|
- normal = vector_3::cross(v1, v2);
|
|
831
|
+ } while (location.x_index >= 0 && --max_iterations);
|
840
|
832
|
|
841
|
|
- normal = normal.get_normal();
|
|
833
|
+ STOW_PROBE();
|
|
834
|
+ restore_ubl_active_state_and_leave();
|
842
|
835
|
|
843
|
|
- /**
|
844
|
|
- * This vector is normal to the tilted plane.
|
845
|
|
- * However, we don't know its direction. We need it to point up. So if
|
846
|
|
- * Z is negative, we need to invert the sign of all components of the vector
|
847
|
|
- */
|
848
|
|
- if (normal.z < 0.0) {
|
849
|
|
- normal.x = -normal.x;
|
850
|
|
- normal.y = -normal.y;
|
851
|
|
- normal.z = -normal.z;
|
|
836
|
+ do_blocking_move_to_xy(
|
|
837
|
+ constrain(lx - (X_PROBE_OFFSET_FROM_EXTRUDER), UBL_MESH_MIN_X, UBL_MESH_MAX_X),
|
|
838
|
+ constrain(ly - (Y_PROBE_OFFSET_FROM_EXTRUDER), UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)
|
|
839
|
+ );
|
852
|
840
|
}
|
853
|
841
|
|
854
|
|
- rotation = matrix_3x3::create_look_at(vector_3(normal.x, normal.y, 1));
|
855
|
|
-
|
856
|
|
- if (g29_verbose_level > 2) {
|
857
|
|
- SERIAL_ECHOPGM("bed plane normal = [");
|
858
|
|
- SERIAL_PROTOCOL_F(normal.x, 7);
|
859
|
|
- SERIAL_PROTOCOLCHAR(',');
|
860
|
|
- SERIAL_PROTOCOL_F(normal.y, 7);
|
861
|
|
- SERIAL_PROTOCOLCHAR(',');
|
862
|
|
- SERIAL_PROTOCOL_F(normal.z, 7);
|
863
|
|
- SERIAL_ECHOLNPGM("]");
|
864
|
|
- rotation.debug(PSTR("rotation matrix:"));
|
865
|
|
- }
|
|
842
|
+ void unified_bed_leveling::tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3) {
|
|
843
|
+ matrix_3x3 rotation;
|
|
844
|
+ vector_3 v1 = vector_3( (UBL_PROBE_PT_1_X - UBL_PROBE_PT_2_X),
|
|
845
|
+ (UBL_PROBE_PT_1_Y - UBL_PROBE_PT_2_Y),
|
|
846
|
+ (z1 - z2) ),
|
|
847
|
+
|
|
848
|
+ v2 = vector_3( (UBL_PROBE_PT_3_X - UBL_PROBE_PT_2_X),
|
|
849
|
+ (UBL_PROBE_PT_3_Y - UBL_PROBE_PT_2_Y),
|
|
850
|
+ (z3 - z2) ),
|
|
851
|
+
|
|
852
|
+ normal = vector_3::cross(v1, v2);
|
|
853
|
+
|
|
854
|
+ normal = normal.get_normal();
|
|
855
|
+
|
|
856
|
+ /**
|
|
857
|
+ * This vector is normal to the tilted plane.
|
|
858
|
+ * However, we don't know its direction. We need it to point up. So if
|
|
859
|
+ * Z is negative, we need to invert the sign of all components of the vector
|
|
860
|
+ */
|
|
861
|
+ if (normal.z < 0.0) {
|
|
862
|
+ normal.x = -normal.x;
|
|
863
|
+ normal.y = -normal.y;
|
|
864
|
+ normal.z = -normal.z;
|
|
865
|
+ }
|
866
|
866
|
|
867
|
|
- //
|
868
|
|
- // All of 3 of these points should give us the same d constant
|
869
|
|
- //
|
|
867
|
+ rotation = matrix_3x3::create_look_at(vector_3(normal.x, normal.y, 1));
|
870
|
868
|
|
871
|
|
- float t = normal.x * (UBL_PROBE_PT_1_X) + normal.y * (UBL_PROBE_PT_1_Y),
|
872
|
|
- d = t + normal.z * z1;
|
|
869
|
+ if (g29_verbose_level > 2) {
|
|
870
|
+ SERIAL_ECHOPGM("bed plane normal = [");
|
|
871
|
+ SERIAL_PROTOCOL_F(normal.x, 7);
|
|
872
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
873
|
+ SERIAL_PROTOCOL_F(normal.y, 7);
|
|
874
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
875
|
+ SERIAL_PROTOCOL_F(normal.z, 7);
|
|
876
|
+ SERIAL_ECHOLNPGM("]");
|
|
877
|
+ rotation.debug(PSTR("rotation matrix:"));
|
|
878
|
+ }
|
873
|
879
|
|
874
|
|
- if (g29_verbose_level>2) {
|
875
|
|
- SERIAL_ECHOPGM("D constant: ");
|
876
|
|
- SERIAL_PROTOCOL_F(d, 7);
|
877
|
|
- SERIAL_ECHOLNPGM(" ");
|
878
|
|
- }
|
|
880
|
+ //
|
|
881
|
+ // All of 3 of these points should give us the same d constant
|
|
882
|
+ //
|
879
|
883
|
|
880
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
881
|
|
- if (DEBUGGING(LEVELING)) {
|
882
|
|
- SERIAL_ECHOPGM("d from 1st point: ");
|
883
|
|
- SERIAL_ECHO_F(d, 6);
|
884
|
|
- SERIAL_EOL();
|
885
|
|
- t = normal.x * (UBL_PROBE_PT_2_X) + normal.y * (UBL_PROBE_PT_2_Y);
|
886
|
|
- d = t + normal.z * z2;
|
887
|
|
- SERIAL_ECHOPGM("d from 2nd point: ");
|
888
|
|
- SERIAL_ECHO_F(d, 6);
|
889
|
|
- SERIAL_EOL();
|
890
|
|
- t = normal.x * (UBL_PROBE_PT_3_X) + normal.y * (UBL_PROBE_PT_3_Y);
|
891
|
|
- d = t + normal.z * z3;
|
892
|
|
- SERIAL_ECHOPGM("d from 3rd point: ");
|
893
|
|
- SERIAL_ECHO_F(d, 6);
|
894
|
|
- SERIAL_EOL();
|
|
884
|
+ float t = normal.x * (UBL_PROBE_PT_1_X) + normal.y * (UBL_PROBE_PT_1_Y),
|
|
885
|
+ d = t + normal.z * z1;
|
|
886
|
+
|
|
887
|
+ if (g29_verbose_level>2) {
|
|
888
|
+ SERIAL_ECHOPGM("D constant: ");
|
|
889
|
+ SERIAL_PROTOCOL_F(d, 7);
|
|
890
|
+ SERIAL_ECHOLNPGM(" ");
|
895
|
891
|
}
|
896
|
|
- #endif
|
897
|
892
|
|
898
|
|
- for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
|
899
|
|
- for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
|
900
|
|
- float x_tmp = mesh_index_to_xpos(i),
|
901
|
|
- y_tmp = mesh_index_to_ypos(j),
|
902
|
|
- z_tmp = z_values[i][j];
|
903
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
904
|
|
- if (DEBUGGING(LEVELING)) {
|
905
|
|
- SERIAL_ECHOPGM("before rotation = [");
|
906
|
|
- SERIAL_PROTOCOL_F(x_tmp, 7);
|
907
|
|
- SERIAL_PROTOCOLCHAR(',');
|
908
|
|
- SERIAL_PROTOCOL_F(y_tmp, 7);
|
909
|
|
- SERIAL_PROTOCOLCHAR(',');
|
910
|
|
- SERIAL_PROTOCOL_F(z_tmp, 7);
|
911
|
|
- SERIAL_ECHOPGM("] ---> ");
|
912
|
|
- safe_delay(20);
|
913
|
|
- }
|
914
|
|
- #endif
|
915
|
|
- apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
|
916
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
917
|
|
- if (DEBUGGING(LEVELING)) {
|
918
|
|
- SERIAL_ECHOPGM("after rotation = [");
|
919
|
|
- SERIAL_PROTOCOL_F(x_tmp, 7);
|
920
|
|
- SERIAL_PROTOCOLCHAR(',');
|
921
|
|
- SERIAL_PROTOCOL_F(y_tmp, 7);
|
922
|
|
- SERIAL_PROTOCOLCHAR(',');
|
923
|
|
- SERIAL_PROTOCOL_F(z_tmp, 7);
|
924
|
|
- SERIAL_ECHOLNPGM("]");
|
925
|
|
- safe_delay(55);
|
926
|
|
- }
|
927
|
|
- #endif
|
928
|
|
- z_values[i][j] += z_tmp - d;
|
|
893
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
894
|
+ if (DEBUGGING(LEVELING)) {
|
|
895
|
+ SERIAL_ECHOPGM("d from 1st point: ");
|
|
896
|
+ SERIAL_ECHO_F(d, 6);
|
|
897
|
+ SERIAL_EOL();
|
|
898
|
+ t = normal.x * (UBL_PROBE_PT_2_X) + normal.y * (UBL_PROBE_PT_2_Y);
|
|
899
|
+ d = t + normal.z * z2;
|
|
900
|
+ SERIAL_ECHOPGM("d from 2nd point: ");
|
|
901
|
+ SERIAL_ECHO_F(d, 6);
|
|
902
|
+ SERIAL_EOL();
|
|
903
|
+ t = normal.x * (UBL_PROBE_PT_3_X) + normal.y * (UBL_PROBE_PT_3_Y);
|
|
904
|
+ d = t + normal.z * z3;
|
|
905
|
+ SERIAL_ECHOPGM("d from 3rd point: ");
|
|
906
|
+ SERIAL_ECHO_F(d, 6);
|
|
907
|
+ SERIAL_EOL();
|
|
908
|
+ }
|
|
909
|
+ #endif
|
|
910
|
+
|
|
911
|
+ for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
|
|
912
|
+ for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
|
|
913
|
+ float x_tmp = mesh_index_to_xpos(i),
|
|
914
|
+ y_tmp = mesh_index_to_ypos(j),
|
|
915
|
+ z_tmp = z_values[i][j];
|
|
916
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
917
|
+ if (DEBUGGING(LEVELING)) {
|
|
918
|
+ SERIAL_ECHOPGM("before rotation = [");
|
|
919
|
+ SERIAL_PROTOCOL_F(x_tmp, 7);
|
|
920
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
921
|
+ SERIAL_PROTOCOL_F(y_tmp, 7);
|
|
922
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
923
|
+ SERIAL_PROTOCOL_F(z_tmp, 7);
|
|
924
|
+ SERIAL_ECHOPGM("] ---> ");
|
|
925
|
+ safe_delay(20);
|
|
926
|
+ }
|
|
927
|
+ #endif
|
|
928
|
+ apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
|
|
929
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
930
|
+ if (DEBUGGING(LEVELING)) {
|
|
931
|
+ SERIAL_ECHOPGM("after rotation = [");
|
|
932
|
+ SERIAL_PROTOCOL_F(x_tmp, 7);
|
|
933
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
934
|
+ SERIAL_PROTOCOL_F(y_tmp, 7);
|
|
935
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
936
|
+ SERIAL_PROTOCOL_F(z_tmp, 7);
|
|
937
|
+ SERIAL_ECHOLNPGM("]");
|
|
938
|
+ safe_delay(55);
|
|
939
|
+ }
|
|
940
|
+ #endif
|
|
941
|
+ z_values[i][j] += z_tmp - d;
|
|
942
|
+ }
|
929
|
943
|
}
|
930
|
944
|
}
|
931
|
|
- }
|
|
945
|
+
|
|
946
|
+ #endif // HAS_BED_PROBE
|
932
|
947
|
|
933
|
948
|
#if ENABLED(NEWPANEL)
|
|
949
|
+
|
934
|
950
|
float unified_bed_leveling::measure_point_with_encoder() {
|
935
|
951
|
|
936
|
952
|
while (ubl_lcd_clicked()) delay(50); // wait for user to release encoder wheel
|
|
@@ -1080,7 +1096,8 @@
|
1080
|
1096
|
do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
|
1081
|
1097
|
do_blocking_move_to_xy(lx, ly);
|
1082
|
1098
|
}
|
1083
|
|
- #endif
|
|
1099
|
+
|
|
1100
|
+ #endif // NEWPANEL
|
1084
|
1101
|
|
1085
|
1102
|
bool unified_bed_leveling::g29_parameter_parsing() {
|
1086
|
1103
|
bool err_flag = false;
|
|
@@ -1114,19 +1131,34 @@
|
1114
|
1131
|
}
|
1115
|
1132
|
|
1116
|
1133
|
if (parser.seen('P')) {
|
1117
|
|
- g29_phase_value = parser.value_int();
|
1118
|
|
- if (!WITHIN(g29_phase_value, 0, 6)) {
|
1119
|
|
- SERIAL_PROTOCOLLNPGM("?(P)hase value invalid (0-6).\n");
|
1120
|
|
- err_flag = true;
|
1121
|
|
- }
|
|
1134
|
+ const int pv = parser.value_int();
|
|
1135
|
+ #if !HAS_BED_PROBE
|
|
1136
|
+ if (pv == 1) {
|
|
1137
|
+ SERIAL_PROTOCOLLNPGM("G29 P1 requires a probe.\n");
|
|
1138
|
+ err_flag = true;
|
|
1139
|
+ }
|
|
1140
|
+ else
|
|
1141
|
+ #endif
|
|
1142
|
+ {
|
|
1143
|
+ g29_phase_value = pv;
|
|
1144
|
+ if (!WITHIN(g29_phase_value, 0, 6)) {
|
|
1145
|
+ SERIAL_PROTOCOLLNPGM("?(P)hase value invalid (0-6).\n");
|
|
1146
|
+ err_flag = true;
|
|
1147
|
+ }
|
|
1148
|
+ }
|
1122
|
1149
|
}
|
1123
|
1150
|
|
1124
|
1151
|
if (parser.seen('J')) {
|
1125
|
|
- g29_grid_size = parser.has_value() ? parser.value_int() : 0;
|
1126
|
|
- if (g29_grid_size && !WITHIN(g29_grid_size, 2, 9)) {
|
1127
|
|
- SERIAL_PROTOCOLLNPGM("?Invalid grid size (J) specified (2-9).\n");
|
|
1152
|
+ #if HAS_BED_PROBE
|
|
1153
|
+ g29_grid_size = parser.has_value() ? parser.value_int() : 0;
|
|
1154
|
+ if (g29_grid_size && !WITHIN(g29_grid_size, 2, 9)) {
|
|
1155
|
+ SERIAL_PROTOCOLLNPGM("?Invalid grid size (J) specified (2-9).\n");
|
|
1156
|
+ err_flag = true;
|
|
1157
|
+ }
|
|
1158
|
+ #else
|
|
1159
|
+ SERIAL_PROTOCOLLNPGM("G29 J action requires a probe.\n");
|
1128
|
1160
|
err_flag = true;
|
1129
|
|
- }
|
|
1161
|
+ #endif
|
1130
|
1162
|
}
|
1131
|
1163
|
|
1132
|
1164
|
if (g29_x_flag != g29_y_flag) {
|
|
@@ -1624,128 +1656,66 @@
|
1624
|
1656
|
}
|
1625
|
1657
|
}
|
1626
|
1658
|
|
1627
|
|
- void unified_bed_leveling::tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map) {
|
1628
|
|
- constexpr int16_t x_min = max(MIN_PROBE_X, UBL_MESH_MIN_X),
|
1629
|
|
- x_max = min(MAX_PROBE_X, UBL_MESH_MAX_X),
|
1630
|
|
- y_min = max(MIN_PROBE_Y, UBL_MESH_MIN_Y),
|
1631
|
|
- y_max = min(MAX_PROBE_Y, UBL_MESH_MAX_Y);
|
1632
|
|
-
|
1633
|
|
- const float dx = float(x_max - x_min) / (g29_grid_size - 1.0),
|
1634
|
|
- dy = float(y_max - y_min) / (g29_grid_size - 1.0);
|
1635
|
|
-
|
1636
|
|
- struct linear_fit_data lsf_results;
|
1637
|
|
- incremental_LSF_reset(&lsf_results);
|
1638
|
|
-
|
1639
|
|
- bool zig_zag = false;
|
1640
|
|
- for (uint8_t ix = 0; ix < g29_grid_size; ix++) {
|
1641
|
|
- const float x = float(x_min) + ix * dx;
|
1642
|
|
- for (int8_t iy = 0; iy < g29_grid_size; iy++) {
|
1643
|
|
- const float y = float(y_min) + dy * (zig_zag ? g29_grid_size - 1 - iy : iy);
|
1644
|
|
- float measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), parser.seen('E'), g29_verbose_level); // TODO: Needs error handling
|
1645
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
1646
|
|
- if (DEBUGGING(LEVELING)) {
|
1647
|
|
- SERIAL_CHAR('(');
|
1648
|
|
- SERIAL_PROTOCOL_F(x, 7);
|
1649
|
|
- SERIAL_CHAR(',');
|
1650
|
|
- SERIAL_PROTOCOL_F(y, 7);
|
1651
|
|
- SERIAL_ECHOPGM(") logical: ");
|
1652
|
|
- SERIAL_CHAR('(');
|
1653
|
|
- SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(x), 7);
|
1654
|
|
- SERIAL_CHAR(',');
|
1655
|
|
- SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(y), 7);
|
1656
|
|
- SERIAL_ECHOPGM(") measured: ");
|
1657
|
|
- SERIAL_PROTOCOL_F(measured_z, 7);
|
1658
|
|
- SERIAL_ECHOPGM(" correction: ");
|
1659
|
|
- SERIAL_PROTOCOL_F(get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)), 7);
|
1660
|
|
- }
|
1661
|
|
- #endif
|
|
1659
|
+ #if HAS_BED_PROBE
|
1662
|
1660
|
|
1663
|
|
- measured_z -= get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)) /* + zprobe_zoffset */ ;
|
1664
|
|
-
|
1665
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
1666
|
|
- if (DEBUGGING(LEVELING)) {
|
1667
|
|
- SERIAL_ECHOPGM(" final >>>---> ");
|
1668
|
|
- SERIAL_PROTOCOL_F(measured_z, 7);
|
1669
|
|
- SERIAL_EOL();
|
1670
|
|
- }
|
1671
|
|
- #endif
|
1672
|
|
-
|
1673
|
|
- incremental_LSF(&lsf_results, x, y, measured_z);
|
1674
|
|
- }
|
1675
|
|
-
|
1676
|
|
- zig_zag ^= true;
|
1677
|
|
- }
|
1678
|
|
-
|
1679
|
|
- if (finish_incremental_LSF(&lsf_results)) {
|
1680
|
|
- SERIAL_ECHOPGM("Could not complete LSF!");
|
1681
|
|
- return;
|
1682
|
|
- }
|
|
1661
|
+ void unified_bed_leveling::tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map) {
|
|
1662
|
+ constexpr int16_t x_min = max(MIN_PROBE_X, UBL_MESH_MIN_X),
|
|
1663
|
+ x_max = min(MAX_PROBE_X, UBL_MESH_MAX_X),
|
|
1664
|
+ y_min = max(MIN_PROBE_Y, UBL_MESH_MIN_Y),
|
|
1665
|
+ y_max = min(MAX_PROBE_Y, UBL_MESH_MAX_Y);
|
1683
|
1666
|
|
1684
|
|
- if (g29_verbose_level > 3) {
|
1685
|
|
- SERIAL_ECHOPGM("LSF Results A=");
|
1686
|
|
- SERIAL_PROTOCOL_F(lsf_results.A, 7);
|
1687
|
|
- SERIAL_ECHOPGM(" B=");
|
1688
|
|
- SERIAL_PROTOCOL_F(lsf_results.B, 7);
|
1689
|
|
- SERIAL_ECHOPGM(" D=");
|
1690
|
|
- SERIAL_PROTOCOL_F(lsf_results.D, 7);
|
1691
|
|
- SERIAL_EOL();
|
1692
|
|
- }
|
|
1667
|
+ const float dx = float(x_max - x_min) / (g29_grid_size - 1.0),
|
|
1668
|
+ dy = float(y_max - y_min) / (g29_grid_size - 1.0);
|
1693
|
1669
|
|
1694
|
|
- vector_3 normal = vector_3(lsf_results.A, lsf_results.B, 1.0000).get_normal();
|
|
1670
|
+ struct linear_fit_data lsf_results;
|
|
1671
|
+ incremental_LSF_reset(&lsf_results);
|
|
1672
|
+
|
|
1673
|
+ bool zig_zag = false;
|
|
1674
|
+ for (uint8_t ix = 0; ix < g29_grid_size; ix++) {
|
|
1675
|
+ const float x = float(x_min) + ix * dx;
|
|
1676
|
+ for (int8_t iy = 0; iy < g29_grid_size; iy++) {
|
|
1677
|
+ const float y = float(y_min) + dy * (zig_zag ? g29_grid_size - 1 - iy : iy);
|
|
1678
|
+ float measured_z = probe_pt(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y), parser.seen('E'), g29_verbose_level); // TODO: Needs error handling
|
|
1679
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
1680
|
+ if (DEBUGGING(LEVELING)) {
|
|
1681
|
+ SERIAL_CHAR('(');
|
|
1682
|
+ SERIAL_PROTOCOL_F(x, 7);
|
|
1683
|
+ SERIAL_CHAR(',');
|
|
1684
|
+ SERIAL_PROTOCOL_F(y, 7);
|
|
1685
|
+ SERIAL_ECHOPGM(") logical: ");
|
|
1686
|
+ SERIAL_CHAR('(');
|
|
1687
|
+ SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(x), 7);
|
|
1688
|
+ SERIAL_CHAR(',');
|
|
1689
|
+ SERIAL_PROTOCOL_F(LOGICAL_X_POSITION(y), 7);
|
|
1690
|
+ SERIAL_ECHOPGM(") measured: ");
|
|
1691
|
+ SERIAL_PROTOCOL_F(measured_z, 7);
|
|
1692
|
+ SERIAL_ECHOPGM(" correction: ");
|
|
1693
|
+ SERIAL_PROTOCOL_F(get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)), 7);
|
|
1694
|
+ }
|
|
1695
|
+ #endif
|
1695
|
1696
|
|
1696
|
|
- if (g29_verbose_level > 2) {
|
1697
|
|
- SERIAL_ECHOPGM("bed plane normal = [");
|
1698
|
|
- SERIAL_PROTOCOL_F(normal.x, 7);
|
1699
|
|
- SERIAL_PROTOCOLCHAR(',');
|
1700
|
|
- SERIAL_PROTOCOL_F(normal.y, 7);
|
1701
|
|
- SERIAL_PROTOCOLCHAR(',');
|
1702
|
|
- SERIAL_PROTOCOL_F(normal.z, 7);
|
1703
|
|
- SERIAL_ECHOLNPGM("]");
|
1704
|
|
- }
|
|
1697
|
+ measured_z -= get_z_correction(LOGICAL_X_POSITION(x), LOGICAL_Y_POSITION(y)) /* + zprobe_zoffset */ ;
|
1705
|
1698
|
|
1706
|
|
- matrix_3x3 rotation = matrix_3x3::create_look_at(vector_3(lsf_results.A, lsf_results.B, 1));
|
|
1699
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
1700
|
+ if (DEBUGGING(LEVELING)) {
|
|
1701
|
+ SERIAL_ECHOPGM(" final >>>---> ");
|
|
1702
|
+ SERIAL_PROTOCOL_F(measured_z, 7);
|
|
1703
|
+ SERIAL_EOL();
|
|
1704
|
+ }
|
|
1705
|
+ #endif
|
1707
|
1706
|
|
1708
|
|
- for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
|
1709
|
|
- for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
|
1710
|
|
- float x_tmp = mesh_index_to_xpos(i),
|
1711
|
|
- y_tmp = mesh_index_to_ypos(j),
|
1712
|
|
- z_tmp = z_values[i][j];
|
1713
|
|
-
|
1714
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
1715
|
|
- if (DEBUGGING(LEVELING)) {
|
1716
|
|
- SERIAL_ECHOPGM("before rotation = [");
|
1717
|
|
- SERIAL_PROTOCOL_F(x_tmp, 7);
|
1718
|
|
- SERIAL_PROTOCOLCHAR(',');
|
1719
|
|
- SERIAL_PROTOCOL_F(y_tmp, 7);
|
1720
|
|
- SERIAL_PROTOCOLCHAR(',');
|
1721
|
|
- SERIAL_PROTOCOL_F(z_tmp, 7);
|
1722
|
|
- SERIAL_ECHOPGM("] ---> ");
|
1723
|
|
- safe_delay(20);
|
1724
|
|
- }
|
1725
|
|
- #endif
|
|
1707
|
+ incremental_LSF(&lsf_results, x, y, measured_z);
|
|
1708
|
+ }
|
1726
|
1709
|
|
1727
|
|
- apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
|
1728
|
|
-
|
1729
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
1730
|
|
- if (DEBUGGING(LEVELING)) {
|
1731
|
|
- SERIAL_ECHOPGM("after rotation = [");
|
1732
|
|
- SERIAL_PROTOCOL_F(x_tmp, 7);
|
1733
|
|
- SERIAL_PROTOCOLCHAR(',');
|
1734
|
|
- SERIAL_PROTOCOL_F(y_tmp, 7);
|
1735
|
|
- SERIAL_PROTOCOLCHAR(',');
|
1736
|
|
- SERIAL_PROTOCOL_F(z_tmp, 7);
|
1737
|
|
- SERIAL_ECHOLNPGM("]");
|
1738
|
|
- safe_delay(55);
|
1739
|
|
- }
|
1740
|
|
- #endif
|
|
1710
|
+ zig_zag ^= true;
|
|
1711
|
+ }
|
1741
|
1712
|
|
1742
|
|
- z_values[i][j] += z_tmp - lsf_results.D;
|
|
1713
|
+ if (finish_incremental_LSF(&lsf_results)) {
|
|
1714
|
+ SERIAL_ECHOPGM("Could not complete LSF!");
|
|
1715
|
+ return;
|
1743
|
1716
|
}
|
1744
|
|
- }
|
1745
|
1717
|
|
1746
|
|
- #if ENABLED(DEBUG_LEVELING_FEATURE)
|
1747
|
|
- if (DEBUGGING(LEVELING)) {
|
1748
|
|
- rotation.debug(PSTR("rotation matrix:"));
|
|
1718
|
+ if (g29_verbose_level > 3) {
|
1749
|
1719
|
SERIAL_ECHOPGM("LSF Results A=");
|
1750
|
1720
|
SERIAL_PROTOCOL_F(lsf_results.A, 7);
|
1751
|
1721
|
SERIAL_ECHOPGM(" B=");
|
|
@@ -1753,21 +1723,87 @@
|
1753
|
1723
|
SERIAL_ECHOPGM(" D=");
|
1754
|
1724
|
SERIAL_PROTOCOL_F(lsf_results.D, 7);
|
1755
|
1725
|
SERIAL_EOL();
|
1756
|
|
- safe_delay(55);
|
|
1726
|
+ }
|
1757
|
1727
|
|
|
1728
|
+ vector_3 normal = vector_3(lsf_results.A, lsf_results.B, 1.0000).get_normal();
|
|
1729
|
+
|
|
1730
|
+ if (g29_verbose_level > 2) {
|
1758
|
1731
|
SERIAL_ECHOPGM("bed plane normal = [");
|
1759
|
1732
|
SERIAL_PROTOCOL_F(normal.x, 7);
|
1760
|
1733
|
SERIAL_PROTOCOLCHAR(',');
|
1761
|
1734
|
SERIAL_PROTOCOL_F(normal.y, 7);
|
1762
|
1735
|
SERIAL_PROTOCOLCHAR(',');
|
1763
|
1736
|
SERIAL_PROTOCOL_F(normal.z, 7);
|
1764
|
|
- SERIAL_ECHOPGM("]\n");
|
1765
|
|
- SERIAL_EOL();
|
|
1737
|
+ SERIAL_ECHOLNPGM("]");
|
1766
|
1738
|
}
|
1767
|
|
- #endif
|
1768
|
1739
|
|
1769
|
|
- if (do_ubl_mesh_map) display_map(g29_map_type);
|
1770
|
|
- }
|
|
1740
|
+ matrix_3x3 rotation = matrix_3x3::create_look_at(vector_3(lsf_results.A, lsf_results.B, 1));
|
|
1741
|
+
|
|
1742
|
+ for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
|
|
1743
|
+ for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
|
|
1744
|
+ float x_tmp = mesh_index_to_xpos(i),
|
|
1745
|
+ y_tmp = mesh_index_to_ypos(j),
|
|
1746
|
+ z_tmp = z_values[i][j];
|
|
1747
|
+
|
|
1748
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
1749
|
+ if (DEBUGGING(LEVELING)) {
|
|
1750
|
+ SERIAL_ECHOPGM("before rotation = [");
|
|
1751
|
+ SERIAL_PROTOCOL_F(x_tmp, 7);
|
|
1752
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
1753
|
+ SERIAL_PROTOCOL_F(y_tmp, 7);
|
|
1754
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
1755
|
+ SERIAL_PROTOCOL_F(z_tmp, 7);
|
|
1756
|
+ SERIAL_ECHOPGM("] ---> ");
|
|
1757
|
+ safe_delay(20);
|
|
1758
|
+ }
|
|
1759
|
+ #endif
|
|
1760
|
+
|
|
1761
|
+ apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
|
|
1762
|
+
|
|
1763
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
1764
|
+ if (DEBUGGING(LEVELING)) {
|
|
1765
|
+ SERIAL_ECHOPGM("after rotation = [");
|
|
1766
|
+ SERIAL_PROTOCOL_F(x_tmp, 7);
|
|
1767
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
1768
|
+ SERIAL_PROTOCOL_F(y_tmp, 7);
|
|
1769
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
1770
|
+ SERIAL_PROTOCOL_F(z_tmp, 7);
|
|
1771
|
+ SERIAL_ECHOLNPGM("]");
|
|
1772
|
+ safe_delay(55);
|
|
1773
|
+ }
|
|
1774
|
+ #endif
|
|
1775
|
+
|
|
1776
|
+ z_values[i][j] += z_tmp - lsf_results.D;
|
|
1777
|
+ }
|
|
1778
|
+ }
|
|
1779
|
+
|
|
1780
|
+ #if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
1781
|
+ if (DEBUGGING(LEVELING)) {
|
|
1782
|
+ rotation.debug(PSTR("rotation matrix:"));
|
|
1783
|
+ SERIAL_ECHOPGM("LSF Results A=");
|
|
1784
|
+ SERIAL_PROTOCOL_F(lsf_results.A, 7);
|
|
1785
|
+ SERIAL_ECHOPGM(" B=");
|
|
1786
|
+ SERIAL_PROTOCOL_F(lsf_results.B, 7);
|
|
1787
|
+ SERIAL_ECHOPGM(" D=");
|
|
1788
|
+ SERIAL_PROTOCOL_F(lsf_results.D, 7);
|
|
1789
|
+ SERIAL_EOL();
|
|
1790
|
+ safe_delay(55);
|
|
1791
|
+
|
|
1792
|
+ SERIAL_ECHOPGM("bed plane normal = [");
|
|
1793
|
+ SERIAL_PROTOCOL_F(normal.x, 7);
|
|
1794
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
1795
|
+ SERIAL_PROTOCOL_F(normal.y, 7);
|
|
1796
|
+ SERIAL_PROTOCOLCHAR(',');
|
|
1797
|
+ SERIAL_PROTOCOL_F(normal.z, 7);
|
|
1798
|
+ SERIAL_ECHOPGM("]\n");
|
|
1799
|
+ SERIAL_EOL();
|
|
1800
|
+ }
|
|
1801
|
+ #endif
|
|
1802
|
+
|
|
1803
|
+ if (do_ubl_mesh_map) display_map(g29_map_type);
|
|
1804
|
+ }
|
|
1805
|
+
|
|
1806
|
+ #endif // HAS_BED_PROBE
|
1771
|
1807
|
|
1772
|
1808
|
#if ENABLED(UBL_G29_P31)
|
1773
|
1809
|
void unified_bed_leveling::smart_fill_wlsf(const float &weight_factor) {
|