|
@@ -41,6 +41,10 @@
|
41
|
41
|
|
42
|
42
|
#define SERVO_LEVELING defined(ENABLE_AUTO_BED_LEVELING) && PROBE_SERVO_DEACTIVATION_DELAY > 0
|
43
|
43
|
|
|
44
|
+#if defined(MESH_BED_LEVELING)
|
|
45
|
+ #include "mesh_bed_leveling.h"
|
|
46
|
+#endif
|
|
47
|
+
|
44
|
48
|
#include "ultralcd.h"
|
45
|
49
|
#include "planner.h"
|
46
|
50
|
#include "stepper.h"
|
|
@@ -1737,6 +1741,11 @@ inline void gcode_G28() {
|
1737
|
1741
|
#endif
|
1738
|
1742
|
#endif
|
1739
|
1743
|
|
|
1744
|
+ #if defined(MESH_BED_LEVELING)
|
|
1745
|
+ uint8_t mbl_was_active = mbl.active;
|
|
1746
|
+ mbl.active = 0;
|
|
1747
|
+ #endif
|
|
1748
|
+
|
1740
|
1749
|
saved_feedrate = feedrate;
|
1741
|
1750
|
saved_feedmultiply = feedmultiply;
|
1742
|
1751
|
feedmultiply = 100;
|
|
@@ -1951,12 +1960,112 @@ inline void gcode_G28() {
|
1951
|
1960
|
enable_endstops(false);
|
1952
|
1961
|
#endif
|
1953
|
1962
|
|
|
1963
|
+ #if defined(MESH_BED_LEVELING)
|
|
1964
|
+ if (mbl_was_active) {
|
|
1965
|
+ current_position[X_AXIS] = mbl.get_x(0);
|
|
1966
|
+ current_position[Y_AXIS] = mbl.get_y(0);
|
|
1967
|
+ destination[X_AXIS] = current_position[X_AXIS];
|
|
1968
|
+ destination[Y_AXIS] = current_position[Y_AXIS];
|
|
1969
|
+ destination[Z_AXIS] = current_position[Z_AXIS];
|
|
1970
|
+ destination[E_AXIS] = current_position[E_AXIS];
|
|
1971
|
+ feedrate = homing_feedrate[X_AXIS];
|
|
1972
|
+ plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate, active_extruder);
|
|
1973
|
+ st_synchronize();
|
|
1974
|
+ current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
|
|
1975
|
+ plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
|
1976
|
+ mbl.active = 1;
|
|
1977
|
+ }
|
|
1978
|
+ #endif
|
|
1979
|
+
|
1954
|
1980
|
feedrate = saved_feedrate;
|
1955
|
1981
|
feedmultiply = saved_feedmultiply;
|
1956
|
1982
|
previous_millis_cmd = millis();
|
1957
|
1983
|
endstops_hit_on_purpose();
|
1958
|
1984
|
}
|
1959
|
1985
|
|
|
1986
|
+#if defined(MESH_BED_LEVELING)
|
|
1987
|
+
|
|
1988
|
+ inline void gcode_G29() {
|
|
1989
|
+ static int probe_point = -1;
|
|
1990
|
+ int state = 0;
|
|
1991
|
+ if (code_seen('S') || code_seen('s')) {
|
|
1992
|
+ state = code_value_long();
|
|
1993
|
+ if (state < 0 || state > 2) {
|
|
1994
|
+ SERIAL_PROTOCOLPGM("S out of range (0-2).\n");
|
|
1995
|
+ return;
|
|
1996
|
+ }
|
|
1997
|
+ }
|
|
1998
|
+
|
|
1999
|
+ if (state == 0) {
|
|
2000
|
+ if (mbl.active) {
|
|
2001
|
+ SERIAL_PROTOCOLPGM("Num X,Y: ");
|
|
2002
|
+ SERIAL_PROTOCOL(MESH_NUM_X_POINTS);
|
|
2003
|
+ SERIAL_PROTOCOLPGM(",");
|
|
2004
|
+ SERIAL_PROTOCOL(MESH_NUM_Y_POINTS);
|
|
2005
|
+ SERIAL_PROTOCOLPGM("\nZ search height: ");
|
|
2006
|
+ SERIAL_PROTOCOL(MESH_HOME_SEARCH_Z);
|
|
2007
|
+ SERIAL_PROTOCOLPGM("\nMeasured points:\n");
|
|
2008
|
+ for (int y=0; y<MESH_NUM_Y_POINTS; y++) {
|
|
2009
|
+ for (int x=0; x<MESH_NUM_X_POINTS; x++) {
|
|
2010
|
+ SERIAL_PROTOCOLPGM(" ");
|
|
2011
|
+ SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5);
|
|
2012
|
+ }
|
|
2013
|
+ SERIAL_EOL;
|
|
2014
|
+ }
|
|
2015
|
+ } else {
|
|
2016
|
+ SERIAL_PROTOCOLPGM("Mesh bed leveling not active.\n");
|
|
2017
|
+ }
|
|
2018
|
+
|
|
2019
|
+ } else if (state == 1) {
|
|
2020
|
+
|
|
2021
|
+ mbl.reset();
|
|
2022
|
+ probe_point = 0;
|
|
2023
|
+ enquecommands_P(PSTR("G28"));
|
|
2024
|
+ enquecommands_P(PSTR("G29 S2"));
|
|
2025
|
+
|
|
2026
|
+ } else if (state == 2) {
|
|
2027
|
+
|
|
2028
|
+ if (probe_point < 0) {
|
|
2029
|
+ SERIAL_PROTOCOLPGM("Mesh probing not started.\n");
|
|
2030
|
+ return;
|
|
2031
|
+ }
|
|
2032
|
+ int ix, iy;
|
|
2033
|
+ if (probe_point == 0) {
|
|
2034
|
+ current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
|
|
2035
|
+ plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
|
2036
|
+ } else {
|
|
2037
|
+ ix = (probe_point-1) % MESH_NUM_X_POINTS;
|
|
2038
|
+ iy = (probe_point-1) / MESH_NUM_X_POINTS;
|
|
2039
|
+ if (iy&1) {
|
|
2040
|
+ ix = (MESH_NUM_X_POINTS - 1) - ix;
|
|
2041
|
+ }
|
|
2042
|
+ mbl.set_z(ix, iy, current_position[Z_AXIS]);
|
|
2043
|
+ current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
|
|
2044
|
+ plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
|
|
2045
|
+ st_synchronize();
|
|
2046
|
+ }
|
|
2047
|
+ if (probe_point == MESH_NUM_X_POINTS*MESH_NUM_Y_POINTS) {
|
|
2048
|
+ SERIAL_PROTOCOLPGM("Mesh done.\n");
|
|
2049
|
+ probe_point = -1;
|
|
2050
|
+ mbl.active = 1;
|
|
2051
|
+ enquecommands_P(PSTR("G28"));
|
|
2052
|
+ return;
|
|
2053
|
+ }
|
|
2054
|
+ ix = probe_point % MESH_NUM_X_POINTS;
|
|
2055
|
+ iy = probe_point / MESH_NUM_X_POINTS;
|
|
2056
|
+ if (iy&1) {
|
|
2057
|
+ ix = (MESH_NUM_X_POINTS - 1) - ix;
|
|
2058
|
+ }
|
|
2059
|
+ current_position[X_AXIS] = mbl.get_x(ix);
|
|
2060
|
+ current_position[Y_AXIS] = mbl.get_y(iy);
|
|
2061
|
+ plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
|
|
2062
|
+ st_synchronize();
|
|
2063
|
+ probe_point++;
|
|
2064
|
+ }
|
|
2065
|
+ }
|
|
2066
|
+
|
|
2067
|
+#endif
|
|
2068
|
+
|
1960
|
2069
|
#ifdef ENABLE_AUTO_BED_LEVELING
|
1961
|
2070
|
|
1962
|
2071
|
|
|
@@ -4661,6 +4770,12 @@ void process_commands() {
|
4661
|
4770
|
gcode_G28();
|
4662
|
4771
|
break;
|
4663
|
4772
|
|
|
4773
|
+ #if defined(MESH_BED_LEVELING)
|
|
4774
|
+ case 29:
|
|
4775
|
+ gcode_G29();
|
|
4776
|
+ break;
|
|
4777
|
+ #endif
|
|
4778
|
+
|
4664
|
4779
|
#ifdef ENABLE_AUTO_BED_LEVELING
|
4665
|
4780
|
|
4666
|
4781
|
case 29:
|
|
@@ -5280,6 +5395,81 @@ void prepare_move_raw()
|
5280
|
5395
|
}
|
5281
|
5396
|
#endif
|
5282
|
5397
|
|
|
5398
|
+#if defined(MESH_BED_LEVELING)
|
|
5399
|
+#if !defined(MIN)
|
|
5400
|
+#define MIN(_v1, _v2) (((_v1) < (_v2)) ? (_v1) : (_v2))
|
|
5401
|
+#endif
|
|
5402
|
+
|
|
5403
|
+void mesh_plan_buffer_line(float x, float y, float z, const float e, float feed_rate, const uint8_t &extruder, uint8_t x_splits=0xff, uint8_t y_splits=0xff)
|
|
5404
|
+{
|
|
5405
|
+ if (!mbl.active) {
|
|
5406
|
+ plan_buffer_line(x, y, z, e, feed_rate, extruder);
|
|
5407
|
+ for(int8_t i=0; i < NUM_AXIS; i++) {
|
|
5408
|
+ current_position[i] = destination[i];
|
|
5409
|
+ }
|
|
5410
|
+ return;
|
|
5411
|
+ }
|
|
5412
|
+ int pix = mbl.select_x_index(current_position[X_AXIS]);
|
|
5413
|
+ int piy = mbl.select_y_index(current_position[Y_AXIS]);
|
|
5414
|
+ int ix = mbl.select_x_index(x);
|
|
5415
|
+ int iy = mbl.select_y_index(y);
|
|
5416
|
+ pix = MIN(pix, MESH_NUM_X_POINTS-2);
|
|
5417
|
+ piy = MIN(piy, MESH_NUM_Y_POINTS-2);
|
|
5418
|
+ ix = MIN(ix, MESH_NUM_X_POINTS-2);
|
|
5419
|
+ iy = MIN(iy, MESH_NUM_Y_POINTS-2);
|
|
5420
|
+ if (pix == ix && piy == iy) {
|
|
5421
|
+
|
|
5422
|
+ plan_buffer_line(x, y, z, e, feed_rate, extruder);
|
|
5423
|
+ for(int8_t i=0; i < NUM_AXIS; i++) {
|
|
5424
|
+ current_position[i] = destination[i];
|
|
5425
|
+ }
|
|
5426
|
+ return;
|
|
5427
|
+ }
|
|
5428
|
+ float nx, ny, ne, normalized_dist;
|
|
5429
|
+ if (ix > pix && (x_splits) & BIT(ix)) {
|
|
5430
|
+ nx = mbl.get_x(ix);
|
|
5431
|
+ normalized_dist = (nx - current_position[X_AXIS])/(x - current_position[X_AXIS]);
|
|
5432
|
+ ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
|
|
5433
|
+ ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
|
|
5434
|
+ x_splits ^= BIT(ix);
|
|
5435
|
+ } else if (ix < pix && (x_splits) & BIT(pix)) {
|
|
5436
|
+ nx = mbl.get_x(pix);
|
|
5437
|
+ normalized_dist = (nx - current_position[X_AXIS])/(x - current_position[X_AXIS]);
|
|
5438
|
+ ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
|
|
5439
|
+ ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
|
|
5440
|
+ x_splits ^= BIT(pix);
|
|
5441
|
+ } else if (iy > piy && (y_splits) & BIT(iy)) {
|
|
5442
|
+ ny = mbl.get_y(iy);
|
|
5443
|
+ normalized_dist = (ny - current_position[Y_AXIS])/(y - current_position[Y_AXIS]);
|
|
5444
|
+ nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
|
|
5445
|
+ ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
|
|
5446
|
+ y_splits ^= BIT(iy);
|
|
5447
|
+ } else if (iy < piy && (y_splits) & BIT(piy)) {
|
|
5448
|
+ ny = mbl.get_y(piy);
|
|
5449
|
+ normalized_dist = (ny - current_position[Y_AXIS])/(y - current_position[Y_AXIS]);
|
|
5450
|
+ nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
|
|
5451
|
+ ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
|
|
5452
|
+ y_splits ^= BIT(piy);
|
|
5453
|
+ } else {
|
|
5454
|
+
|
|
5455
|
+ plan_buffer_line(x, y, z, e, feed_rate, extruder);
|
|
5456
|
+ for(int8_t i=0; i < NUM_AXIS; i++) {
|
|
5457
|
+ current_position[i] = destination[i];
|
|
5458
|
+ }
|
|
5459
|
+ return;
|
|
5460
|
+ }
|
|
5461
|
+
|
|
5462
|
+ destination[X_AXIS] = nx;
|
|
5463
|
+ destination[Y_AXIS] = ny;
|
|
5464
|
+ destination[E_AXIS] = ne;
|
|
5465
|
+ mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
|
|
5466
|
+ destination[X_AXIS] = x;
|
|
5467
|
+ destination[Y_AXIS] = y;
|
|
5468
|
+ destination[E_AXIS] = e;
|
|
5469
|
+ mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
|
|
5470
|
+}
|
|
5471
|
+#endif
|
|
5472
|
+
|
5283
|
5473
|
void prepare_move()
|
5284
|
5474
|
{
|
5285
|
5475
|
clamp_to_software_endstops(destination);
|
|
@@ -5395,10 +5585,14 @@ for (int s = 1; s <= steps; s++) {
|
5395
|
5585
|
#if ! (defined DELTA || defined SCARA)
|
5396
|
5586
|
|
5397
|
5587
|
if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
|
5398
|
|
- plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
|
5399
|
|
- }
|
5400
|
|
- else {
|
|
5588
|
+ plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
|
|
5589
|
+ } else {
|
|
5590
|
+#if defined(MESH_BED_LEVELING)
|
|
5591
|
+ mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
|
|
5592
|
+ return;
|
|
5593
|
+#else
|
5401
|
5594
|
plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
|
|
5595
|
+#endif
|
5402
|
5596
|
}
|
5403
|
5597
|
#endif
|
5404
|
5598
|
|