|
|
@ -353,10 +353,10 @@ static long gcode_N, gcode_LastN, Stopped_gcode_LastN = 0;
|
|
|
|
* the main loop. The process_next_command function parses the next
|
|
|
|
* the main loop. The process_next_command function parses the next
|
|
|
|
* command and hands off execution to individual handler functions.
|
|
|
|
* command and hands off execution to individual handler functions.
|
|
|
|
*/
|
|
|
|
*/
|
|
|
|
static char command_queue[BUFSIZE][MAX_CMD_SIZE];
|
|
|
|
uint8_t commands_in_queue = 0; // Count of commands in the queue
|
|
|
|
static uint8_t cmd_queue_index_r = 0, // Ring buffer read position
|
|
|
|
static uint8_t cmd_queue_index_r = 0, // Ring buffer read position
|
|
|
|
cmd_queue_index_w = 0, // Ring buffer write position
|
|
|
|
cmd_queue_index_w = 0; // Ring buffer write position
|
|
|
|
commands_in_queue = 0; // Count of commands in the queue
|
|
|
|
static char command_queue[BUFSIZE][MAX_CMD_SIZE];
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
/**
|
|
|
|
* Current GCode Command
|
|
|
|
* Current GCode Command
|
|
|
@ -3502,6 +3502,12 @@ inline void gcode_G4() {
|
|
|
|
|
|
|
|
|
|
|
|
#endif // Z_SAFE_HOMING
|
|
|
|
#endif // Z_SAFE_HOMING
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(PROBE_MANUALLY)
|
|
|
|
|
|
|
|
static bool g29_in_progress = false;
|
|
|
|
|
|
|
|
#else
|
|
|
|
|
|
|
|
constexpr bool g29_in_progress = false;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
/**
|
|
|
|
* G28: Home all axes according to settings
|
|
|
|
* G28: Home all axes according to settings
|
|
|
|
*
|
|
|
|
*
|
|
|
@ -3529,6 +3535,11 @@ inline void gcode_G28() {
|
|
|
|
// Wait for planner moves to finish!
|
|
|
|
// Wait for planner moves to finish!
|
|
|
|
stepper.synchronize();
|
|
|
|
stepper.synchronize();
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Cancel the active G29 session
|
|
|
|
|
|
|
|
#if ENABLED(PROBE_MANUALLY)
|
|
|
|
|
|
|
|
g29_in_progress = false;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
// Disable the leveling matrix before homing
|
|
|
|
// Disable the leveling matrix before homing
|
|
|
|
#if PLANNER_LEVELING
|
|
|
|
#if PLANNER_LEVELING
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_UBL)
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_UBL)
|
|
|
@ -3719,9 +3730,9 @@ inline void gcode_G28() {
|
|
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(MESH_BED_LEVELING)
|
|
|
|
#if ENABLED(MESH_BED_LEVELING) || ENABLED(PROBE_MANUALLY)
|
|
|
|
|
|
|
|
|
|
|
|
inline void _mbl_goto_xy(const float &x, const float &y) {
|
|
|
|
inline void _manual_goto_xy(const float &x, const float &y) {
|
|
|
|
const float old_feedrate_mm_s = feedrate_mm_s;
|
|
|
|
const float old_feedrate_mm_s = feedrate_mm_s;
|
|
|
|
|
|
|
|
|
|
|
|
#if MANUAL_PROBE_HEIGHT > 0
|
|
|
|
#if MANUAL_PROBE_HEIGHT > 0
|
|
|
@ -3745,6 +3756,10 @@ inline void gcode_G28() {
|
|
|
|
stepper.synchronize();
|
|
|
|
stepper.synchronize();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(MESH_BED_LEVELING)
|
|
|
|
|
|
|
|
|
|
|
|
// Save 130 bytes with non-duplication of PSTR
|
|
|
|
// Save 130 bytes with non-duplication of PSTR
|
|
|
|
void say_not_entered() { SERIAL_PROTOCOLLNPGM(" not entered."); }
|
|
|
|
void say_not_entered() { SERIAL_PROTOCOLLNPGM(" not entered."); }
|
|
|
|
|
|
|
|
|
|
|
@ -3835,7 +3850,7 @@ inline void gcode_G28() {
|
|
|
|
// If there's another point to sample, move there with optional lift.
|
|
|
|
// If there's another point to sample, move there with optional lift.
|
|
|
|
if (mbl_probe_index < (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) {
|
|
|
|
if (mbl_probe_index < (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) {
|
|
|
|
mbl.zigzag(mbl_probe_index, px, py);
|
|
|
|
mbl.zigzag(mbl_probe_index, px, py);
|
|
|
|
_mbl_goto_xy(mbl.index_to_xpos[px], mbl.index_to_ypos[py]);
|
|
|
|
_manual_goto_xy(mbl.index_to_xpos[px], mbl.index_to_ypos[py]);
|
|
|
|
|
|
|
|
|
|
|
|
#if HAS_SOFTWARE_ENDSTOPS
|
|
|
|
#if HAS_SOFTWARE_ENDSTOPS
|
|
|
|
// Disable software endstops to allow manual adjustment
|
|
|
|
// Disable software endstops to allow manual adjustment
|
|
|
@ -3917,50 +3932,86 @@ inline void gcode_G28() {
|
|
|
|
|
|
|
|
|
|
|
|
#elif HAS_ABL && DISABLED(AUTO_BED_LEVELING_UBL)
|
|
|
|
#elif HAS_ABL && DISABLED(AUTO_BED_LEVELING_UBL)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ABL_GRID
|
|
|
|
|
|
|
|
#if ENABLED(PROBE_Y_FIRST)
|
|
|
|
|
|
|
|
#define PR_OUTER_VAR xCount
|
|
|
|
|
|
|
|
#define PR_OUTER_END abl_grid_points_x
|
|
|
|
|
|
|
|
#define PR_INNER_VAR yCount
|
|
|
|
|
|
|
|
#define PR_INNER_END abl_grid_points_y
|
|
|
|
|
|
|
|
#else
|
|
|
|
|
|
|
|
#define PR_OUTER_VAR yCount
|
|
|
|
|
|
|
|
#define PR_OUTER_END abl_grid_points_y
|
|
|
|
|
|
|
|
#define PR_INNER_VAR xCount
|
|
|
|
|
|
|
|
#define PR_INNER_END abl_grid_points_x
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
/**
|
|
|
|
* G29: Detailed Z probe, probes the bed at 3 or more points.
|
|
|
|
* G29: Detailed Z probe, probes the bed at 3 or more points.
|
|
|
|
* Will fail if the printer has not been homed with G28.
|
|
|
|
* Will fail if the printer has not been homed with G28.
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* Enhanced G29 Auto Bed Leveling Probe Routine
|
|
|
|
* Enhanced G29 Auto Bed Leveling Probe Routine
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* Parameters With LINEAR and BILINEAR:
|
|
|
|
* D Dry-Run mode. Just evaluate the bed Topology - Don't apply
|
|
|
|
|
|
|
|
* or alter the bed level data. Useful to check the topology
|
|
|
|
|
|
|
|
* after a first run of G29.
|
|
|
|
|
|
|
|
*
|
|
|
|
|
|
|
|
* J Jettison current bed leveling data
|
|
|
|
|
|
|
|
*
|
|
|
|
|
|
|
|
* V Set the verbose level (0-4). Example: "G29 V3"
|
|
|
|
|
|
|
|
*
|
|
|
|
|
|
|
|
* Parameters With LINEAR leveling only:
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* P Set the size of the grid that will be probed (P x P points).
|
|
|
|
* P Set the size of the grid that will be probed (P x P points).
|
|
|
|
* Not supported by non-linear delta printer bed leveling.
|
|
|
|
|
|
|
|
* Example: "G29 P4"
|
|
|
|
* Example: "G29 P4"
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* S Set the XY travel speed between probe points (in units/min)
|
|
|
|
* X Set the X size of the grid that will be probed (X x Y points).
|
|
|
|
|
|
|
|
* Example: "G29 X7 Y5"
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* D Dry-Run mode. Just evaluate the bed Topology - Don't apply
|
|
|
|
* Y Set the Y size of the grid that will be probed (X x Y points).
|
|
|
|
* or clean the rotation Matrix. Useful to check the topology
|
|
|
|
|
|
|
|
* after a first run of G29.
|
|
|
|
|
|
|
|
*
|
|
|
|
|
|
|
|
* V Set the verbose level (0-4). Example: "G29 V3"
|
|
|
|
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* T Generate a Bed Topology Report. Example: "G29 P5 T" for a detailed report.
|
|
|
|
* T Generate a Bed Topology Report. Example: "G29 P5 T" for a detailed report.
|
|
|
|
* This is useful for manual bed leveling and finding flaws in the bed (to
|
|
|
|
* This is useful for manual bed leveling and finding flaws in the bed (to
|
|
|
|
* assist with part placement).
|
|
|
|
* assist with part placement).
|
|
|
|
* Not supported by non-linear delta printer bed leveling.
|
|
|
|
* Not supported by non-linear delta printer bed leveling.
|
|
|
|
*
|
|
|
|
*
|
|
|
|
|
|
|
|
* Parameters With LINEAR and BILINEAR leveling only:
|
|
|
|
|
|
|
|
*
|
|
|
|
|
|
|
|
* S Set the XY travel speed between probe points (in units/min)
|
|
|
|
|
|
|
|
*
|
|
|
|
* F Set the Front limit of the probing grid
|
|
|
|
* F Set the Front limit of the probing grid
|
|
|
|
* B Set the Back limit of the probing grid
|
|
|
|
* B Set the Back limit of the probing grid
|
|
|
|
* L Set the Left limit of the probing grid
|
|
|
|
* L Set the Left limit of the probing grid
|
|
|
|
* R Set the Right limit of the probing grid
|
|
|
|
* R Set the Right limit of the probing grid
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* Parameters with BILINEAR only:
|
|
|
|
* Parameters with BILINEAR leveling only:
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* Z Supply an additional Z probe offset
|
|
|
|
* Z Supply an additional Z probe offset
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* Global Parameters:
|
|
|
|
* Extra parameters with PROBE_MANUALLY:
|
|
|
|
|
|
|
|
*
|
|
|
|
|
|
|
|
* To do manual probing simply repeat G29 until the procedure is complete.
|
|
|
|
|
|
|
|
* The first G29 accepts parameters. 'G29 Q' for status, 'G29 A' to abort.
|
|
|
|
|
|
|
|
*
|
|
|
|
|
|
|
|
* Q Query leveling and G29 state
|
|
|
|
|
|
|
|
*
|
|
|
|
|
|
|
|
* A Abort current leveling procedure
|
|
|
|
|
|
|
|
*
|
|
|
|
|
|
|
|
* W Write a mesh point. (Ignored during leveling.)
|
|
|
|
|
|
|
|
* X Required X for mesh point
|
|
|
|
|
|
|
|
* Y Required Y for mesh point
|
|
|
|
|
|
|
|
* Z Required Z for mesh point
|
|
|
|
*
|
|
|
|
*
|
|
|
|
* E/e By default G29 will engage the Z probe, test the bed, then disengage.
|
|
|
|
* Without PROBE_MANUALLY:
|
|
|
|
|
|
|
|
*
|
|
|
|
|
|
|
|
* E By default G29 will engage the Z probe, test the bed, then disengage.
|
|
|
|
* Include "E" to engage/disengage the Z probe for each sample.
|
|
|
|
* Include "E" to engage/disengage the Z probe for each sample.
|
|
|
|
* There's no extra effect if you have a fixed Z probe.
|
|
|
|
* There's no extra effect if you have a fixed Z probe.
|
|
|
|
* Usage: "G29 E" or "G29 e"
|
|
|
|
|
|
|
|
*
|
|
|
|
*
|
|
|
|
*/
|
|
|
|
*/
|
|
|
|
inline void gcode_G29() {
|
|
|
|
inline void gcode_G29() {
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// G29 Q is also available if debugging
|
|
|
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
|
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
|
|
const bool query = code_seen('Q');
|
|
|
|
const bool query = code_seen('Q');
|
|
|
|
const uint8_t old_debug_flags = marlin_debug_flags;
|
|
|
|
const uint8_t old_debug_flags = marlin_debug_flags;
|
|
|
@ -3970,37 +4021,148 @@ inline void gcode_G28() {
|
|
|
|
log_machine_info();
|
|
|
|
log_machine_info();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
marlin_debug_flags = old_debug_flags;
|
|
|
|
marlin_debug_flags = old_debug_flags;
|
|
|
|
if (query) return;
|
|
|
|
#if DISABLED(PROBE_MANUALLY)
|
|
|
|
|
|
|
|
if (query) return;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
// Don't allow auto-leveling without homing first
|
|
|
|
// Don't allow auto-leveling without homing first
|
|
|
|
if (axis_unhomed_error(true, true, true)) return;
|
|
|
|
if (axis_unhomed_error(true, true, true)) return;
|
|
|
|
|
|
|
|
|
|
|
|
const int verbose_level = code_seen('V') ? code_value_int() : 1;
|
|
|
|
// Define local vars 'static' for manual probing, 'auto' otherwise
|
|
|
|
if (verbose_level < 0 || verbose_level > 4) {
|
|
|
|
#if ENABLED(PROBE_MANUALLY)
|
|
|
|
SERIAL_PROTOCOLLNPGM("?(V)erbose Level is implausible (0-4).");
|
|
|
|
#define ABL_VAR static
|
|
|
|
return;
|
|
|
|
#else
|
|
|
|
}
|
|
|
|
#define ABL_VAR
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
bool dryrun = code_seen('D'),
|
|
|
|
ABL_VAR int verbose_level, abl_probe_index;
|
|
|
|
stow_probe_after_each = code_seen('E');
|
|
|
|
ABL_VAR float xProbe, yProbe, measured_z;
|
|
|
|
|
|
|
|
ABL_VAR bool dryrun, abl_should_enable;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if HAS_SOFTWARE_ENDSTOPS
|
|
|
|
|
|
|
|
ABL_VAR bool enable_soft_endstops = true;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
#if ABL_GRID
|
|
|
|
#if ABL_GRID
|
|
|
|
|
|
|
|
ABL_VAR uint8_t PR_OUTER_VAR;
|
|
|
|
|
|
|
|
ABL_VAR int8_t PR_INNER_VAR;
|
|
|
|
|
|
|
|
ABL_VAR int left_probe_bed_position, right_probe_bed_position, front_probe_bed_position, back_probe_bed_position;
|
|
|
|
|
|
|
|
ABL_VAR float xGridSpacing, yGridSpacing;
|
|
|
|
|
|
|
|
|
|
|
|
if (verbose_level > 0) {
|
|
|
|
#define ABL_GRID_MAX (ABL_GRID_MAX_POINTS_X) * (ABL_GRID_MAX_POINTS_Y)
|
|
|
|
SERIAL_PROTOCOLLNPGM("G29 Auto Bed Leveling");
|
|
|
|
|
|
|
|
if (dryrun) SERIAL_PROTOCOLLNPGM("Running in DRY-RUN mode");
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ABL_PLANAR
|
|
|
|
#if ABL_PLANAR
|
|
|
|
|
|
|
|
ABL_VAR uint8_t abl_grid_points_x = ABL_GRID_MAX_POINTS_X,
|
|
|
|
|
|
|
|
abl_grid_points_y = ABL_GRID_MAX_POINTS_Y;
|
|
|
|
|
|
|
|
ABL_VAR int abl2;
|
|
|
|
|
|
|
|
ABL_VAR bool do_topography_map;
|
|
|
|
|
|
|
|
#else // 3-point
|
|
|
|
|
|
|
|
uint8_t constexpr abl_grid_points_x = ABL_GRID_MAX_POINTS_X,
|
|
|
|
|
|
|
|
abl_grid_points_y = ABL_GRID_MAX_POINTS_Y;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int constexpr abl2 = ABL_GRID_MAX;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ABL_VAR float zoffset;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#elif ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ABL_VAR int indexIntoAB[ABL_GRID_MAX_POINTS_X][ABL_GRID_MAX_POINTS_Y];
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
ABL_VAR float eqnAMatrix[ABL_GRID_MAX * 3], // "A" matrix of the linear system of equations
|
|
|
|
|
|
|
|
eqnBVector[ABL_GRID_MAX], // "B" vector of Z points
|
|
|
|
|
|
|
|
mean;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#elif ENABLED(AUTO_BED_LEVELING_3POINT)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Probe at 3 arbitrary points
|
|
|
|
|
|
|
|
ABL_VAR vector_3 points[3] = {
|
|
|
|
|
|
|
|
vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, 0),
|
|
|
|
|
|
|
|
vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, 0),
|
|
|
|
|
|
|
|
vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, 0)
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif // AUTO_BED_LEVELING_3POINT
|
|
|
|
|
|
|
|
|
|
|
|
bool do_topography_map = verbose_level > 2 || code_seen('T');
|
|
|
|
/**
|
|
|
|
|
|
|
|
* On the initial G29 fetch command parameters.
|
|
|
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
if (!g29_in_progress) {
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
abl_probe_index = 0;
|
|
|
|
|
|
|
|
abl_should_enable = planner.abl_enabled;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (code_seen('W')) {
|
|
|
|
|
|
|
|
if (!bilinear_grid_spacing[X_AXIS]) {
|
|
|
|
|
|
|
|
SERIAL_ERROR_START;
|
|
|
|
|
|
|
|
SERIAL_ERRORLNPGM("No bilinear grid");
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
const float z = code_seen('Z') && code_has_value() ? code_value_float() : 99999;
|
|
|
|
|
|
|
|
if (!WITHIN(z, -10, 10)) {
|
|
|
|
|
|
|
|
SERIAL_ERROR_START;
|
|
|
|
|
|
|
|
SERIAL_ERRORLNPGM("Bad Z value");
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
const float x = code_seen('X') && code_has_value() ? code_value_float() : 99999,
|
|
|
|
|
|
|
|
y = code_seen('Y') && code_has_value() ? code_value_float() : 99999;
|
|
|
|
|
|
|
|
int8_t i = code_seen('I') && code_has_value() ? code_value_byte() : -1,
|
|
|
|
|
|
|
|
j = code_seen('J') && code_has_value() ? code_value_byte() : -1;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (x < 99998 && y < 99998) {
|
|
|
|
|
|
|
|
// Get nearest i / j from x / y
|
|
|
|
|
|
|
|
i = (x - LOGICAL_X_POSITION(bilinear_start[X_AXIS]) + 0.5 * xGridSpacing) / xGridSpacing;
|
|
|
|
|
|
|
|
j = (y - LOGICAL_Y_POSITION(bilinear_start[Y_AXIS]) + 0.5 * yGridSpacing) / yGridSpacing;
|
|
|
|
|
|
|
|
i = constrain(i, 0, ABL_GRID_MAX_POINTS_X - 1);
|
|
|
|
|
|
|
|
j = constrain(j, 0, ABL_GRID_MAX_POINTS_Y - 1);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if (WITHIN(i, 0, ABL_GRID_MAX_POINTS_X - 1) && WITHIN(j, 0, ABL_GRID_MAX_POINTS_Y)) {
|
|
|
|
|
|
|
|
set_bed_leveling_enabled(false);
|
|
|
|
|
|
|
|
bed_level_grid[i][j] = z;
|
|
|
|
|
|
|
|
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
|
|
|
|
|
|
|
|
bed_level_virt_interpolate();
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
set_bed_leveling_enabled(abl_should_enable);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
} // code_seen('W')
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if PLANNER_LEVELING
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Jettison bed leveling data
|
|
|
|
|
|
|
|
if (code_seen('J')) {
|
|
|
|
|
|
|
|
reset_bed_level();
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
verbose_level = code_seen('V') && code_has_value() ? code_value_int() : 0;
|
|
|
|
|
|
|
|
if (!WITHIN(verbose_level, 0, 4)) {
|
|
|
|
|
|
|
|
SERIAL_PROTOCOLLNPGM("?(V)erbose Level is implausible (0-4).");
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
dryrun = code_seen('D') ? code_value_bool() : false;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
do_topography_map = verbose_level > 2 || code_seen('T');
|
|
|
|
|
|
|
|
|
|
|
|
// X and Y specify points in each direction, overriding the default
|
|
|
|
// X and Y specify points in each direction, overriding the default
|
|
|
|
// These values may be saved with the completed mesh
|
|
|
|
// These values may be saved with the completed mesh
|
|
|
|
int abl_grid_points_x = code_seen('X') ? code_value_int() : ABL_GRID_MAX_POINTS_X,
|
|
|
|
abl_grid_points_x = code_seen('X') ? code_value_int() : ABL_GRID_MAX_POINTS_X;
|
|
|
|
abl_grid_points_y = code_seen('Y') ? code_value_int() : ABL_GRID_MAX_POINTS_Y;
|
|
|
|
abl_grid_points_y = code_seen('Y') ? code_value_int() : ABL_GRID_MAX_POINTS_Y;
|
|
|
|
|
|
|
|
|
|
|
|
if (code_seen('P')) abl_grid_points_x = abl_grid_points_y = code_value_int();
|
|
|
|
if (code_seen('P')) abl_grid_points_x = abl_grid_points_y = code_value_int();
|
|
|
|
|
|
|
|
|
|
|
|
if (abl_grid_points_x < 2 || abl_grid_points_y < 2) {
|
|
|
|
if (abl_grid_points_x < 2 || abl_grid_points_y < 2) {
|
|
|
@ -4008,91 +4170,98 @@ inline void gcode_G28() {
|
|
|
|
return;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
#else
|
|
|
|
abl2 = abl_grid_points_x * abl_grid_points_y;
|
|
|
|
|
|
|
|
|
|
|
|
const uint8_t abl_grid_points_x = ABL_GRID_MAX_POINTS_X, abl_grid_points_y = ABL_GRID_MAX_POINTS_Y;
|
|
|
|
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
zoffset = code_seen('Z') ? code_value_axis_units(Z_AXIS) : 0;
|
|
|
|
|
|
|
|
#if HAS_BED_PROBE
|
|
|
|
|
|
|
|
zoffset += zprobe_zoffset;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
xy_probe_feedrate_mm_s = MMM_TO_MMS(code_seen('S') ? code_value_linear_units() : XY_PROBE_SPEED);
|
|
|
|
#if ABL_GRID
|
|
|
|
|
|
|
|
|
|
|
|
int left_probe_bed_position = code_seen('L') ? (int)code_value_axis_units(X_AXIS) : LOGICAL_X_POSITION(LEFT_PROBE_BED_POSITION),
|
|
|
|
xy_probe_feedrate_mm_s = MMM_TO_MMS(code_seen('S') ? code_value_linear_units() : XY_PROBE_SPEED);
|
|
|
|
right_probe_bed_position = code_seen('R') ? (int)code_value_axis_units(X_AXIS) : LOGICAL_X_POSITION(RIGHT_PROBE_BED_POSITION),
|
|
|
|
|
|
|
|
front_probe_bed_position = code_seen('F') ? (int)code_value_axis_units(Y_AXIS) : LOGICAL_Y_POSITION(FRONT_PROBE_BED_POSITION),
|
|
|
|
|
|
|
|
back_probe_bed_position = code_seen('B') ? (int)code_value_axis_units(Y_AXIS) : LOGICAL_Y_POSITION(BACK_PROBE_BED_POSITION);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
const bool left_out_l = left_probe_bed_position < LOGICAL_X_POSITION(MIN_PROBE_X),
|
|
|
|
left_probe_bed_position = code_seen('L') ? (int)code_value_axis_units(X_AXIS) : LOGICAL_X_POSITION(LEFT_PROBE_BED_POSITION);
|
|
|
|
left_out = left_out_l || left_probe_bed_position > right_probe_bed_position - (MIN_PROBE_EDGE),
|
|
|
|
right_probe_bed_position = code_seen('R') ? (int)code_value_axis_units(X_AXIS) : LOGICAL_X_POSITION(RIGHT_PROBE_BED_POSITION);
|
|
|
|
right_out_r = right_probe_bed_position > LOGICAL_X_POSITION(MAX_PROBE_X),
|
|
|
|
front_probe_bed_position = code_seen('F') ? (int)code_value_axis_units(Y_AXIS) : LOGICAL_Y_POSITION(FRONT_PROBE_BED_POSITION);
|
|
|
|
right_out = right_out_r || right_probe_bed_position < left_probe_bed_position + MIN_PROBE_EDGE,
|
|
|
|
back_probe_bed_position = code_seen('B') ? (int)code_value_axis_units(Y_AXIS) : LOGICAL_Y_POSITION(BACK_PROBE_BED_POSITION);
|
|
|
|
front_out_f = front_probe_bed_position < LOGICAL_Y_POSITION(MIN_PROBE_Y),
|
|
|
|
|
|
|
|
front_out = front_out_f || front_probe_bed_position > back_probe_bed_position - (MIN_PROBE_EDGE),
|
|
|
|
|
|
|
|
back_out_b = back_probe_bed_position > LOGICAL_Y_POSITION(MAX_PROBE_Y),
|
|
|
|
|
|
|
|
back_out = back_out_b || back_probe_bed_position < front_probe_bed_position + MIN_PROBE_EDGE;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (left_out || right_out || front_out || back_out) {
|
|
|
|
const bool left_out_l = left_probe_bed_position < LOGICAL_X_POSITION(MIN_PROBE_X),
|
|
|
|
if (left_out) {
|
|
|
|
left_out = left_out_l || left_probe_bed_position > right_probe_bed_position - (MIN_PROBE_EDGE),
|
|
|
|
out_of_range_error(PSTR("(L)eft"));
|
|
|
|
right_out_r = right_probe_bed_position > LOGICAL_X_POSITION(MAX_PROBE_X),
|
|
|
|
left_probe_bed_position = left_out_l ? LOGICAL_X_POSITION(MIN_PROBE_X) : right_probe_bed_position - (MIN_PROBE_EDGE);
|
|
|
|
right_out = right_out_r || right_probe_bed_position < left_probe_bed_position + MIN_PROBE_EDGE,
|
|
|
|
}
|
|
|
|
front_out_f = front_probe_bed_position < LOGICAL_Y_POSITION(MIN_PROBE_Y),
|
|
|
|
if (right_out) {
|
|
|
|
front_out = front_out_f || front_probe_bed_position > back_probe_bed_position - (MIN_PROBE_EDGE),
|
|
|
|
out_of_range_error(PSTR("(R)ight"));
|
|
|
|
back_out_b = back_probe_bed_position > LOGICAL_Y_POSITION(MAX_PROBE_Y),
|
|
|
|
right_probe_bed_position = right_out_r ? LOGICAL_Y_POSITION(MAX_PROBE_X) : left_probe_bed_position + MIN_PROBE_EDGE;
|
|
|
|
back_out = back_out_b || back_probe_bed_position < front_probe_bed_position + MIN_PROBE_EDGE;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (front_out) {
|
|
|
|
|
|
|
|
out_of_range_error(PSTR("(F)ront"));
|
|
|
|
|
|
|
|
front_probe_bed_position = front_out_f ? LOGICAL_Y_POSITION(MIN_PROBE_Y) : back_probe_bed_position - (MIN_PROBE_EDGE);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if (back_out) {
|
|
|
|
|
|
|
|
out_of_range_error(PSTR("(B)ack"));
|
|
|
|
|
|
|
|
back_probe_bed_position = back_out_b ? LOGICAL_Y_POSITION(MAX_PROBE_Y) : front_probe_bed_position + MIN_PROBE_EDGE;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif // ABL_GRID
|
|
|
|
if (left_out || right_out || front_out || back_out) {
|
|
|
|
|
|
|
|
if (left_out) {
|
|
|
|
|
|
|
|
out_of_range_error(PSTR("(L)eft"));
|
|
|
|
|
|
|
|
left_probe_bed_position = left_out_l ? LOGICAL_X_POSITION(MIN_PROBE_X) : right_probe_bed_position - (MIN_PROBE_EDGE);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if (right_out) {
|
|
|
|
|
|
|
|
out_of_range_error(PSTR("(R)ight"));
|
|
|
|
|
|
|
|
right_probe_bed_position = right_out_r ? LOGICAL_Y_POSITION(MAX_PROBE_X) : left_probe_bed_position + MIN_PROBE_EDGE;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if (front_out) {
|
|
|
|
|
|
|
|
out_of_range_error(PSTR("(F)ront"));
|
|
|
|
|
|
|
|
front_probe_bed_position = front_out_f ? LOGICAL_Y_POSITION(MIN_PROBE_Y) : back_probe_bed_position - (MIN_PROBE_EDGE);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
if (back_out) {
|
|
|
|
|
|
|
|
out_of_range_error(PSTR("(B)ack"));
|
|
|
|
|
|
|
|
back_probe_bed_position = back_out_b ? LOGICAL_Y_POSITION(MAX_PROBE_Y) : front_probe_bed_position + MIN_PROBE_EDGE;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
stepper.synchronize();
|
|
|
|
// probe at the points of a lattice grid
|
|
|
|
|
|
|
|
xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (abl_grid_points_x - 1);
|
|
|
|
|
|
|
|
yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (abl_grid_points_y - 1);
|
|
|
|
|
|
|
|
|
|
|
|
// Disable auto bed leveling during G29
|
|
|
|
#endif // ABL_GRID
|
|
|
|
bool abl_should_enable = planner.abl_enabled;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
planner.abl_enabled = false;
|
|
|
|
if (verbose_level > 0) {
|
|
|
|
|
|
|
|
SERIAL_PROTOCOLLNPGM("G29 Auto Bed Leveling");
|
|
|
|
|
|
|
|
if (dryrun) SERIAL_PROTOCOLLNPGM("Running in DRY-RUN mode");
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if (!dryrun) {
|
|
|
|
stepper.synchronize();
|
|
|
|
// Re-orient the current position without leveling
|
|
|
|
|
|
|
|
// based on where the steppers are positioned.
|
|
|
|
|
|
|
|
set_current_from_steppers_for_axis(ALL_AXES);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Sync the planner to where the steppers stopped
|
|
|
|
// Disable auto bed leveling during G29
|
|
|
|
SYNC_PLAN_POSITION_KINEMATIC();
|
|
|
|
planner.abl_enabled = false;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
setup_for_endstop_or_probe_move();
|
|
|
|
if (!dryrun) {
|
|
|
|
|
|
|
|
// Re-orient the current position without leveling
|
|
|
|
|
|
|
|
// based on where the steppers are positioned.
|
|
|
|
|
|
|
|
set_current_from_steppers_for_axis(ALL_AXES);
|
|
|
|
|
|
|
|
|
|
|
|
// Deploy the probe. Probe will raise if needed.
|
|
|
|
// Sync the planner to where the steppers stopped
|
|
|
|
if (DEPLOY_PROBE()) {
|
|
|
|
SYNC_PLAN_POSITION_KINEMATIC();
|
|
|
|
planner.abl_enabled = abl_should_enable;
|
|
|
|
}
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
float xProbe = 0, yProbe = 0, measured_z = 0;
|
|
|
|
setup_for_endstop_or_probe_move();
|
|
|
|
|
|
|
|
|
|
|
|
#if ABL_GRID
|
|
|
|
//xProbe = yProbe = measured_z = 0;
|
|
|
|
|
|
|
|
|
|
|
|
// probe at the points of a lattice grid
|
|
|
|
#if HAS_BED_PROBE
|
|
|
|
const float xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (abl_grid_points_x - 1),
|
|
|
|
// Deploy the probe. Probe will raise if needed.
|
|
|
|
yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (abl_grid_points_y - 1);
|
|
|
|
if (DEPLOY_PROBE()) {
|
|
|
|
|
|
|
|
planner.abl_enabled = abl_should_enable;
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
|
|
|
|
|
|
|
|
|
|
|
float zoffset = zprobe_zoffset;
|
|
|
|
|
|
|
|
if (code_seen('Z')) zoffset += code_value_axis_units(Z_AXIS);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if ( xGridSpacing != bilinear_grid_spacing[X_AXIS]
|
|
|
|
if ( xGridSpacing != bilinear_grid_spacing[X_AXIS]
|
|
|
|
|| yGridSpacing != bilinear_grid_spacing[Y_AXIS]
|
|
|
|
|| yGridSpacing != bilinear_grid_spacing[Y_AXIS]
|
|
|
|
|| left_probe_bed_position != bilinear_start[X_AXIS]
|
|
|
|
|| left_probe_bed_position != LOGICAL_X_POSITION(bilinear_start[X_AXIS])
|
|
|
|
|| front_probe_bed_position != bilinear_start[Y_AXIS]
|
|
|
|
|| front_probe_bed_position != LOGICAL_Y_POSITION(bilinear_start[Y_AXIS])
|
|
|
|
) {
|
|
|
|
) {
|
|
|
|
if (dryrun) {
|
|
|
|
if (dryrun) {
|
|
|
|
// Before reset bed level, re-enable to correct the position
|
|
|
|
// Before reset bed level, re-enable to correct the position
|
|
|
@ -4101,164 +4270,311 @@ inline void gcode_G28() {
|
|
|
|
// Reset grid to 0.0 or "not probed". (Also disables ABL)
|
|
|
|
// Reset grid to 0.0 or "not probed". (Also disables ABL)
|
|
|
|
reset_bed_level();
|
|
|
|
reset_bed_level();
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
|
|
|
|
// Initialize a grid with the given dimensions
|
|
|
|
bilinear_grid_spacing_virt[X_AXIS] = xGridSpacing / (BILINEAR_SUBDIVISIONS);
|
|
|
|
|
|
|
|
bilinear_grid_spacing_virt[Y_AXIS] = yGridSpacing / (BILINEAR_SUBDIVISIONS);
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
bilinear_grid_spacing[X_AXIS] = xGridSpacing;
|
|
|
|
bilinear_grid_spacing[X_AXIS] = xGridSpacing;
|
|
|
|
bilinear_grid_spacing[Y_AXIS] = yGridSpacing;
|
|
|
|
bilinear_grid_spacing[Y_AXIS] = yGridSpacing;
|
|
|
|
bilinear_start[X_AXIS] = RAW_X_POSITION(left_probe_bed_position);
|
|
|
|
bilinear_start[X_AXIS] = RAW_X_POSITION(left_probe_bed_position);
|
|
|
|
bilinear_start[Y_AXIS] = RAW_Y_POSITION(front_probe_bed_position);
|
|
|
|
bilinear_start[Y_AXIS] = RAW_Y_POSITION(front_probe_bed_position);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
|
|
|
|
|
|
|
|
bilinear_grid_spacing_virt[X_AXIS] = xGridSpacing / (BILINEAR_SUBDIVISIONS);
|
|
|
|
|
|
|
|
bilinear_grid_spacing_virt[Y_AXIS] = yGridSpacing / (BILINEAR_SUBDIVISIONS);
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
// Can't re-enable (on error) until the new grid is written
|
|
|
|
// Can't re-enable (on error) until the new grid is written
|
|
|
|
abl_should_enable = false;
|
|
|
|
abl_should_enable = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
#elif ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
|
|
#elif ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
mean = 0.0;
|
|
|
|
* solve the plane equation ax + by + d = z
|
|
|
|
|
|
|
|
* A is the matrix with rows [x y 1] for all the probed points
|
|
|
|
|
|
|
|
* B is the vector of the Z positions
|
|
|
|
|
|
|
|
* 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
|
|
|
|
|
|
|
|
* so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
|
|
|
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
const int abl2 = abl_grid_points_x * abl_grid_points_y;
|
|
|
|
#endif // AUTO_BED_LEVELING_LINEAR
|
|
|
|
|
|
|
|
|
|
|
|
int indexIntoAB[abl_grid_points_x][abl_grid_points_y],
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_3POINT)
|
|
|
|
probe_index = -1;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
float eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations
|
|
|
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
|
|
eqnBVector[abl2], // "B" vector of Z points
|
|
|
|
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("> 3-point Leveling");
|
|
|
|
mean = 0.0;
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
#endif // AUTO_BED_LEVELING_LINEAR
|
|
|
|
// Probe at 3 arbitrary points
|
|
|
|
|
|
|
|
points[0].z = points[1].z = points[2].z = 0;
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(PROBE_Y_FIRST)
|
|
|
|
#endif // AUTO_BED_LEVELING_3POINT
|
|
|
|
#define PR_OUTER_VAR xCount
|
|
|
|
|
|
|
|
#define PR_OUTER_NUM abl_grid_points_x
|
|
|
|
|
|
|
|
#define PR_INNER_VAR yCount
|
|
|
|
|
|
|
|
#define PR_INNER_NUM abl_grid_points_y
|
|
|
|
|
|
|
|
#else
|
|
|
|
|
|
|
|
#define PR_OUTER_VAR yCount
|
|
|
|
|
|
|
|
#define PR_OUTER_NUM abl_grid_points_y
|
|
|
|
|
|
|
|
#define PR_INNER_VAR xCount
|
|
|
|
|
|
|
|
#define PR_INNER_NUM abl_grid_points_x
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
bool zig = PR_OUTER_NUM & 1; // Always end at RIGHT and BACK_PROBE_BED_POSITION
|
|
|
|
} // !g29_in_progress
|
|
|
|
|
|
|
|
|
|
|
|
// Outer loop is Y with PROBE_Y_FIRST disabled
|
|
|
|
#if ENABLED(PROBE_MANUALLY)
|
|
|
|
for (uint8_t PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_NUM; PR_OUTER_VAR++) {
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int8_t inStart, inStop, inInc;
|
|
|
|
// Abort current G29 procedure, go back to ABLStart
|
|
|
|
|
|
|
|
if (code_seen('A') && g29_in_progress) {
|
|
|
|
|
|
|
|
SERIAL_PROTOCOLLNPGM("Manual G29 aborted");
|
|
|
|
|
|
|
|
#if HAS_SOFTWARE_ENDSTOPS
|
|
|
|
|
|
|
|
soft_endstops_enabled = enable_soft_endstops;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
planner.abl_enabled = abl_should_enable;
|
|
|
|
|
|
|
|
g29_in_progress = false;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if (zig) { // away from origin
|
|
|
|
// Query G29 status
|
|
|
|
inStart = 0;
|
|
|
|
if (code_seen('Q')) {
|
|
|
|
inStop = PR_INNER_NUM;
|
|
|
|
if (!g29_in_progress)
|
|
|
|
inInc = 1;
|
|
|
|
SERIAL_PROTOCOLLNPGM("Manual G29 idle");
|
|
|
|
}
|
|
|
|
else {
|
|
|
|
else { // towards origin
|
|
|
|
SERIAL_PROTOCOLPAIR("Manual G29 point ", abl_probe_index + 1);
|
|
|
|
inStart = PR_INNER_NUM - 1;
|
|
|
|
SERIAL_PROTOCOLLNPAIR(" of ", abl2);
|
|
|
|
inStop = -1;
|
|
|
|
|
|
|
|
inInc = -1;
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (code_seen('A') || code_seen('Q')) return;
|
|
|
|
|
|
|
|
|
|
|
|
zig = !zig; // zag
|
|
|
|
// Fall through to probe the first point
|
|
|
|
|
|
|
|
g29_in_progress = true;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (abl_probe_index == 0) {
|
|
|
|
|
|
|
|
// For the initial G29 S2 save software endstop state
|
|
|
|
|
|
|
|
#if HAS_SOFTWARE_ENDSTOPS
|
|
|
|
|
|
|
|
enable_soft_endstops = soft_endstops_enabled;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else {
|
|
|
|
|
|
|
|
// For G29 after adjusting Z.
|
|
|
|
|
|
|
|
// Save the previous Z before going to the next point
|
|
|
|
|
|
|
|
measured_z = current_position[Z_AXIS];
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
mean += measured_z;
|
|
|
|
|
|
|
|
eqnBVector[abl_probe_index] = measured_z;
|
|
|
|
|
|
|
|
eqnAMatrix[abl_probe_index + 0 * abl2] = xProbe;
|
|
|
|
|
|
|
|
eqnAMatrix[abl_probe_index + 1 * abl2] = yProbe;
|
|
|
|
|
|
|
|
eqnAMatrix[abl_probe_index + 2 * abl2] = 1;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
|
|
|
|
|
|
|
|
|
|
|
// Inner loop is Y with PROBE_Y_FIRST enabled
|
|
|
|
bed_level_grid[xCount][yCount] = measured_z + zoffset;
|
|
|
|
for (int8_t PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; PR_INNER_VAR += inInc) {
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
float xBase = left_probe_bed_position + xGridSpacing * xCount,
|
|
|
|
#elif ENABLED(AUTO_BED_LEVELING_3POINT)
|
|
|
|
yBase = front_probe_bed_position + yGridSpacing * yCount;
|
|
|
|
|
|
|
|
|
|
|
|
points[i].z = measured_z;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//
|
|
|
|
|
|
|
|
// If there's another point to sample, move there with optional lift.
|
|
|
|
|
|
|
|
//
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ABL_GRID
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Find a next point to probe
|
|
|
|
|
|
|
|
// On the first G29 this will be the first probe point
|
|
|
|
|
|
|
|
while (abl_probe_index < abl2) {
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Set xCount, yCount based on abl_probe_index, with zig-zag
|
|
|
|
|
|
|
|
PR_OUTER_VAR = abl_probe_index / PR_INNER_END;
|
|
|
|
|
|
|
|
PR_INNER_VAR = abl_probe_index - (PR_OUTER_VAR * PR_INNER_END);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
bool zig = (PR_OUTER_VAR & 1) != ((PR_OUTER_END) & 1);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (zig) PR_INNER_VAR = (PR_INNER_END - 1) - PR_INNER_VAR;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
const float xBase = left_probe_bed_position + xGridSpacing * xCount,
|
|
|
|
|
|
|
|
yBase = front_probe_bed_position + yGridSpacing * yCount;
|
|
|
|
|
|
|
|
|
|
|
|
xProbe = floor(xBase + (xBase < 0 ? 0 : 0.5));
|
|
|
|
xProbe = floor(xBase + (xBase < 0 ? 0 : 0.5));
|
|
|
|
yProbe = floor(yBase + (yBase < 0 ? 0 : 0.5));
|
|
|
|
yProbe = floor(yBase + (yBase < 0 ? 0 : 0.5));
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
|
|
indexIntoAB[xCount][yCount] = ++probe_index;
|
|
|
|
indexIntoAB[xCount][yCount] = abl_probe_index;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
float pos[XYZ] = { xProbe, yProbe, 0 };
|
|
|
|
|
|
|
|
if (position_is_reachable(pos)) break;
|
|
|
|
|
|
|
|
++abl_probe_index;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Is there a next point to move to?
|
|
|
|
|
|
|
|
if (abl_probe_index < abl2) {
|
|
|
|
|
|
|
|
_manual_goto_xy(xProbe, yProbe); // Can be used here too!
|
|
|
|
|
|
|
|
++abl_probe_index;
|
|
|
|
|
|
|
|
#if HAS_SOFTWARE_ENDSTOPS
|
|
|
|
|
|
|
|
// Disable software endstops to allow manual adjustment
|
|
|
|
|
|
|
|
// If G29 is not completed, they will not be re-enabled
|
|
|
|
|
|
|
|
soft_endstops_enabled = false;
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else {
|
|
|
|
|
|
|
|
// Then leveling is done!
|
|
|
|
|
|
|
|
// G29 finishing code goes here
|
|
|
|
|
|
|
|
|
|
|
|
#if IS_KINEMATIC
|
|
|
|
// After recording the last point, activate abl
|
|
|
|
// Avoid probing outside the round or hexagonal area
|
|
|
|
SERIAL_PROTOCOLLNPGM("Grid probing done.");
|
|
|
|
float pos[XYZ] = { xProbe, yProbe, 0 };
|
|
|
|
g29_in_progress = false;
|
|
|
|
if (!position_is_reachable(pos, true)) continue;
|
|
|
|
|
|
|
|
|
|
|
|
// Re-enable software endstops, if needed
|
|
|
|
|
|
|
|
#if HAS_SOFTWARE_ENDSTOPS
|
|
|
|
|
|
|
|
soft_endstops_enabled = enable_soft_endstops;
|
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
measured_z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
|
|
|
|
#elif ENABLED(AUTO_BED_LEVELING_3POINT)
|
|
|
|
|
|
|
|
|
|
|
|
if (measured_z == NAN) {
|
|
|
|
// Probe at 3 arbitrary points
|
|
|
|
planner.abl_enabled = abl_should_enable;
|
|
|
|
if (abl_probe_index < 3) {
|
|
|
|
return;
|
|
|
|
xProbe = LOGICAL_X_POSITION(points[i].x);
|
|
|
|
|
|
|
|
yProbe = LOGICAL_Y_POSITION(points[i].y);
|
|
|
|
|
|
|
|
++abl_probe_index;
|
|
|
|
|
|
|
|
#if HAS_SOFTWARE_ENDSTOPS
|
|
|
|
|
|
|
|
// Disable software endstops to allow manual adjustment
|
|
|
|
|
|
|
|
// If G29 is not completed, they will not be re-enabled
|
|
|
|
|
|
|
|
soft_endstops_enabled = false;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
else {
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
SERIAL_PROTOCOLLNPGM("3-point probing done.");
|
|
|
|
|
|
|
|
g29_in_progress = false;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Re-enable software endstops, if needed
|
|
|
|
|
|
|
|
#if HAS_SOFTWARE_ENDSTOPS
|
|
|
|
|
|
|
|
soft_endstops_enabled = enable_soft_endstops;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (!dryrun) {
|
|
|
|
|
|
|
|
vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
|
|
|
|
|
|
|
|
if (planeNormal.z < 0) {
|
|
|
|
|
|
|
|
planeNormal.x *= -1;
|
|
|
|
|
|
|
|
planeNormal.y *= -1;
|
|
|
|
|
|
|
|
planeNormal.z *= -1;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Can't re-enable (on error) until the new grid is written
|
|
|
|
|
|
|
|
abl_should_enable = false;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
mean += measured_z;
|
|
|
|
#endif // AUTO_BED_LEVELING_3POINT
|
|
|
|
eqnBVector[probe_index] = measured_z;
|
|
|
|
|
|
|
|
eqnAMatrix[probe_index + 0 * abl2] = xProbe;
|
|
|
|
|
|
|
|
eqnAMatrix[probe_index + 1 * abl2] = yProbe;
|
|
|
|
|
|
|
|
eqnAMatrix[probe_index + 2 * abl2] = 1;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
bed_level_grid[xCount][yCount] = measured_z + zoffset;
|
|
|
|
#else // !PROBE_MANUALLY
|
|
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
idle();
|
|
|
|
bool stow_probe_after_each = code_seen('E');
|
|
|
|
|
|
|
|
|
|
|
|
} // inner
|
|
|
|
#if ABL_GRID
|
|
|
|
} // outer
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#elif ENABLED(AUTO_BED_LEVELING_3POINT)
|
|
|
|
bool zig = PR_OUTER_END & 1; // Always end at RIGHT and BACK_PROBE_BED_POSITION
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(DEBUG_LEVELING_FEATURE)
|
|
|
|
// Outer loop is Y with PROBE_Y_FIRST disabled
|
|
|
|
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("> 3-point Leveling");
|
|
|
|
for (uint8_t PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END; PR_OUTER_VAR++) {
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Probe at 3 arbitrary points
|
|
|
|
int8_t inStart, inStop, inInc;
|
|
|
|
vector_3 points[3] = {
|
|
|
|
|
|
|
|
vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, 0),
|
|
|
|
|
|
|
|
vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, 0),
|
|
|
|
|
|
|
|
vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, 0)
|
|
|
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
for (uint8_t i = 0; i < 3; ++i) {
|
|
|
|
if (zig) { // away from origin
|
|
|
|
// Retain the last probe position
|
|
|
|
inStart = 0;
|
|
|
|
xProbe = LOGICAL_X_POSITION(points[i].x);
|
|
|
|
inStop = PR_INNER_END;
|
|
|
|
yProbe = LOGICAL_Y_POSITION(points[i].y);
|
|
|
|
inInc = 1;
|
|
|
|
measured_z = points[i].z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
else { // towards origin
|
|
|
|
|
|
|
|
inStart = PR_INNER_END - 1;
|
|
|
|
|
|
|
|
inStop = -1;
|
|
|
|
|
|
|
|
inInc = -1;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
if (measured_z == NAN) {
|
|
|
|
zig = !zig; // zag
|
|
|
|
planner.abl_enabled = abl_should_enable;
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (!dryrun) {
|
|
|
|
// Inner loop is Y with PROBE_Y_FIRST enabled
|
|
|
|
vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
|
|
|
|
for (int8_t PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; PR_INNER_VAR += inInc) {
|
|
|
|
if (planeNormal.z < 0) {
|
|
|
|
|
|
|
|
planeNormal.x *= -1;
|
|
|
|
float xBase = left_probe_bed_position + xGridSpacing * xCount,
|
|
|
|
planeNormal.y *= -1;
|
|
|
|
yBase = front_probe_bed_position + yGridSpacing * yCount;
|
|
|
|
planeNormal.z *= -1;
|
|
|
|
|
|
|
|
|
|
|
|
xProbe = floor(xBase + (xBase < 0 ? 0 : 0.5));
|
|
|
|
|
|
|
|
yProbe = floor(yBase + (yBase < 0 ? 0 : 0.5));
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
|
|
|
|
|
|
indexIntoAB[xCount][yCount] = ++abl_probe_index;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if IS_KINEMATIC
|
|
|
|
|
|
|
|
// Avoid probing outside the round or hexagonal area
|
|
|
|
|
|
|
|
float pos[XYZ] = { xProbe, yProbe, 0 };
|
|
|
|
|
|
|
|
if (!position_is_reachable(pos, true)) continue;
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
measured_z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (measured_z == NAN) {
|
|
|
|
|
|
|
|
planner.abl_enabled = abl_should_enable;
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
mean += measured_z;
|
|
|
|
|
|
|
|
eqnBVector[abl_probe_index] = measured_z;
|
|
|
|
|
|
|
|
eqnAMatrix[abl_probe_index + 0 * abl2] = xProbe;
|
|
|
|
|
|
|
|
eqnAMatrix[abl_probe_index + 1 * abl2] = yProbe;
|
|
|
|
|
|
|
|
eqnAMatrix[abl_probe_index + 2 * abl2] = 1;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
bed_level_grid[xCount][yCount] = measured_z + zoffset;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
abl_should_enable = false;
|
|
|
|
|
|
|
|
idle();
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
} // inner
|
|
|
|
|
|
|
|
} // outer
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#elif ENABLED(AUTO_BED_LEVELING_3POINT)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Probe at 3 arbitrary points
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
for (uint8_t i = 0; i < 3; ++i) {
|
|
|
|
|
|
|
|
// Retain the last probe position
|
|
|
|
|
|
|
|
xProbe = LOGICAL_X_POSITION(points[i].x);
|
|
|
|
|
|
|
|
yProbe = LOGICAL_Y_POSITION(points[i].y);
|
|
|
|
|
|
|
|
measured_z = points[i].z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Can't re-enable (on error) until the new grid is written
|
|
|
|
if (measured_z == NAN) {
|
|
|
|
abl_should_enable = false;
|
|
|
|
planner.abl_enabled = abl_should_enable;
|
|
|
|
}
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
#endif // AUTO_BED_LEVELING_3POINT
|
|
|
|
if (!dryrun) {
|
|
|
|
|
|
|
|
vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
|
|
|
|
|
|
|
|
if (planeNormal.z < 0) {
|
|
|
|
|
|
|
|
planeNormal.x *= -1;
|
|
|
|
|
|
|
|
planeNormal.y *= -1;
|
|
|
|
|
|
|
|
planeNormal.z *= -1;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
|
|
|
|
|
|
|
|
|
|
|
// Raise to _Z_CLEARANCE_DEPLOY_PROBE. Stow the probe.
|
|
|
|
// Can't re-enable (on error) until the new grid is written
|
|
|
|
if (STOW_PROBE()) {
|
|
|
|
abl_should_enable = false;
|
|
|
|
planner.abl_enabled = abl_should_enable;
|
|
|
|
}
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
#endif // AUTO_BED_LEVELING_3POINT
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// Raise to _Z_CLEARANCE_DEPLOY_PROBE. Stow the probe.
|
|
|
|
|
|
|
|
if (STOW_PROBE()) {
|
|
|
|
|
|
|
|
planner.abl_enabled = abl_should_enable;
|
|
|
|
|
|
|
|
return;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif // !PROBE_MANUALLY
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//
|
|
|
|
|
|
|
|
// G29 Finishing Code
|
|
|
|
//
|
|
|
|
//
|
|
|
|
// Unless this is a dry run, auto bed leveling will
|
|
|
|
// Unless this is a dry run, auto bed leveling will
|
|
|
|
// definitely be enabled after this point
|
|
|
|
// definitely be enabled after this point
|
|
|
@ -4286,7 +4602,14 @@ inline void gcode_G28() {
|
|
|
|
|
|
|
|
|
|
|
|
// For LINEAR leveling calculate matrix, print reports, correct the position
|
|
|
|
// For LINEAR leveling calculate matrix, print reports, correct the position
|
|
|
|
|
|
|
|
|
|
|
|
// solve lsq problem
|
|
|
|
/**
|
|
|
|
|
|
|
|
* solve the plane equation ax + by + d = z
|
|
|
|
|
|
|
|
* A is the matrix with rows [x y 1] for all the probed points
|
|
|
|
|
|
|
|
* B is the vector of the Z positions
|
|
|
|
|
|
|
|
* 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
|
|
|
|
|
|
|
|
* so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
|
|
|
|
|
|
|
|
*/
|
|
|
|
float plane_equation_coefficients[3];
|
|
|
|
float plane_equation_coefficients[3];
|
|
|
|
qr_solve(plane_equation_coefficients, abl2, 3, eqnAMatrix, eqnBVector);
|
|
|
|
qr_solve(plane_equation_coefficients, abl2, 3, eqnAMatrix, eqnBVector);
|
|
|
|
|
|
|
|
|
|
|
|