|
@@ -1200,22 +1200,24 @@ static void retract_z_probe() {
|
1200
|
1200
|
#endif
|
1201
|
1201
|
}
|
1202
|
1202
|
|
|
1203
|
+enum ProbeAction { ProbeEngageRetract, ProbeEngage, ProbeStay, ProbeRetract };
|
|
1204
|
+
|
1203
|
1205
|
/// Probe bed height at position (x,y), returns the measured z value
|
1204
|
|
-static float probe_pt(float x, float y, float z_before, int retract_action=0) {
|
|
1206
|
+static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeEngageRetract) {
|
1205
|
1207
|
// move to right place
|
1206
|
1208
|
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
|
1207
|
1209
|
do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
|
1208
|
1210
|
|
1209
|
|
-#ifndef Z_PROBE_SLED
|
1210
|
|
- if ((retract_action==0) || (retract_action==1))
|
1211
|
|
- engage_z_probe(); // Engage Z Servo endstop if available
|
1212
|
|
-#endif // Z_PROBE_SLED
|
|
1211
|
+ #ifndef Z_PROBE_SLED
|
|
1212
|
+ if (retract_action == ProbeEngageRetract || retract_action == ProbeEngage) engage_z_probe();
|
|
1213
|
+ #endif
|
|
1214
|
+
|
1213
|
1215
|
run_z_probe();
|
1214
|
1216
|
float measured_z = current_position[Z_AXIS];
|
1215
|
|
-#ifndef Z_PROBE_SLED
|
1216
|
|
- if ((retract_action==0) || (retract_action==3))
|
1217
|
|
- retract_z_probe();
|
1218
|
|
-#endif // Z_PROBE_SLED
|
|
1217
|
+
|
|
1218
|
+ #ifndef Z_PROBE_SLED
|
|
1219
|
+ if (retract_action == ProbeEngageRetract || retract_action == ProbeRetract) retract_z_probe();
|
|
1220
|
+ #endif
|
1219
|
1221
|
|
1220
|
1222
|
SERIAL_PROTOCOLPGM(MSG_BED);
|
1221
|
1223
|
SERIAL_PROTOCOLPGM(" x: ");
|
|
@@ -1376,6 +1378,11 @@ void refresh_cmd_timeout(void)
|
1376
|
1378
|
#endif //FWRETRACT
|
1377
|
1379
|
|
1378
|
1380
|
#ifdef Z_PROBE_SLED
|
|
1381
|
+
|
|
1382
|
+ #ifndef SLED_DOCKING_OFFSET
|
|
1383
|
+ #define SLED_DOCKING_OFFSET 0
|
|
1384
|
+ #endif
|
|
1385
|
+
|
1379
|
1386
|
//
|
1380
|
1387
|
// Method to dock/undock a sled designed by Charles Bell.
|
1381
|
1388
|
//
|
|
@@ -1719,193 +1726,286 @@ void process_commands()
|
1719
|
1726
|
break;
|
1720
|
1727
|
|
1721
|
1728
|
#ifdef ENABLE_AUTO_BED_LEVELING
|
|
1729
|
+
|
|
1730
|
+ #if Z_MIN_PIN == -1
|
|
1731
|
+ #error "You must have a Z_MIN endstop in order to enable Auto Bed Leveling!!! Z_MIN_PIN must point to a valid hardware pin."
|
|
1732
|
+ #endif
|
|
1733
|
+
|
|
1734
|
+ /**
|
|
1735
|
+ * Enhanced G29 Auto Bed Leveling Probe Routine
|
|
1736
|
+ *
|
|
1737
|
+ * Parameters With AUTO_BED_LEVELING_GRID:
|
|
1738
|
+ *
|
|
1739
|
+ * P Set the size of the grid that will be probed (P x P points).
|
|
1740
|
+ * Example: "G29 P4"
|
|
1741
|
+ *
|
|
1742
|
+ * V Set the verbose level (0-4). Example: "G29 V3"
|
|
1743
|
+ *
|
|
1744
|
+ * T Generate a Bed Topology Report. Example: "G29 P5 T" for a detailed report.
|
|
1745
|
+ * This is useful for manual bed leveling and finding flaws in the bed (to
|
|
1746
|
+ * assist with part placement).
|
|
1747
|
+ *
|
|
1748
|
+ * F Set the Front limit of the probing grid
|
|
1749
|
+ * B Set the Back limit of the probing grid
|
|
1750
|
+ * L Set the Left limit of the probing grid
|
|
1751
|
+ * R Set the Right limit of the probing grid
|
|
1752
|
+ *
|
|
1753
|
+ * Global Parameters:
|
|
1754
|
+ *
|
|
1755
|
+ * E/e By default G29 engages / disengages the probe for each point.
|
|
1756
|
+ * Include "E" to engage and disengage the probe just once.
|
|
1757
|
+ * There's no extra effect if you have a fixed probe.
|
|
1758
|
+ * Usage: "G29 E" or "G29 e"
|
|
1759
|
+ *
|
|
1760
|
+ */
|
|
1761
|
+
|
1722
|
1762
|
case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
|
1723
|
|
- // Override probing area by providing [F]ront [B]ack [L]eft [R]ight Grid[P]oints values
|
1724
|
|
- {
|
1725
|
|
- #if Z_MIN_PIN == -1
|
1726
|
|
- #error "You must have a Z_MIN endstop in order to enable Auto Bed Leveling feature!!! Z_MIN_PIN must point to a valid hardware pin."
|
1727
|
|
- #endif
|
|
1763
|
+ {
|
|
1764
|
+ // Use one of these defines to specify the origin
|
|
1765
|
+ // for a topographical map to be printed for your bed.
|
|
1766
|
+ #define ORIGIN_BACK_LEFT 1
|
|
1767
|
+ #define ORIGIN_FRONT_RIGHT 2
|
|
1768
|
+ #define ORIGIN_BACK_RIGHT 3
|
|
1769
|
+ #define ORIGIN_FRONT_LEFT 4
|
|
1770
|
+ #define TOPO_ORIGIN ORIGIN_FRONT_LEFT
|
|
1771
|
+
|
|
1772
|
+ // Prevent user from running a G29 without first homing in X and Y
|
|
1773
|
+ if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS])) {
|
|
1774
|
+ LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
|
|
1775
|
+ SERIAL_ECHO_START;
|
|
1776
|
+ SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
|
|
1777
|
+ break; // abort G29, since we don't know where we are
|
|
1778
|
+ }
|
1728
|
1779
|
|
1729
|
|
- // Prevent user from running a G29 without first homing in X and Y
|
1730
|
|
- if (! (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) )
|
1731
|
|
- {
|
1732
|
|
- LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
|
1733
|
|
- SERIAL_ECHO_START;
|
1734
|
|
- SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
|
1735
|
|
- break; // abort G29, since we don't know where we are
|
1736
|
|
- }
|
|
1780
|
+ bool enhanced_g29 = code_seen('E') || code_seen('e');
|
1737
|
1781
|
|
1738
|
|
-#ifdef Z_PROBE_SLED
|
1739
|
|
- dock_sled(false);
|
1740
|
|
-#endif // Z_PROBE_SLED
|
1741
|
|
- st_synchronize();
|
1742
|
|
- // make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
|
1743
|
|
- //vector_3 corrected_position = plan_get_position_mm();
|
1744
|
|
- //corrected_position.debug("position before G29");
|
1745
|
|
- plan_bed_level_matrix.set_to_identity();
|
1746
|
|
- vector_3 uncorrected_position = plan_get_position();
|
1747
|
|
- //uncorrected_position.debug("position durring G29");
|
1748
|
|
- current_position[X_AXIS] = uncorrected_position.x;
|
1749
|
|
- current_position[Y_AXIS] = uncorrected_position.y;
|
1750
|
|
- current_position[Z_AXIS] = uncorrected_position.z;
|
1751
|
|
- plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
1752
|
|
- setup_for_endstop_move();
|
|
1782
|
+ #ifdef AUTO_BED_LEVELING_GRID
|
1753
|
1783
|
|
1754
|
|
- feedrate = homing_feedrate[Z_AXIS];
|
1755
|
|
-#ifdef AUTO_BED_LEVELING_GRID
|
1756
|
|
- // probe at the points of a lattice grid
|
1757
|
|
- int left_probe_bed_position=LEFT_PROBE_BED_POSITION;
|
1758
|
|
- int right_probe_bed_position=RIGHT_PROBE_BED_POSITION;
|
1759
|
|
- int back_probe_bed_position=BACK_PROBE_BED_POSITION;
|
1760
|
|
- int front_probe_bed_position=FRONT_PROBE_BED_POSITION;
|
1761
|
|
- int auto_bed_leveling_grid_points=AUTO_BED_LEVELING_GRID_POINTS;
|
1762
|
|
- if (code_seen('L')) left_probe_bed_position=(int)code_value();
|
1763
|
|
- if (code_seen('R')) right_probe_bed_position=(int)code_value();
|
1764
|
|
- if (code_seen('B')) back_probe_bed_position=(int)code_value();
|
1765
|
|
- if (code_seen('F')) front_probe_bed_position=(int)code_value();
|
1766
|
|
- if (code_seen('P')) auto_bed_leveling_grid_points=(int)code_value();
|
|
1784
|
+ // Example Syntax: G29 N4 V2 E T
|
|
1785
|
+ int verbose_level = 1;
|
1767
|
1786
|
|
1768
|
|
- int xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points-1);
|
1769
|
|
- int yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points-1);
|
|
1787
|
+ bool topo_flag = code_seen('T') || code_seen('t');
|
1770
|
1788
|
|
|
1789
|
+ if (code_seen('V') || code_seen('v')) {
|
|
1790
|
+ verbose_level = code_value();
|
|
1791
|
+ if (verbose_level < 0 || verbose_level > 4) {
|
|
1792
|
+ SERIAL_PROTOCOLPGM("?Verbose Level not plausible (0-4).\n");
|
|
1793
|
+ break;
|
|
1794
|
+ }
|
|
1795
|
+ if (verbose_level > 0) {
|
|
1796
|
+ SERIAL_PROTOCOLPGM("Enhanced G29 Auto_Bed_Leveling Code V1.25:\n");
|
|
1797
|
+ SERIAL_PROTOCOLPGM("Full support at http://3dprintboard.com\n");
|
|
1798
|
+ if (verbose_level > 2) topo_flag = true;
|
|
1799
|
+ }
|
|
1800
|
+ }
|
1771
|
1801
|
|
1772
|
|
- // solve the plane equation ax + by + d = z
|
1773
|
|
- // A is the matrix with rows [x y 1] for all the probed points
|
1774
|
|
- // B is the vector of the Z positions
|
1775
|
|
- // the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
|
1776
|
|
- // so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
|
|
1802
|
+ int auto_bed_leveling_grid_points = code_seen('P') ? code_value_long() : AUTO_BED_LEVELING_GRID_POINTS;
|
|
1803
|
+ if (auto_bed_leveling_grid_points < 2 || auto_bed_leveling_grid_points > AUTO_BED_LEVELING_GRID_POINTS) {
|
|
1804
|
+ SERIAL_PROTOCOLPGM("?Number of probed points not plausible (2 minimum).\n");
|
|
1805
|
+ break;
|
|
1806
|
+ }
|
1777
|
1807
|
|
1778
|
|
- // "A" matrix of the linear system of equations
|
1779
|
|
- double eqnAMatrix[auto_bed_leveling_grid_points*auto_bed_leveling_grid_points*3];
|
|
1808
|
+ int left_probe_bed_position = code_seen('L') ? code_value_long() : LEFT_PROBE_BED_POSITION;
|
|
1809
|
+ int right_probe_bed_position = code_seen('R') ? code_value_long() : RIGHT_PROBE_BED_POSITION;
|
|
1810
|
+ int back_probe_bed_position = code_seen('B') ? code_value_long() : BACK_PROBE_BED_POSITION;
|
|
1811
|
+ int front_probe_bed_position = code_seen('F') ? code_value_long() : FRONT_PROBE_BED_POSITION;
|
1780
|
1812
|
|
1781
|
|
- // "B" vector of Z points
|
1782
|
|
- double eqnBVector[auto_bed_leveling_grid_points*auto_bed_leveling_grid_points];
|
|
1813
|
+ #endif
|
1783
|
1814
|
|
|
1815
|
+ #ifdef Z_PROBE_SLED
|
|
1816
|
+ dock_sled(false); // engage (un-dock) the probe
|
|
1817
|
+ #endif
|
1784
|
1818
|
|
|
1819
|
+ st_synchronize();
|
|
1820
|
+ // make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
|
|
1821
|
+ //vector_3 corrected_position = plan_get_position_mm();
|
|
1822
|
+ //corrected_position.debug("position before G29");
|
|
1823
|
+ plan_bed_level_matrix.set_to_identity();
|
|
1824
|
+ vector_3 uncorrected_position = plan_get_position();
|
|
1825
|
+ //uncorrected_position.debug("position durring G29");
|
|
1826
|
+ current_position[X_AXIS] = uncorrected_position.x;
|
|
1827
|
+ current_position[Y_AXIS] = uncorrected_position.y;
|
|
1828
|
+ current_position[Z_AXIS] = uncorrected_position.z;
|
|
1829
|
+ plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
|
1830
|
+ setup_for_endstop_move();
|
1785
|
1831
|
|
1786
|
|
- int probePointCounter = 0;
|
1787
|
|
- bool zig = true;
|
|
1832
|
+ feedrate = homing_feedrate[Z_AXIS];
|
1788
|
1833
|
|
1789
|
|
- for (int yProbe=front_probe_bed_position; yProbe <= back_probe_bed_position; yProbe += yGridSpacing)
|
|
1834
|
+ #ifdef AUTO_BED_LEVELING_GRID
|
|
1835
|
+ // probe at the points of a lattice grid
|
1790
|
1836
|
|
1791
|
|
- {
|
1792
|
|
- int xProbe, xInc;
|
1793
|
|
- if (zig)
|
1794
|
|
- {
|
1795
|
|
- xProbe = left_probe_bed_position;
|
1796
|
|
- //xEnd = right_probe_bed_position;
|
1797
|
|
- xInc = xGridSpacing;
|
1798
|
|
- zig = false;
|
1799
|
|
- } else // zag
|
1800
|
|
- {
|
1801
|
|
- xProbe = right_probe_bed_position;
|
1802
|
|
- //xEnd = left_probe_bed_position;
|
1803
|
|
- xInc = -xGridSpacing;
|
1804
|
|
- zig = true;
|
1805
|
|
- }
|
|
1837
|
+ int xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points - 1);
|
|
1838
|
+ int yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points - 1);
|
1806
|
1839
|
|
1807
|
|
- for (int xCount=0; xCount < auto_bed_leveling_grid_points; xCount++)
|
1808
|
|
- {
|
1809
|
|
- float z_before;
|
1810
|
|
- if (probePointCounter == 0)
|
1811
|
|
- {
|
1812
|
|
- // raise before probing
|
1813
|
|
- z_before = Z_RAISE_BEFORE_PROBING;
|
1814
|
|
- } else
|
1815
|
|
- {
|
1816
|
|
- // raise extruder
|
1817
|
|
- z_before = current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS;
|
1818
|
|
- }
|
1819
|
|
-
|
1820
|
|
- float measured_z;
|
1821
|
|
- //Enhanced G29 - Do not retract servo between probes
|
1822
|
|
- if (code_seen('E') || code_seen('e') )
|
1823
|
|
- {
|
1824
|
|
- if ((yProbe==FRONT_PROBE_BED_POSITION) && (xCount==0))
|
1825
|
|
- {
|
1826
|
|
- measured_z = probe_pt(xProbe, yProbe, z_before,1);
|
1827
|
|
- } else if ((yProbe==FRONT_PROBE_BED_POSITION + (yGridSpacing * (AUTO_BED_LEVELING_GRID_POINTS-1))) && (xCount == AUTO_BED_LEVELING_GRID_POINTS-1))
|
1828
|
|
- {
|
1829
|
|
- measured_z = probe_pt(xProbe, yProbe, z_before,3);
|
1830
|
|
- } else {
|
1831
|
|
- measured_z = probe_pt(xProbe, yProbe, z_before,2);
|
1832
|
|
- }
|
1833
|
|
- } else {
|
1834
|
|
- measured_z = probe_pt(xProbe, yProbe, z_before);
|
1835
|
|
- }
|
1836
|
|
-
|
1837
|
|
- eqnBVector[probePointCounter] = measured_z;
|
1838
|
|
-
|
1839
|
|
- eqnAMatrix[probePointCounter + 0*auto_bed_leveling_grid_points*auto_bed_leveling_grid_points] = xProbe;
|
1840
|
|
- eqnAMatrix[probePointCounter + 1*auto_bed_leveling_grid_points*auto_bed_leveling_grid_points] = yProbe;
|
1841
|
|
- eqnAMatrix[probePointCounter + 2*auto_bed_leveling_grid_points*auto_bed_leveling_grid_points] = 1;
|
1842
|
|
- probePointCounter++;
|
1843
|
|
- xProbe += xInc;
|
1844
|
|
- }
|
|
1840
|
+ // solve the plane equation ax + by + d = z
|
|
1841
|
+ // A is the matrix with rows [x y 1] for all the probed points
|
|
1842
|
+ // B is the vector of the Z positions
|
|
1843
|
+ // the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
|
|
1844
|
+ // so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
|
|
1845
|
+
|
|
1846
|
+ int abl2 = auto_bed_leveling_grid_points * auto_bed_leveling_grid_points;
|
|
1847
|
+
|
|
1848
|
+ double eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations
|
|
1849
|
+ eqnBVector[abl2], // "B" vector of Z points
|
|
1850
|
+ mean = 0.0;
|
|
1851
|
+
|
|
1852
|
+ int probePointCounter = 0;
|
|
1853
|
+ bool zig = true;
|
|
1854
|
+
|
|
1855
|
+ for (int yProbe = front_probe_bed_position; yProbe <= back_probe_bed_position; yProbe += yGridSpacing) {
|
|
1856
|
+ int xProbe, xInc;
|
|
1857
|
+
|
|
1858
|
+ if (zig)
|
|
1859
|
+ xProbe = left_probe_bed_position, xInc = xGridSpacing;
|
|
1860
|
+ else
|
|
1861
|
+ xProbe = right_probe_bed_position, xInc = -xGridSpacing;
|
|
1862
|
+
|
|
1863
|
+ // If topo_flag is set then don't zig-zag. Just scan in one direction.
|
|
1864
|
+ // This gets the probe points in more readable order.
|
|
1865
|
+ if (!topo_flag) zig = !zig;
|
|
1866
|
+
|
|
1867
|
+ for (int xCount = 0; xCount < auto_bed_leveling_grid_points; xCount++) {
|
|
1868
|
+ // raise extruder
|
|
1869
|
+ float z_before = probePointCounter == 0 ? Z_RAISE_BEFORE_PROBING : current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS,
|
|
1870
|
+ measured_z;
|
|
1871
|
+
|
|
1872
|
+ // Enhanced G29 - Do not retract servo between probes
|
|
1873
|
+ ProbeAction act;
|
|
1874
|
+ if (enhanced_g29) {
|
|
1875
|
+ if (yProbe == front_probe_bed_position && xCount == 0)
|
|
1876
|
+ act = ProbeEngage;
|
|
1877
|
+ else if (yProbe == front_probe_bed_position + (yGridSpacing * (auto_bed_leveling_grid_points - 1)) && xCount == auto_bed_leveling_grid_points - 1)
|
|
1878
|
+ act = ProbeRetract;
|
|
1879
|
+ else
|
|
1880
|
+ act = ProbeStay;
|
1845
|
1881
|
}
|
1846
|
|
- clean_up_after_endstop_move();
|
|
1882
|
+ else
|
|
1883
|
+ act = ProbeEngageRetract;
|
1847
|
1884
|
|
1848
|
|
- // solve lsq problem
|
1849
|
|
- double *plane_equation_coefficients = qr_solve(auto_bed_leveling_grid_points*auto_bed_leveling_grid_points, 3, eqnAMatrix, eqnBVector);
|
|
1885
|
+ measured_z = probe_pt(xProbe, yProbe, z_before, act);
|
1850
|
1886
|
|
1851
|
|
- SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
|
1852
|
|
- SERIAL_PROTOCOL(plane_equation_coefficients[0]);
|
1853
|
|
- SERIAL_PROTOCOLPGM(" b: ");
|
1854
|
|
- SERIAL_PROTOCOL(plane_equation_coefficients[1]);
|
1855
|
|
- SERIAL_PROTOCOLPGM(" d: ");
|
1856
|
|
- SERIAL_PROTOCOLLN(plane_equation_coefficients[2]);
|
|
1887
|
+ mean += measured_z;
|
1857
|
1888
|
|
|
1889
|
+ eqnBVector[probePointCounter] = measured_z;
|
|
1890
|
+ eqnAMatrix[probePointCounter + 0 * abl2] = xProbe;
|
|
1891
|
+ eqnAMatrix[probePointCounter + 1 * abl2] = yProbe;
|
|
1892
|
+ eqnAMatrix[probePointCounter + 2 * abl2] = 1;
|
1858
|
1893
|
|
1859
|
|
- set_bed_level_equation_lsq(plane_equation_coefficients);
|
|
1894
|
+ probePointCounter++;
|
|
1895
|
+ xProbe += xInc;
|
1860
|
1896
|
|
1861
|
|
- free(plane_equation_coefficients);
|
|
1897
|
+ } //xProbe
|
1862
|
1898
|
|
1863
|
|
-#else // AUTO_BED_LEVELING_GRID not defined
|
|
1899
|
+ } //yProbe
|
1864
|
1900
|
|
1865
|
|
- // Probe at 3 arbitrary points
|
1866
|
|
- // Enhanced G29
|
1867
|
|
-
|
1868
|
|
- float z_at_pt_1, z_at_pt_2, z_at_pt_3;
|
1869
|
|
-
|
1870
|
|
- if (code_seen('E') || code_seen('e')) {
|
1871
|
|
- // probe 1
|
1872
|
|
- z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING,1);
|
1873
|
|
- // probe 2
|
1874
|
|
- z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS,2);
|
1875
|
|
- // probe 3
|
1876
|
|
- z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS,3);
|
1877
|
|
- }
|
1878
|
|
- else {
|
1879
|
|
- // probe 1
|
1880
|
|
- z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING);
|
1881
|
|
- // probe 2
|
1882
|
|
- z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
|
1883
|
|
- // probe 3
|
1884
|
|
- z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
|
1885
|
|
- }
|
1886
|
|
- clean_up_after_endstop_move();
|
1887
|
|
- set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
|
|
1901
|
+ clean_up_after_endstop_move();
|
1888
|
1902
|
|
|
1903
|
+ // solve lsq problem
|
|
1904
|
+ double *plane_equation_coefficients = qr_solve(abl2, 3, eqnAMatrix, eqnBVector);
|
|
1905
|
+
|
|
1906
|
+ mean /= abl2;
|
|
1907
|
+
|
|
1908
|
+ if (verbose_level) {
|
|
1909
|
+ SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
|
|
1910
|
+ SERIAL_PROTOCOL(plane_equation_coefficients[0]);
|
|
1911
|
+ SERIAL_PROTOCOLPGM(" b: ");
|
|
1912
|
+ SERIAL_PROTOCOL(plane_equation_coefficients[1]);
|
|
1913
|
+ SERIAL_PROTOCOLPGM(" d: ");
|
|
1914
|
+ SERIAL_PROTOCOLLN(plane_equation_coefficients[2]);
|
|
1915
|
+ if (verbose_level > 2) {
|
|
1916
|
+ SERIAL_PROTOCOLPGM("Mean of sampled points: ");
|
|
1917
|
+ SERIAL_PROTOCOL_F(mean, 6);
|
|
1918
|
+ SERIAL_PROTOCOLPGM(" \n");
|
|
1919
|
+ }
|
|
1920
|
+ }
|
1889
|
1921
|
|
1890
|
|
-#endif // AUTO_BED_LEVELING_GRID
|
1891
|
|
- st_synchronize();
|
|
1922
|
+ if (topo_flag) {
|
1892
|
1923
|
|
1893
|
|
- // The following code correct the Z height difference from z-probe position and hotend tip position.
|
1894
|
|
- // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
|
1895
|
|
- // When the bed is uneven, this height must be corrected.
|
1896
|
|
- real_z = float(st_get_position(Z_AXIS))/axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane)
|
1897
|
|
- x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
|
1898
|
|
- y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
|
1899
|
|
- z_tmp = current_position[Z_AXIS];
|
|
1924
|
+ int xx, yy;
|
1900
|
1925
|
|
1901
|
|
- apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset
|
1902
|
|
- current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner.
|
1903
|
|
- plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
1904
|
|
-#ifdef Z_PROBE_SLED
|
1905
|
|
- dock_sled(true, -SLED_DOCKING_OFFSET); // correct for over travel.
|
1906
|
|
-#endif // Z_PROBE_SLED
|
|
1926
|
+ SERIAL_PROTOCOLPGM(" \nBed Height Topography: \n");
|
|
1927
|
+ #if TOPO_ORIGIN == ORIGIN_FRONT_LEFT
|
|
1928
|
+ for (yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--)
|
|
1929
|
+ #else
|
|
1930
|
+ for (yy = 0; yy < auto_bed_leveling_grid_points; yy++)
|
|
1931
|
+ #endif
|
|
1932
|
+ {
|
|
1933
|
+ #if TOPO_ORIGIN == ORIGIN_BACK_RIGHT
|
|
1934
|
+ for (xx = auto_bed_leveling_grid_points - 1; xx >= 0; xx--)
|
|
1935
|
+ #else
|
|
1936
|
+ for (xx = 0; xx < auto_bed_leveling_grid_points; xx++)
|
|
1937
|
+ #endif
|
|
1938
|
+ {
|
|
1939
|
+ int ind =
|
|
1940
|
+ #if TOPO_ORIGIN == ORIGIN_BACK_RIGHT || TOPO_ORIGIN == ORIGIN_FRONT_LEFT
|
|
1941
|
+ yy * auto_bed_leveling_grid_points + xx
|
|
1942
|
+ #elif TOPO_ORIGIN == ORIGIN_BACK_LEFT
|
|
1943
|
+ xx * auto_bed_leveling_grid_points + yy
|
|
1944
|
+ #elif TOPO_ORIGIN == ORIGIN_FRONT_RIGHT
|
|
1945
|
+ abl2 - xx * auto_bed_leveling_grid_points - yy - 1
|
|
1946
|
+ #endif
|
|
1947
|
+ ;
|
|
1948
|
+ float diff = eqnBVector[ind] - mean;
|
|
1949
|
+ if (diff >= 0.0)
|
|
1950
|
+ SERIAL_PROTOCOLPGM(" +"); // Watch column alignment in Pronterface
|
|
1951
|
+ else
|
|
1952
|
+ SERIAL_PROTOCOLPGM(" -");
|
|
1953
|
+ SERIAL_PROTOCOL_F(diff, 5);
|
|
1954
|
+ } // xx
|
|
1955
|
+ SERIAL_PROTOCOLPGM("\n");
|
|
1956
|
+ } // yy
|
|
1957
|
+ SERIAL_PROTOCOLPGM("\n");
|
|
1958
|
+
|
|
1959
|
+ } //topo_flag
|
|
1960
|
+
|
|
1961
|
+
|
|
1962
|
+ set_bed_level_equation_lsq(plane_equation_coefficients);
|
|
1963
|
+ free(plane_equation_coefficients);
|
|
1964
|
+
|
|
1965
|
+ #else // !AUTO_BED_LEVELING_GRID
|
|
1966
|
+
|
|
1967
|
+ // Probe at 3 arbitrary points
|
|
1968
|
+ float z_at_pt_1, z_at_pt_2, z_at_pt_3;
|
|
1969
|
+
|
|
1970
|
+ if (enhanced_g29) {
|
|
1971
|
+ // Basic Enhanced G29
|
|
1972
|
+ z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING, ProbeEngage);
|
|
1973
|
+ z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, ProbeStay);
|
|
1974
|
+ z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, ProbeRetract);
|
1907
|
1975
|
}
|
1908
|
|
- break;
|
|
1976
|
+ else {
|
|
1977
|
+ z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING);
|
|
1978
|
+ z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
|
|
1979
|
+ z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
|
|
1980
|
+ }
|
|
1981
|
+ clean_up_after_endstop_move();
|
|
1982
|
+ set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
|
|
1983
|
+
|
|
1984
|
+ #endif // !AUTO_BED_LEVELING_GRID
|
|
1985
|
+
|
|
1986
|
+ st_synchronize();
|
|
1987
|
+
|
|
1988
|
+ if (verbose_level > 0)
|
|
1989
|
+ plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");
|
|
1990
|
+
|
|
1991
|
+ // The following code correct the Z height difference from z-probe position and hotend tip position.
|
|
1992
|
+ // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
|
|
1993
|
+ // When the bed is uneven, this height must be corrected.
|
|
1994
|
+ real_z = float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane)
|
|
1995
|
+ x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
|
|
1996
|
+ y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
|
|
1997
|
+ z_tmp = current_position[Z_AXIS];
|
|
1998
|
+
|
|
1999
|
+ apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset
|
|
2000
|
+ current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner.
|
|
2001
|
+ plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
|
2002
|
+
|
|
2003
|
+ #ifdef Z_PROBE_SLED
|
|
2004
|
+ dock_sled(true, -SLED_DOCKING_OFFSET); // dock the probe, correcting for over-travel
|
|
2005
|
+ #endif
|
|
2006
|
+ }
|
|
2007
|
+ break;
|
|
2008
|
+
|
1909
|
2009
|
#ifndef Z_PROBE_SLED
|
1910
|
2010
|
case 30: // G30 Single Z Probe
|
1911
|
2011
|
{
|