Browse Source

Patches to UBL

Scott Lahteine 7 years ago
parent
commit
15edb41cee
3 changed files with 191 additions and 217 deletions
  1. 2
    4
      Marlin/Marlin.h
  2. 76
    92
      Marlin/least_squares_fit.cpp
  3. 113
    121
      Marlin/ubl_G29.cpp

+ 2
- 4
Marlin/Marlin.h View File

@@ -299,10 +299,8 @@ float code_value_temp_diff();
299 299
 #endif
300 300
 
301 301
 #if ENABLED(AUTO_BED_LEVELING_UBL)
302
-struct linear_fit {
303
-	double A, B, D;
304
-};
305
-struct linear_fit *lsf_linear_fit( double *, double *, double *, int );
302
+  typedef struct { double A, B, D; } linear_fit;
303
+  linear_fit* lsf_linear_fit(double x[], double y[], double z[], const int);
306 304
 #endif
307 305
 
308 306
 #if PLANNER_LEVELING

+ 76
- 92
Marlin/least_squares_fit.cpp View File

@@ -23,111 +23,95 @@
23 23
 /**
24 24
  * Least Squares Best Fit  By Roxy and Ed Williams
25 25
  *
26
- * This algorythm is high speed and has a very small code footprint.
27
- * Its results are identical to both the Iterative Least Squares published
28
- * earlier by Roxy and the QR_SOLVE solution.   If used in place of QR_SOLVE
29
- * it saves roughly 10KB of program memory.
26
+ * This algorithm is high speed and has a very small code footprint.
27
+ * Its results are identical to both the Iterative Least-Squares published
28
+ * earlier by Roxy and the QR_SOLVE solution. If used in place of QR_SOLVE
29
+ * it saves roughly 10K of program memory.
30 30
  *
31 31
  */
32 32
 
33 33
 #include "MarlinConfig.h"
34 34
 
35
-#if ENABLED(AUTO_BED_LEVELING_UBL)	// Currently only used by UBL, but is applicable to Grid Based (Linear) Bed Leveling
36
-  #include <math.h>
37
-  #include "ubl.h"
38
-  #include "Marlin.h"
39
-
40
-double linear_fit_average(double *, int);
41
-double linear_fit_average_squared(double *, int);
42
-double linear_fit_average_mixed_terms(double *, double *, int );
43
-double linear_fit_average_product(double *matrix1, double *matrix2, int n);
44
-void   linear_fit_subtract_mean(double *matrix, double bar, int n);
45
-double linear_fit_max_abs(double *, int);
46
-
47
-struct linear_fit linear_fit_results;
48
-
49
-struct linear_fit *lsf_linear_fit(double *x, double *y, double *z, int n) {
50
-	double xbar, ybar, zbar;
51
-	double x2bar, y2bar;
52
-	double xybar, xzbar, yzbar;
53
-	double D;
54
-	int i;
55
-
56
-	linear_fit_results.A = 0.0;
57
-	linear_fit_results.B = 0.0;
58
-	linear_fit_results.D = 0.0;
59
-
60
-	xbar = linear_fit_average(x, n);
61
-	ybar = linear_fit_average(y, n);
62
-	zbar = linear_fit_average(z, n);
63
-
64
-	linear_fit_subtract_mean( x, xbar, n);
65
-	linear_fit_subtract_mean( y, ybar, n);
66
-	linear_fit_subtract_mean( z, zbar, n);
67
-
68
-	x2bar = linear_fit_average_product( x, x, n);
69
-	y2bar = linear_fit_average_product( y, y, n);
70
-	xybar =	linear_fit_average_product( x, y, n);
71
-	xzbar =	linear_fit_average_product( x, z, n);
72
-	yzbar =	linear_fit_average_product( y, z, n);
73
-
74
-	D = x2bar*y2bar - xybar*xybar;
75
-	for(i=0; i<n; i++) {
76
-		if (fabs(D) <= 1e-15*( linear_fit_max_abs(x, n) + linear_fit_max_abs(y, n))) {
77
-			printf( "error: x,y points are collinear at index:%d \n", i );
78
-			return NULL;
79
-		}
80
-	}
81
-
82
-	linear_fit_results.A = -(xzbar*y2bar - yzbar*xybar) / D;
83
-	linear_fit_results.B = -(yzbar*x2bar - xzbar*xybar) / D;
84
-//	linear_fit_results.D = -(zbar - linear_fit_results->A*xbar - linear_fit_results->B*ybar);
85
-	linear_fit_results.D = -(zbar + linear_fit_results.A*xbar + linear_fit_results.B*ybar);
86
-
87
-	return &linear_fit_results;
35
+#if ENABLED(AUTO_BED_LEVELING_UBL)  // Currently only used by UBL, but is applicable to Grid Based (Linear) Bed Leveling
36
+
37
+#include "ubl.h"
38
+#include "Marlin.h"
39
+#include "macros.h"
40
+#include <math.h>
41
+
42
+double linear_fit_average(double m[], const int);
43
+//double linear_fit_average_squared(double m[], const int);
44
+//double linear_fit_average_mixed_terms(double m1[], double m2[], const int);
45
+double linear_fit_average_product(double matrix1[], double matrix2[], const int n);
46
+void   linear_fit_subtract_mean(double matrix[], double bar, const int n);
47
+double linear_fit_max_abs(double m[], const int);
48
+
49
+linear_fit linear_fit_results;
50
+
51
+linear_fit* lsf_linear_fit(double x[], double y[], double z[], const int n) {
52
+  double xbar, ybar, zbar,
53
+         x2bar, y2bar,
54
+         xybar, xzbar, yzbar,
55
+         D;
56
+
57
+  linear_fit_results.A = 0.0;
58
+  linear_fit_results.B = 0.0;
59
+  linear_fit_results.D = 0.0;
60
+
61
+  xbar = linear_fit_average(x, n);
62
+  ybar = linear_fit_average(y, n);
63
+  zbar = linear_fit_average(z, n);
64
+
65
+  linear_fit_subtract_mean(x, xbar, n);
66
+  linear_fit_subtract_mean(y, ybar, n);
67
+  linear_fit_subtract_mean(z, zbar, n);
68
+
69
+  x2bar = linear_fit_average_product(x, x, n);
70
+  y2bar = linear_fit_average_product(y, y, n);
71
+  xybar = linear_fit_average_product(x, y, n);
72
+  xzbar = linear_fit_average_product(x, z, n);
73
+  yzbar = linear_fit_average_product(y, z, n);
74
+
75
+  D = x2bar * y2bar - xybar * xybar;
76
+  for (int i = 0; i < n; i++) {
77
+    if (fabs(D) <= 1e-15 * (linear_fit_max_abs(x, n) + linear_fit_max_abs(y, n))) {
78
+      printf("error: x,y points are collinear at index:%d\n", i);
79
+      return NULL;
80
+    }
81
+  }
82
+
83
+  linear_fit_results.A = -(xzbar * y2bar - yzbar * xybar) / D;
84
+  linear_fit_results.B = -(yzbar * x2bar - xzbar * xybar) / D;
85
+  // linear_fit_results.D = -(zbar - linear_fit_results->A * xbar - linear_fit_results->B * ybar);
86
+  linear_fit_results.D = -(zbar + linear_fit_results.A * xbar + linear_fit_results.B * ybar);
87
+
88
+  return &linear_fit_results;
88 89
 }
89 90
 
90
-
91
-
92
-
93
-double linear_fit_average(double *matrix, int n)
94
-{
95
-	int i;
96
-	double sum=0.0;
97
-
98
-	for (i = 0; i < n; i++)
99
-		sum += matrix[i];
100
-	return sum / (double) n;
91
+double linear_fit_average(double *matrix, const int n) {
92
+  double sum = 0.0;
93
+  for (int i = 0; i < n; i++)
94
+    sum += matrix[i];
95
+  return sum / (double)n;
101 96
 }
102 97
 
103
-double linear_fit_average_product(double *matrix1, double *matrix2, int n) {
104
-	int i;
105
-	double sum = 0.0;
106
-
107
-	for (i = 0; i < n; i++)
108
-		sum += matrix1[i] * matrix2[i];
109
-	return sum / (double) n;
98
+double linear_fit_average_product(double *matrix1, double *matrix2, const int n) {
99
+  double sum = 0.0;
100
+  for (int i = 0; i < n; i++)
101
+    sum += matrix1[i] * matrix2[i];
102
+  return sum / (double)n;
110 103
 }
111 104
 
112
-
113
-
114
-void linear_fit_subtract_mean(double *matrix, double bar, int n) {
115
-	int i;
116
-
117
-	for (i = 0; i < n; i++) {
118
-		matrix[i] -= bar;
119
-	}
120
-	return;
105
+void linear_fit_subtract_mean(double *matrix, double bar, const int n) {
106
+  for (int i = 0; i < n; i++)
107
+    matrix[i] -= bar;
121 108
 }
122 109
 
123
-double linear_fit_max_abs(double *matrix, int n) {
124
-	int i;
125
-	double max_abs = 0.0;
126
-
127
-	for(i=0; i<n; i++)
128
-		if ( max_abs < fabs(matrix[i]))
129
-			max_abs = fabs(matrix[i]);
130
-	return max_abs;
110
+double linear_fit_max_abs(double *matrix, const int n) {
111
+  double max_abs = 0.0;
112
+  for (int i = 0; i < n; i++)
113
+    NOLESS(max_abs, fabs(matrix[i]));
114
+  return max_abs;
131 115
 }
132 116
 #endif
133 117
 

+ 113
- 121
Marlin/ubl_G29.cpp View File

@@ -40,7 +40,7 @@
40 40
   bool lcd_clicked();
41 41
   void lcd_implementation_clear();
42 42
   void lcd_mesh_edit_setup(float initial);
43
-  void tilt_mesh_based_on_probed_grid( const bool );
43
+  void tilt_mesh_based_on_probed_grid(const bool);
44 44
   float lcd_mesh_edit();
45 45
   void lcd_z_offset_edit_setup(float);
46 46
   float lcd_z_offset_edit();
@@ -51,7 +51,7 @@
51 51
   extern bool code_has_value();
52 52
   extern float probe_pt(float x, float y, bool, int);
53 53
   extern bool set_probe_deployed(bool);
54
- 
54
+
55 55
   bool ProbeStay = true;
56 56
 
57 57
   constexpr float ubl_3_point_1_X = UBL_PROBE_PT_1_X,
@@ -175,7 +175,7 @@
175 175
    *                    area you are manually probing. Note that the command tries to start you in a corner
176 176
    *                    of the bed where movement will be predictable. You can force the location to be used in
177 177
    *                    the distance calculations by using the X and Y parameters. You may find it is helpful to
178
-   *                    print out a Mesh Map (G29 O ) to understand where the mesh is invalidated and where
178
+   *                    print out a Mesh Map (G29 O) to understand where the mesh is invalidated and where
179 179
    *                    the nozzle will need to move in order to complete the command. The C parameter is
180 180
    *                    available on the Phase 2 command also and indicates the search for points to measure should
181 181
    *                    be done based on the current location of the nozzle.
@@ -393,11 +393,11 @@
393 393
         SERIAL_PROTOCOLLNPGM("ERROR - grid size must be 2 or more");
394 394
         return;
395 395
       }
396
-      if (grid_size_G > GRID_MAX_POINTS_X || grid_size_G > GRID_MAX_POINTS_Y ) {
396
+      if (grid_size_G > GRID_MAX_POINTS_X || grid_size_G > GRID_MAX_POINTS_Y) {
397 397
         SERIAL_PROTOCOLLNPGM("ERROR - grid size can NOT exceed GRID_MAX_POINTS_X nor GRID_MAX_POINTS_Y");
398 398
         return;
399 399
       }
400
-      tilt_mesh_based_on_probed_grid( code_seen('O')||code_seen('M'));
400
+      tilt_mesh_based_on_probed_grid(code_seen('O') || code_seen('M'));
401 401
     }
402 402
 
403 403
     if (code_seen('P')) {
@@ -419,14 +419,14 @@
419 419
           //
420 420
           // Invalidate Entire Mesh and Automatically Probe Mesh in areas that can be reached by the probe
421 421
           //
422
-          if (!code_seen('C') ) {
422
+          if (!code_seen('C')) {
423 423
             ubl.invalidate();
424 424
             SERIAL_PROTOCOLLNPGM("Mesh invalidated. Probing mesh.\n");
425 425
           }
426 426
           if (g29_verbose_level > 1) {
427
-            SERIAL_ECHOPGM("Probing Mesh Points Closest to (");
428
-            SERIAL_ECHO(x_pos);
429
-            SERIAL_ECHOPAIR(",", y_pos);
427
+            SERIAL_PROTOCOLPAIR("Probing Mesh Points Closest to (", x_pos);
428
+            SERIAL_PROTOCOLCHAR(',');
429
+            SERIAL_PROTOCOL(y_pos);
430 430
             SERIAL_PROTOCOLLNPGM(")\n");
431 431
           }
432 432
           probe_entire_mesh(x_pos + X_PROBE_OFFSET_FROM_EXTRUDER, y_pos + Y_PROBE_OFFSET_FROM_EXTRUDER,
@@ -440,16 +440,16 @@
440 440
           SERIAL_PROTOCOLLNPGM("Manually probing unreachable mesh locations.\n");
441 441
           do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
442 442
           if (!x_flag && !y_flag) {      // use a good default location for the path
443
-            x_pos = X_MIN_POS;
444
-            y_pos = Y_MIN_POS;
445
-            if (X_PROBE_OFFSET_FROM_EXTRUDER > 0)   // The flipped > and < operators on these two comparisons is
446
-              x_pos = X_MAX_POS;                    // intentional. It should cause the probed points to follow a
447
-
448
-            if (Y_PROBE_OFFSET_FROM_EXTRUDER < 0)   // nice path on Cartesian printers. It may make sense to
449
-              y_pos = Y_MAX_POS;                    // have Delta printers default to the center of the bed.
443
+            // The flipped > and < operators on these two comparisons is
444
+            // intentional. It should cause the probed points to follow a
445
+            // nice path on Cartesian printers. It may make sense to
446
+            // have Delta printers default to the center of the bed.
447
+            // For now, until that is decided, it can be forced with the X
448
+            // and Y parameters.
449
+            x_pos = X_PROBE_OFFSET_FROM_EXTRUDER > 0 ? X_MAX_POS : X_MIN_POS;
450
+            y_pos = Y_PROBE_OFFSET_FROM_EXTRUDER < 0 ? Y_MAX_POS : Y_MIN_POS;
451
+          }
450 452
 
451
-          }                                         // For now, until that is decided, it can be forced with the X
452
-                                                    // and Y parameters.
453 453
           if (code_seen('C')) {
454 454
             x_pos = current_position[X_AXIS];
455 455
             y_pos = current_position[Y_AXIS];
@@ -674,7 +674,7 @@
674 674
           if (ELAPSED(millis(), nxt)) {
675 675
             SERIAL_PROTOCOLLNPGM("\nZ-Offset Adjustment Stopped.");
676 676
             do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
677
-            lcd_setstatuspgm("Z-Offset Stopped");
677
+            lcd_setstatuspgm(PSTR("Z-Offset Stopped"));
678 678
             restore_ubl_active_state_and_leave();
679 679
             goto LEAVE;
680 680
           }
@@ -693,7 +693,7 @@
693 693
 
694 694
     #if ENABLED(ULTRA_LCD)
695 695
       lcd_reset_alert_level();
696
-      lcd_setstatuspgm("");
696
+      lcd_setstatuspgm(PSTR(""));
697 697
       lcd_quick_feedback();
698 698
     #endif
699 699
 
@@ -773,7 +773,7 @@
773 773
         return;
774 774
       }
775 775
 
776
-      location = find_closest_mesh_point_of_type(INVALID, lx, ly, 1, NULL, do_furthest );  // the '1' says we want the location to be relative to the probe
776
+      location = find_closest_mesh_point_of_type(INVALID, lx, ly, 1, NULL, do_furthest);  // the '1' says we want the location to be relative to the probe
777 777
       if (location.x_index >= 0 && location.y_index >= 0) {
778 778
 
779 779
         const float rawx = ubl.mesh_index_to_xpos[location.x_index],
@@ -891,7 +891,7 @@
891 891
     SERIAL_PROTOCOLLNPGM("Place Shim Under Nozzle and Perform Measurement.");
892 892
     do_blocking_move_to_z(in_height);
893 893
     do_blocking_move_to_xy((float(X_MAX_POS) - float(X_MIN_POS)) / 2.0, (float(Y_MAX_POS) - float(Y_MIN_POS)) / 2.0);
894
-      //, min( planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS])/2.0);
894
+      //, min(planner.max_feedrate_mm_s[X_AXIS], planner.max_feedrate_mm_s[Y_AXIS])/2.0);
895 895
 
896 896
     const float z1 = use_encoder_wheel_to_measure_point();
897 897
     do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE);
@@ -997,7 +997,7 @@
997 997
 
998 998
   bool g29_parameter_parsing() {
999 999
     #if ENABLED(ULTRA_LCD)
1000
-      lcd_setstatuspgm("Doing G29 UBL!");
1000
+      lcd_setstatuspgm(PSTR("Doing G29 UBL!"));
1001 1001
       lcd_quick_feedback();
1002 1002
     #endif
1003 1003
 
@@ -1118,7 +1118,7 @@
1118 1118
     ubl_state_recursion_chk++;
1119 1119
     if (ubl_state_recursion_chk != 1) {
1120 1120
       SERIAL_ECHOLNPGM("save_ubl_active_state_and_disabled() called multiple times in a row.");
1121
-      lcd_setstatuspgm("save_UBL_active() error");
1121
+      lcd_setstatuspgm(PSTR("save_UBL_active() error"));
1122 1122
       lcd_quick_feedback();
1123 1123
       return;
1124 1124
     }
@@ -1129,7 +1129,7 @@
1129 1129
   void restore_ubl_active_state_and_leave() {
1130 1130
     if (--ubl_state_recursion_chk) {
1131 1131
       SERIAL_ECHOLNPGM("restore_ubl_active_state_and_leave() called too many times.");
1132
-      lcd_setstatuspgm("restore_UBL_active() error");
1132
+      lcd_setstatuspgm(PSTR("restore_UBL_active() error"));
1133 1133
       lcd_quick_feedback();
1134 1134
       return;
1135 1135
     }
@@ -1369,7 +1369,7 @@
1369 1369
     memset(not_done, 0xFF, sizeof(not_done));
1370 1370
 
1371 1371
     #if ENABLED(ULTRA_LCD)
1372
-      lcd_setstatuspgm("Fine Tuning Mesh");
1372
+      lcd_setstatuspgm(PSTR("Fine Tuning Mesh"));
1373 1373
     #endif
1374 1374
 
1375 1375
     do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
@@ -1377,7 +1377,7 @@
1377 1377
     do {
1378 1378
       if (do_ubl_mesh_map) ubl.display_map(map_type);
1379 1379
 
1380
-      location = find_closest_mesh_point_of_type( SET_IN_BITMAP, lx,  ly, 0, not_done, false); // The '0' says we want to use the nozzle's position
1380
+      location = find_closest_mesh_point_of_type(SET_IN_BITMAP, lx, ly, 0, not_done, false); // The '0' says we want to use the nozzle's position
1381 1381
                                                                                               // It doesn't matter if the probe can not reach this
1382 1382
                                                                                               // location. This is a manual edit of the Mesh Point.
1383 1383
       if (location.x_index < 0 && location.y_index < 0) continue; // abort if we can't find any more points.
@@ -1428,7 +1428,7 @@
1428 1428
           lcd_return_to_status();
1429 1429
           //SERIAL_PROTOCOLLNPGM("\nFine Tuning of Mesh Stopped.");
1430 1430
           do_blocking_move_to_z(Z_CLEARANCE_DEPLOY_PROBE);
1431
-          lcd_setstatuspgm("Mesh Editing Stopped");
1431
+          lcd_setstatuspgm(PSTR("Mesh Editing Stopped"));
1432 1432
 
1433 1433
           while (ubl_lcd_clicked()) idle();
1434 1434
 
@@ -1456,69 +1456,68 @@
1456 1456
     do_blocking_move_to_xy(lx, ly);
1457 1457
 
1458 1458
     #if ENABLED(ULTRA_LCD)
1459
-      lcd_setstatuspgm("Done Editing Mesh");
1459
+      lcd_setstatuspgm(PSTR("Done Editing Mesh"));
1460 1460
     #endif
1461 1461
     SERIAL_ECHOLNPGM("Done Editing Mesh");
1462 1462
   }
1463 1463
 
1464
-
1465
-  void tilt_mesh_based_on_probed_grid( const bool do_ubl_mesh_map) {
1466
-      int8_t grid_G_index_to_xpos[grid_size_G];  //  UBL MESH X index to be probed
1467
-      int8_t grid_G_index_to_ypos[grid_size_G];  //  UBL MESH Y index to be probed
1468
-      int8_t i, j ,k, xCount, yCount, G_X_index, G_Y_index;  // counter variables
1464
+  void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map) {
1465
+      int8_t grid_G_index_to_xpos[grid_size_G],  //  UBL MESH X index to be probed
1466
+             grid_G_index_to_ypos[grid_size_G],  //  UBL MESH Y index to be probed
1467
+             i, j ,k, xCount, yCount, G_X_index, G_Y_index;  // counter variables
1469 1468
       float z_values_G[grid_size_G][grid_size_G];
1470 1469
 
1471
-      struct linear_fit *results;
1470
+      linear_fit *results;
1472 1471
 
1473 1472
       for (G_Y_index = 0; G_Y_index < grid_size_G; G_Y_index++)
1474 1473
        for (G_X_index = 0; G_X_index < grid_size_G; G_X_index++)
1475 1474
         z_values_G[G_X_index][G_Y_index] = NAN;
1476
-      
1477
-      uint8_t x_min = GRID_MAX_POINTS_X - 1;
1478
-      uint8_t x_max = 0;
1479
-      uint8_t y_min = GRID_MAX_POINTS_Y - 1;
1480
-      uint8_t y_max = 0;
1481
-      
1475
+
1476
+      uint8_t x_min = GRID_MAX_POINTS_X - 1,
1477
+              x_max = 0,
1478
+              y_min = GRID_MAX_POINTS_Y - 1,
1479
+              y_max = 0;
1480
+
1482 1481
       //find min & max probeable points in the mesh
1483
-      for (xCount = 0; xCount < GRID_MAX_POINTS_X ; xCount++) {
1484
-        for (yCount = 0; yCount < GRID_MAX_POINTS_Y ; yCount++) {
1482
+      for (xCount = 0; xCount < GRID_MAX_POINTS_X; xCount++) {
1483
+        for (yCount = 0; yCount < GRID_MAX_POINTS_Y; yCount++) {
1485 1484
           if (WITHIN(ubl.mesh_index_to_xpos[xCount], MIN_PROBE_X, MAX_PROBE_X) && WITHIN(ubl.mesh_index_to_ypos[yCount], MIN_PROBE_Y, MAX_PROBE_Y)) {
1486
-            if (x_min > xCount) x_min = xCount;
1487
-            if (x_max < xCount) x_max = xCount;
1488
-            if (y_min > yCount) y_min = yCount;
1489
-            if (y_max < yCount) y_max = yCount;
1485
+            NOMORE(x_min, xCount);
1486
+            NOLESS(x_max, xCount);
1487
+            NOMORE(y_min, yCount);
1488
+            NOLESS(y_max, yCount);
1490 1489
           }
1491 1490
         }
1492 1491
       }
1493
-      
1494
-      if ((x_max - x_min + 1) < (grid_size_G) || (y_max - y_min + 1) < (grid_size_G)) {
1492
+
1493
+      if (x_max - x_min + 1 < grid_size_G || y_max - y_min + 1 < grid_size_G) {
1495 1494
         SERIAL_ECHOPAIR("ERROR - probeable UBL MESH smaller than grid - X points: ", x_max - x_min + 1);
1496 1495
         SERIAL_ECHOPAIR("  Y points: ", y_max - y_min + 1);
1497 1496
         SERIAL_ECHOLNPAIR("  grid: ", grid_size_G);
1498 1497
         return;
1499 1498
       }
1500
-      
1499
+
1501 1500
       // populate X matrix
1502 1501
       for (G_X_index = 0; G_X_index < grid_size_G; G_X_index++) {
1503
-        grid_G_index_to_xpos[G_X_index] = x_min + G_X_index * (x_max - x_min)/(grid_size_G - 1);
1504
-        if (G_X_index > 0 && grid_G_index_to_xpos[G_X_index - 1] == grid_G_index_to_xpos[G_X_index] ) {
1502
+        grid_G_index_to_xpos[G_X_index] = x_min + G_X_index * (x_max - x_min) / (grid_size_G - 1);
1503
+        if (G_X_index > 0 && grid_G_index_to_xpos[G_X_index - 1] == grid_G_index_to_xpos[G_X_index]) {
1505 1504
           grid_G_index_to_xpos[G_X_index] = grid_G_index_to_xpos[G_X_index - 1] + 1;
1506 1505
         }
1507 1506
       }
1508
-      
1507
+
1509 1508
       // populate Y matrix
1510 1509
       for (G_Y_index = 0; G_Y_index < grid_size_G; G_Y_index++) {
1511
-        grid_G_index_to_ypos[G_Y_index] = y_min + G_Y_index * (y_max - y_min)/(grid_size_G - 1);
1512
-        if (G_Y_index > 0 && grid_G_index_to_ypos[G_Y_index -1] == grid_G_index_to_ypos[G_Y_index] ) {
1510
+        grid_G_index_to_ypos[G_Y_index] = y_min + G_Y_index * (y_max - y_min) / (grid_size_G - 1);
1511
+        if (G_Y_index > 0 && grid_G_index_to_ypos[G_Y_index - 1] == grid_G_index_to_ypos[G_Y_index]) {
1513 1512
           grid_G_index_to_ypos[G_Y_index] = grid_G_index_to_ypos[G_Y_index - 1] + 1;
1514 1513
         }
1515 1514
       }
1516
-      
1515
+
1517 1516
       ubl.has_control_of_lcd_panel = true;
1518 1517
       save_ubl_active_state_and_disable();   // we don't do bed level correction because we want the raw data when we probe
1519
-      
1518
+
1520 1519
       DEPLOY_PROBE();
1521
-      
1520
+
1522 1521
       // this is a copy of the G29 AUTO_BED_LEVELING_BILINEAR method/code
1523 1522
       #undef PROBE_Y_FIRST
1524 1523
       #if ENABLED(PROBE_Y_FIRST)
@@ -1532,15 +1531,15 @@
1532 1531
         #define PR_INNER_VAR xCount
1533 1532
         #define PR_INNER_NUM grid_size_G
1534 1533
       #endif
1535
-      
1534
+
1536 1535
       bool zig = PR_OUTER_NUM & 1;  // Always end at RIGHT and BACK_PROBE_BED_POSITION
1537
-      
1536
+
1538 1537
       // Outer loop is Y with PROBE_Y_FIRST disabled
1539 1538
       for (PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_NUM; PR_OUTER_VAR++) {
1540
-        
1539
+
1541 1540
         int8_t inStart, inStop, inInc;
1542
-        
1543
-SERIAL_ECHOPAIR("\nPR_OUTER_VAR: ", PR_OUTER_VAR);
1541
+
1542
+        SERIAL_ECHOPAIR("\nPR_OUTER_VAR: ", PR_OUTER_VAR);
1544 1543
 
1545 1544
         if (zig) { // away from origin
1546 1545
           inStart = 0;
@@ -1552,87 +1551,80 @@ SERIAL_ECHOPAIR("\nPR_OUTER_VAR: ", PR_OUTER_VAR);
1552 1551
           inStop = -1;
1553 1552
           inInc = -1;
1554 1553
         }
1555
-        
1554
+
1556 1555
         zig = !zig; // zag
1557
-        
1556
+
1558 1557
         // Inner loop is Y with PROBE_Y_FIRST enabled
1559 1558
         for (PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; PR_INNER_VAR += inInc) {
1560
-SERIAL_ECHOPAIR("\nPR_INNER_VAR: ", PR_INNER_VAR);
1559
+          //SERIAL_ECHOPAIR("\nPR_INNER_VAR: ", PR_INNER_VAR);
1560
+
1561
+          //SERIAL_ECHOPAIR("\nCheckpoint: ", 1);
1561 1562
 
1562
-SERIAL_ECHOPAIR("\nCheckpoint: ", 1);
1563
-          
1564 1563
           // end of G29 AUTO_BED_LEVELING_BILINEAR method/code
1565 1564
           if (ubl_lcd_clicked()) {
1566
-SERIAL_ECHOPAIR("\nCheckpoint: ", 2);
1565
+            //SERIAL_ECHOPAIR("\nCheckpoint: ", 2);
1567 1566
             SERIAL_ECHOLNPGM("\nGrid only partially populated.\n");
1568 1567
             lcd_quick_feedback();
1569 1568
             STOW_PROBE();
1570
-SERIAL_ECHOPAIR("\nCheckpoint: ", 3);
1569
+            //SERIAL_ECHOPAIR("\nCheckpoint: ", 3);
1571 1570
             while (ubl_lcd_clicked()) idle();
1572
-SERIAL_ECHOPAIR("\nCheckpoint: ", 4);
1573
-              ubl.has_control_of_lcd_panel = false;
1574
-              restore_ubl_active_state_and_leave();
1575
-              safe_delay(50);  // Debounce the Encoder wheel
1576
-              return;
1577
-            }
1578
-SERIAL_ECHOPAIR("\nCheckpoint: ", 5);
1579
-          
1571
+            //SERIAL_ECHOPAIR("\nCheckpoint: ", 4);
1572
+            ubl.has_control_of_lcd_panel = false;
1573
+            restore_ubl_active_state_and_leave();
1574
+            safe_delay(50);  // Debounce the Encoder wheel
1575
+            return;
1576
+          }
1577
+          //SERIAL_ECHOPAIR("\nCheckpoint: ", 5);
1578
+
1580 1579
           const float probeX = ubl.mesh_index_to_xpos[grid_G_index_to_xpos[xCount]],  //where we want the probe to be
1581 1580
           probeY = ubl.mesh_index_to_ypos[grid_G_index_to_ypos[yCount]];
1582
-SERIAL_ECHOPAIR("\nCheckpoint: ", 6);
1583
-          
1584
-          const float measured_z = probe_pt(LOGICAL_X_POSITION(probeX), LOGICAL_Y_POSITION(probeY), code_seen('E'), (code_seen('V') && code_has_value()) ? code_value_int() : 0 );  // takes into account the offsets
1581
+          //SERIAL_ECHOPAIR("\nCheckpoint: ", 6);
1585 1582
 
1586
-SERIAL_ECHOPAIR("\nmeasured_z: ", measured_z );
1583
+          const float measured_z = probe_pt(LOGICAL_X_POSITION(probeX), LOGICAL_Y_POSITION(probeY), code_seen('E'), (code_seen('V') && code_has_value()) ? code_value_int() : 0);  // takes into account the offsets
1584
+
1585
+          //SERIAL_ECHOPAIR("\nmeasured_z: ", measured_z);
1587 1586
 
1588 1587
           z_values_G[xCount][yCount] = measured_z;
1589
-//SERIAL_LNPGM("\nFine Tuning of Mesh Stopped.");
1588
+          //SERIAL_ECHOLNPGM("\nFine Tuning of Mesh Stopped.");
1590 1589
         }
1591 1590
       }
1592
-      
1593
-SERIAL_ECHO("\nDone probing...\n");
1591
+      //SERIAL_ECHOLNPGM("\nDone probing...\n");
1594 1592
 
1595 1593
       STOW_PROBE();
1596 1594
       restore_ubl_active_state_and_leave();
1597 1595
 
1598
-// ??      ubl.has_control_of_lcd_panel = true;
1599
-//    do_blocking_move_to_xy(ubl.mesh_index_to_xpos[grid_G_index_to_xpos[0]], ubl.mesh_index_to_ypos[grid_G_index_to_ypos[0]]);
1600
-      
1596
+      // ?? ubl.has_control_of_lcd_panel = true;
1597
+      //do_blocking_move_to_xy(ubl.mesh_index_to_xpos[grid_G_index_to_xpos[0]], ubl.mesh_index_to_ypos[grid_G_index_to_ypos[0]]);
1598
+
1601 1599
       // least squares code
1602
-double xxx9[] = { 0,50,100,150,200,           20,70,120,165,195,         0,50,100,150,200,           0,55,100,150,200,           0,65,100,150,205 };
1603
-double yyy9[] = { 0, 1,  2,  3, 4,            50, 51,  52,  53, 54,     100, 101,102,103,104,        150,151,152,153,154,        200,201,202,203,204 };
1604
-double zzz9[] = { 0.01,.002,-.01,-.02,0,      0.01,.002,-.01,-.02,0,     0.01,.002,-.01,-.02,0,      0.01,.002,-.01,-.02,0,      0.01,.002,-.01,-.012,0.01};
1605
-int nine_size = sizeof(xxx9) / sizeof(double);
1606
-
1607
-double xxx0[] = { 0.0, 0.0, 1.0 };	// Expect [0,0,0.1,0]
1608
-double yyy0[] = { 0.0, 1.0, 0.0 };
1609
-double zzz0[] = { 0.1, 0.1, 0.1 };
1610
-int zero_size = sizeof(xxx0) / sizeof(double);
1611
-
1612
-double xxx[] = { 0.0, 0.0, 1.0, 1.0 };	// Expect [0.1,0,0.05,0]
1613
-double yyy[] = { 0.0, 1.0, 0.0, 1.0 };
1614
-double zzz[] = { 0.05, 0.05, 0.15, 0.15 };      
1615
-int three_size = sizeof(xxx) / sizeof(double);
1616
-
1617
-      results = lsf_linear_fit(xxx9, yyy9, zzz9, nine_size);
1618
-SERIAL_ECHOPAIR("\nxxx9->A =", results->A); 
1619
-SERIAL_ECHOPAIR("\nxxx9->B =", results->B); 
1620
-SERIAL_ECHOPAIR("\nxxx9->D =", results->D); 
1621
-SERIAL_ECHO("\n");
1622
-
1623
-      results = lsf_linear_fit(xxx0, yyy0, zzz0, zero_size);
1624
-SERIAL_ECHOPAIR("\nxxx0->A =", results->A); 
1625
-SERIAL_ECHOPAIR("\nxxx0->B =", results->B); 
1626
-SERIAL_ECHOPAIR("\nxxx0->D =", results->D); 
1627
-SERIAL_ECHO("\n");
1628
-
1629
-      results = lsf_linear_fit(xxx, yyy, zzz, three_size);
1630
-SERIAL_ECHOPAIR("\nxxx->A =", results->A); 
1631
-SERIAL_ECHOPAIR("\nxxx->B =", results->B); 
1632
-SERIAL_ECHOPAIR("\nxxx->D =", results->D); 
1633
-SERIAL_ECHO("\n");
1600
+      double xxx9[] = { 0,50,100,150,200,           20,70,120,165,195,         0,50,100,150,200,           0,55,100,150,200,           0,65,100,150,205 },
1601
+             yyy9[] = { 0, 1,  2,  3, 4,            50, 51,  52,  53, 54,     100, 101,102,103,104,        150,151,152,153,154,        200,201,202,203,204 },
1602
+             zzz9[] = { 0.01,.002,-.01,-.02,0,      0.01,.002,-.01,-.02,0,     0.01,.002,-.01,-.02,0,      0.01,.002,-.01,-.02,0,      0.01,.002,-.01,-.012,0.01},
1603
+             xxx0[] = { 0.0, 0.0, 1.0 },  // Expect [0,0,0.1,0]
1604
+             yyy0[] = { 0.0, 1.0, 0.0 },
1605
+             zzz0[] = { 0.1, 0.1, 0.1 },
1606
+             xxx[] = { 0.0, 0.0, 1.0, 1.0 },  // Expect [0.1,0,0.05,0]
1607
+             yyy[] = { 0.0, 1.0, 0.0, 1.0 },
1608
+             zzz[] = { 0.05, 0.05, 0.15, 0.15 };
1609
+
1610
+      results = lsf_linear_fit(xxx9, yyy9, zzz9, COUNT(xxx9));
1611
+      SERIAL_ECHOPAIR("\nxxx9->A =", results->A);
1612
+      SERIAL_ECHOPAIR("\nxxx9->B =", results->B);
1613
+      SERIAL_ECHOPAIR("\nxxx9->D =", results->D);
1614
+      SERIAL_EOL;
1615
+
1616
+      results = lsf_linear_fit(xxx0, yyy0, zzz0, COUNT(xxx0));
1617
+      SERIAL_ECHOPAIR("\nxxx0->A =", results->A);
1618
+      SERIAL_ECHOPAIR("\nxxx0->B =", results->B);
1619
+      SERIAL_ECHOPAIR("\nxxx0->D =", results->D);
1620
+      SERIAL_EOL;
1621
+
1622
+      results = lsf_linear_fit(xxx, yyy, zzz, COUNT(xxx));
1623
+      SERIAL_ECHOPAIR("\nxxx->A =", results->A);
1624
+      SERIAL_ECHOPAIR("\nxxx->B =", results->B);
1625
+      SERIAL_ECHOPAIR("\nxxx->D =", results->D);
1626
+      SERIAL_EOL;
1634 1627
 
1635
-      return;
1636 1628
     }  // end of tilt_mesh_based_on_probed_grid()
1637 1629
 
1638 1630
 #endif // AUTO_BED_LEVELING_UBL

Loading…
Cancel
Save