From 40d3e854f6c55c7a13f245def54a0a09e4c69559 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 15 Sep 2016 00:27:31 -0500 Subject: [PATCH 1/4] Remove redundant extern delta[] --- Marlin/Marlin.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h index 08088f7bfa..8304706c78 100644 --- a/Marlin/Marlin.h +++ b/Marlin/Marlin.h @@ -307,8 +307,7 @@ float code_value_temp_diff(); #endif #if ENABLED(DELTA) - extern float delta[ABC], - endstop_adj[ABC], + extern float endstop_adj[ABC], delta_radius, delta_diagonal_rod, delta_segments_per_second, From 5f2f991192f6655d274c6260324a1aa0080fd7d3 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 15 Sep 2016 02:31:36 -0500 Subject: [PATCH 2/4] Use 'logical' rather than 'target' or 'cartesian' --- Marlin/Marlin.h | 2 +- Marlin/Marlin_main.cpp | 52 +++++++++++++++++++++--------------------- 2 files changed, 27 insertions(+), 27 deletions(-) diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h index 8304706c78..9427cf6547 100644 --- a/Marlin/Marlin.h +++ b/Marlin/Marlin.h @@ -303,7 +303,7 @@ float code_value_temp_diff(); #if IS_KINEMATIC extern float delta[ABC]; - void inverse_kinematics(const float cartesian[XYZ]); + void inverse_kinematics(const float logical[XYZ]); #endif #if ENABLED(DELTA) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 913f6dae01..3a1cdafb08 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -7992,9 +7992,9 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { * This calls planner.buffer_line several times, adding * small incremental moves for DELTA or SCARA. */ - inline bool prepare_kinematic_move_to(float target[NUM_AXIS]) { + inline bool prepare_kinematic_move_to(float logical[NUM_AXIS]) { float difference[NUM_AXIS]; - LOOP_XYZE(i) difference[i] = target[i] - current_position[i]; + LOOP_XYZE(i) difference[i] = logical[i] - current_position[i]; float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS])); if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = abs(difference[E_AXIS]); @@ -8013,18 +8013,18 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { float fraction = float(s) * inv_steps; LOOP_XYZE(i) - target[i] = current_position[i] + difference[i] * fraction; + logical[i] = current_position[i] + difference[i] * fraction; - inverse_kinematics(target); + inverse_kinematics(logical); #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR) - if (!bed_leveling_in_progress) adjust_delta(target); + if (!bed_leveling_in_progress) adjust_delta(logical); #endif - //DEBUG_POS("prepare_kinematic_move_to", target); + //DEBUG_POS("prepare_kinematic_move_to", logical); //DEBUG_POS("prepare_kinematic_move_to", delta); - planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], _feedrate_mm_s, active_extruder); + planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder); } return true; } @@ -8156,20 +8156,20 @@ void prepare_move_to_destination() { * options for G2/G3 arc generation. In future these options may be GCode tunable. */ void plan_arc( - float target[NUM_AXIS], // Destination position - float* offset, // Center of rotation relative to current_position - uint8_t clockwise // Clockwise? + float logical[NUM_AXIS], // Destination position + float* offset, // Center of rotation relative to current_position + uint8_t clockwise // Clockwise? ) { float radius = HYPOT(offset[X_AXIS], offset[Y_AXIS]), center_X = current_position[X_AXIS] + offset[X_AXIS], center_Y = current_position[Y_AXIS] + offset[Y_AXIS], - linear_travel = target[Z_AXIS] - current_position[Z_AXIS], - extruder_travel = target[E_AXIS] - current_position[E_AXIS], + linear_travel = logical[Z_AXIS] - current_position[Z_AXIS], + extruder_travel = logical[E_AXIS] - current_position[E_AXIS], r_X = -offset[X_AXIS], // Radius vector from center to current location r_Y = -offset[Y_AXIS], - rt_X = target[X_AXIS] - center_X, - rt_Y = target[Y_AXIS] - center_Y; + rt_X = logical[X_AXIS] - center_X, + rt_Y = logical[Y_AXIS] - center_Y; // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required. float angular_travel = atan2(r_X * rt_Y - r_Y * rt_X, r_X * rt_X + r_Y * rt_Y); @@ -8177,7 +8177,7 @@ void prepare_move_to_destination() { if (clockwise) angular_travel -= RADIANS(360); // Make a circle if the angular rotation is 0 - if (angular_travel == 0 && current_position[X_AXIS] == target[X_AXIS] && current_position[Y_AXIS] == target[Y_AXIS]) + if (angular_travel == 0 && current_position[X_AXIS] == logical[X_AXIS] && current_position[Y_AXIS] == logical[Y_AXIS]) angular_travel += RADIANS(360); float mm_of_travel = HYPOT(angular_travel * radius, fabs(linear_travel)); @@ -8282,13 +8282,13 @@ void prepare_move_to_destination() { // Ensure last segment arrives at target location. #if IS_KINEMATIC - inverse_kinematics(target); + inverse_kinematics(logical); #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR) - adjust_delta(target); + adjust_delta(logical); #endif - planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], fr_mm_s, active_extruder); + planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], logical[E_AXIS], fr_mm_s, active_extruder); #else - planner.buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], fr_mm_s, active_extruder); + planner.buffer_line(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS], logical[E_AXIS], fr_mm_s, active_extruder); #endif // As far as the parser is concerned, the position is now == target. In reality the @@ -8303,7 +8303,7 @@ void prepare_move_to_destination() { void plan_cubic_move(const float offset[4]) { cubic_b_spline(current_position, destination, offset, MMS_SCALED(feedrate_mm_s), active_extruder); - // As far as the parser is concerned, the position is now == target. In reality the + // As far as the parser is concerned, the position is now == destination. In reality the // motion control system might still be processing the action and the real tool position // in any intermediate location. set_current_to_destination(); @@ -8376,7 +8376,7 @@ void prepare_move_to_destination() { //*/ } - void inverse_kinematics(const float cartesian[XYZ]) { + void inverse_kinematics(const float logical[XYZ]) { // Inverse kinematics. // Perform SCARA IK and place results in delta[]. // The maths and first version were done by QHARLEY. @@ -8384,8 +8384,8 @@ void prepare_move_to_destination() { static float C2, S2, SK1, SK2, THETA, PSI; - float sx = RAW_X_POSITION(cartesian[X_AXIS]) - SCARA_OFFSET_X, //Translate SCARA to standard X Y - sy = RAW_Y_POSITION(cartesian[Y_AXIS]) - SCARA_OFFSET_Y; // With scaling factor. + float sx = RAW_X_POSITION(logical[X_AXIS]) - SCARA_OFFSET_X, // Translate SCARA to standard X Y + sy = RAW_Y_POSITION(logical[Y_AXIS]) - SCARA_OFFSET_Y; // With scaling factor. #if (L1 == L2) C2 = HYPOT2(sx, sy) / (2 * L1_2) - 1; @@ -8403,10 +8403,10 @@ void prepare_move_to_destination() { delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor) - delta[Z_AXIS] = cartesian[Z_AXIS]; + delta[Z_AXIS] = logical[Z_AXIS]; - /** - DEBUG_POS("SCARA IK", cartesian); + /* + DEBUG_POS("SCARA IK", logical); DEBUG_POS("SCARA IK", delta); SERIAL_ECHOPAIR(" SCARA (x,y) ", sx); SERIAL_ECHOPAIR(",", sy); From 9429c7db89201dafe162bbf5382a5db1b594aef6 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 15 Sep 2016 01:21:31 -0500 Subject: [PATCH 3/4] Use ABC indices in delta[] --- Marlin/Marlin_main.cpp | 39 ++++++++++++++++++++------------------- Marlin/planner.cpp | 2 +- Marlin/ultralcd.cpp | 4 ++-- 3 files changed, 23 insertions(+), 22 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 3a1cdafb08..d7e9d49bb2 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -658,16 +658,20 @@ inline void sync_plan_position() { inline void sync_plan_position_e() { planner.set_e_position_mm(current_position[E_AXIS]); } #if IS_KINEMATIC + inline void sync_plan_position_kinematic() { #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position); #endif inverse_kinematics(current_position); - planner.set_position_mm(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]); + planner.set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS]); } #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_kinematic() + #else + #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position() + #endif #if ENABLED(SDSUPPORT) @@ -795,7 +799,6 @@ void setup_homepin(void) { #endif } - void setup_photpin() { #if HAS_PHOTOGRAPH OUT_WRITE(PHOTOGRAPH_PIN, LOW); @@ -1479,7 +1482,7 @@ inline void set_destination_to_current() { memcpy(destination, current_position, #endif refresh_cmd_timeout(); inverse_kinematics(destination); - planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], MMS_SCALED(feedrate_mm_s), active_extruder); + planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], destination[E_AXIS], MMS_SCALED(feedrate_mm_s), active_extruder); set_current_to_destination(); } #endif @@ -5075,22 +5078,20 @@ static void report_current_position() { #if IS_SCARA SERIAL_PROTOCOLPGM("SCARA Theta:"); - SERIAL_PROTOCOL(delta[X_AXIS]); + SERIAL_PROTOCOL(delta[A_AXIS]); SERIAL_PROTOCOLPGM(" Psi+Theta:"); - SERIAL_PROTOCOL(delta[Y_AXIS]); - SERIAL_EOL; + SERIAL_PROTOCOLLN(delta[B_AXIS]); SERIAL_PROTOCOLPGM("SCARA Cal - Theta:"); - SERIAL_PROTOCOL(delta[X_AXIS]); + SERIAL_PROTOCOL(delta[A_AXIS]); SERIAL_PROTOCOLPGM(" Psi+Theta (90):"); - SERIAL_PROTOCOL(delta[Y_AXIS] - delta[X_AXIS] - 90); - SERIAL_EOL; + SERIAL_PROTOCOLLN(delta[B_AXIS] - delta[A_AXIS] - 90); SERIAL_PROTOCOLPGM("SCARA step Cal - Theta:"); - SERIAL_PROTOCOL(delta[X_AXIS] / 90 * planner.axis_steps_per_mm[X_AXIS]); + SERIAL_PROTOCOL(delta[A_AXIS] / 90 * planner.axis_steps_per_mm[A_AXIS]); SERIAL_PROTOCOLPGM(" Psi+Theta:"); - SERIAL_PROTOCOL((delta[Y_AXIS] - delta[X_AXIS]) / 90 * planner.axis_steps_per_mm[Y_AXIS]); - SERIAL_EOL; SERIAL_EOL; + SERIAL_PROTOCOLLN((delta[B_AXIS] - delta[A_AXIS]) / 90 * planner.axis_steps_per_mm[A_AXIS]); + SERIAL_EOL; #endif } @@ -6160,7 +6161,7 @@ inline void gcode_M503() { // Define runplan for move axes #if IS_KINEMATIC #define RUNPLAN(RATE_MM_S) inverse_kinematics(destination); \ - planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], RATE_MM_S, active_extruder); + planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], destination[E_AXIS], RATE_MM_S, active_extruder); #else #define RUNPLAN(RATE_MM_S) line_to_destination(RATE_MM_S); #endif @@ -6282,8 +6283,8 @@ inline void gcode_M503() { #if IS_KINEMATIC // Move XYZ to starting position, then E inverse_kinematics(lastpos); - planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder); - planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], lastpos[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder); + planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], destination[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder); + planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], lastpos[E_AXIS], FILAMENT_CHANGE_XY_FEEDRATE, active_extruder); #else // Move XY to starting position, then Z, then E destination[X_AXIS] = lastpos[X_AXIS]; @@ -8024,7 +8025,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { //DEBUG_POS("prepare_kinematic_move_to", logical); //DEBUG_POS("prepare_kinematic_move_to", delta); - planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder); + planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder); } return true; } @@ -8274,7 +8275,7 @@ void prepare_move_to_destination() { #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR) adjust_delta(arc_target); #endif - planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder); + planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder); #else planner.buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder); #endif @@ -8286,7 +8287,7 @@ void prepare_move_to_destination() { #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR) adjust_delta(logical); #endif - planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], logical[E_AXIS], fr_mm_s, active_extruder); + planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], fr_mm_s, active_extruder); #else planner.buffer_line(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS], logical[E_AXIS], fr_mm_s, active_extruder); #endif @@ -8403,7 +8404,7 @@ void prepare_move_to_destination() { delta[A_AXIS] = DEGREES(THETA); // theta is support arm angle delta[B_AXIS] = DEGREES(THETA + PSI); // equal to sub arm angle (inverted motor) - delta[Z_AXIS] = logical[Z_AXIS]; + delta[C_AXIS] = logical[Z_AXIS]; /* DEBUG_POS("SCARA IK", logical); diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 93084792f1..a37dc1d4a0 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -1205,7 +1205,7 @@ void Planner::refresh_positioning() { LOOP_XYZE(i) steps_to_mm[i] = 1.0 / axis_steps_per_mm[i]; #if IS_KINEMATIC inverse_kinematics(current_position); - set_position_mm(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]); + set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS]); #else set_position_mm(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); #endif diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp index caa8099f67..a1deff0326 100755 --- a/Marlin/ultralcd.cpp +++ b/Marlin/ultralcd.cpp @@ -547,7 +547,7 @@ void kill_screen(const char* lcd_msg) { inline void line_to_current(AxisEnum axis) { #if ENABLED(DELTA) inverse_kinematics(current_position); - planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder); + planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder); #else // !DELTA planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[axis]), active_extruder); #endif // !DELTA @@ -1297,7 +1297,7 @@ void kill_screen(const char* lcd_msg) { if (manual_move_axis != (int8_t)NO_AXIS && ELAPSED(millis(), manual_move_start_time) && !planner.is_full()) { #if ENABLED(DELTA) inverse_kinematics(current_position); - planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index); + planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index); #else planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_e_index); #endif From 77639672d71482c7e8b6b4156b3e8e2e630fe2b6 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 15 Sep 2016 00:20:30 -0500 Subject: [PATCH 4/4] Move NONLINEAR bed leveling to planner This is in advance of moving non-linear bed leveling to the planner class. --- Marlin/Conditionals_post.h | 2 +- Marlin/Marlin.h | 2 +- Marlin/Marlin_main.cpp | 105 ++++++++++++++++--------------------- Marlin/planner.cpp | 22 ++++++++ Marlin/planner_bezier.cpp | 5 +- 5 files changed, 71 insertions(+), 65 deletions(-) diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h index 9861c2b295..91b26ef1db 100644 --- a/Marlin/Conditionals_post.h +++ b/Marlin/Conditionals_post.h @@ -675,7 +675,7 @@ #endif #endif - #define PLANNER_LEVELING (ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_LINEAR)) + #define PLANNER_LEVELING (ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)) /** * Buzzer/Speaker diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h index 9427cf6547..f62dfef66b 100644 --- a/Marlin/Marlin.h +++ b/Marlin/Marlin.h @@ -321,7 +321,7 @@ float code_value_temp_diff(); #if ENABLED(AUTO_BED_LEVELING_NONLINEAR) extern int nonlinear_grid_spacing[2]; - void adjust_delta(float cartesian[XYZ]); + float nonlinear_z_offset(float logical[XYZ]); #endif #if ENABLED(Z_DUAL_ENDSTOPS) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index d7e9d49bb2..5b143f4afb 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -400,7 +400,6 @@ static uint8_t target_extruder; #if ENABLED(AUTO_BED_LEVELING_FEATURE) float xy_probe_feedrate_mm_s = MMM_TO_MMS(XY_PROBE_SPEED); - bool bed_leveling_in_progress = false; #define XY_PROBE_FEEDRATE_MM_S xy_probe_feedrate_mm_s #elif defined(XY_PROBE_SPEED) #define XY_PROBE_FEEDRATE_MM_S MMM_TO_MMS(XY_PROBE_SPEED) @@ -3434,8 +3433,6 @@ inline void gcode_G28() { // Deploy the probe. Probe will raise if needed. if (DEPLOY_PROBE()) return; - bed_leveling_in_progress = true; - float xProbe, yProbe, measured_z = 0; #if ENABLED(AUTO_BED_LEVELING_GRID) @@ -3576,6 +3573,8 @@ inline void gcode_G28() { #elif ENABLED(AUTO_BED_LEVELING_LINEAR) + // For LINEAR leveling calculate matrix, print reports, correct the position + // solve lsq problem double plane_equation_coefficients[3]; qr_solve(plane_equation_coefficients, abl2, 3, eqnAMatrix, eqnBVector); @@ -3669,6 +3668,8 @@ inline void gcode_G28() { } } //do_topography_map + // For LINEAR and 3POINT leveling correct the current position + if (verbose_level > 0) planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:"); @@ -3738,8 +3739,6 @@ inline void gcode_G28() { if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< gcode_G29"); #endif - bed_leveling_in_progress = false; - report_current_position(); KEEPALIVE_STATE(IN_HANDLER); @@ -7638,6 +7637,48 @@ void ok_to_send() { #endif +#if ENABLED(AUTO_BED_LEVELING_NONLINEAR) + + // Get the Z adjustment for non-linear bed leveling + float nonlinear_z_offset(float cartesian[XYZ]) { + if (nonlinear_grid_spacing[X_AXIS] == 0 || nonlinear_grid_spacing[Y_AXIS] == 0) return 0; // G29 not done! + + int half_x = (ABL_GRID_POINTS_X - 1) / 2, + half_y = (ABL_GRID_POINTS_Y - 1) / 2; + float hx2 = half_x - 0.001, hx1 = -hx2, + hy2 = half_y - 0.001, hy1 = -hy2, + grid_x = max(hx1, min(hx2, RAW_X_POSITION(cartesian[X_AXIS]) / nonlinear_grid_spacing[X_AXIS])), + grid_y = max(hy1, min(hy2, RAW_Y_POSITION(cartesian[Y_AXIS]) / nonlinear_grid_spacing[Y_AXIS])); + int floor_x = floor(grid_x), floor_y = floor(grid_y); + float ratio_x = grid_x - floor_x, ratio_y = grid_y - floor_y, + z1 = bed_level_grid[floor_x + half_x][floor_y + half_y], + z2 = bed_level_grid[floor_x + half_x][floor_y + half_y + 1], + z3 = bed_level_grid[floor_x + half_x + 1][floor_y + half_y], + z4 = bed_level_grid[floor_x + half_x + 1][floor_y + half_y + 1], + left = (1 - ratio_y) * z1 + ratio_y * z2, + right = (1 - ratio_y) * z3 + ratio_y * z4; + + /* + SERIAL_ECHOPAIR("grid_x=", grid_x); + SERIAL_ECHOPAIR(" grid_y=", grid_y); + SERIAL_ECHOPAIR(" floor_x=", floor_x); + SERIAL_ECHOPAIR(" floor_y=", floor_y); + SERIAL_ECHOPAIR(" ratio_x=", ratio_x); + SERIAL_ECHOPAIR(" ratio_y=", ratio_y); + SERIAL_ECHOPAIR(" z1=", z1); + SERIAL_ECHOPAIR(" z2=", z2); + SERIAL_ECHOPAIR(" z3=", z3); + SERIAL_ECHOPAIR(" z4=", z4); + SERIAL_ECHOPAIR(" left=", left); + SERIAL_ECHOPAIR(" right=", right); + SERIAL_ECHOPAIR(" offset=", (1 - ratio_x) * left + ratio_x * right); + //*/ + + return (1 - ratio_x) * left + ratio_x * right; + } + +#endif // AUTO_BED_LEVELING_NONLINEAR + #if ENABLED(DELTA) /** @@ -7828,50 +7869,6 @@ void ok_to_send() { forward_kinematics_DELTA(point[A_AXIS], point[B_AXIS], point[C_AXIS]); } - #if ENABLED(AUTO_BED_LEVELING_NONLINEAR) - - // Adjust print surface height by linear interpolation over the bed_level array. - void adjust_delta(float cartesian[XYZ]) { - if (nonlinear_grid_spacing[X_AXIS] == 0 || nonlinear_grid_spacing[Y_AXIS] == 0) return; // G29 not done! - - int half_x = (ABL_GRID_POINTS_X - 1) / 2, - half_y = (ABL_GRID_POINTS_Y - 1) / 2; - float hx2 = half_x - 0.001, hx1 = -hx2, - hy2 = half_y - 0.001, hy1 = -hy2, - grid_x = max(hx1, min(hx2, RAW_X_POSITION(cartesian[X_AXIS]) / nonlinear_grid_spacing[X_AXIS])), - grid_y = max(hy1, min(hy2, RAW_Y_POSITION(cartesian[Y_AXIS]) / nonlinear_grid_spacing[Y_AXIS])); - int floor_x = floor(grid_x), floor_y = floor(grid_y); - float ratio_x = grid_x - floor_x, ratio_y = grid_y - floor_y, - z1 = bed_level_grid[floor_x + half_x][floor_y + half_y], - z2 = bed_level_grid[floor_x + half_x][floor_y + half_y + 1], - z3 = bed_level_grid[floor_x + half_x + 1][floor_y + half_y], - z4 = bed_level_grid[floor_x + half_x + 1][floor_y + half_y + 1], - left = (1 - ratio_y) * z1 + ratio_y * z2, - right = (1 - ratio_y) * z3 + ratio_y * z4, - offset = (1 - ratio_x) * left + ratio_x * right; - - delta[X_AXIS] += offset; - delta[Y_AXIS] += offset; - delta[Z_AXIS] += offset; - - /** - SERIAL_ECHOPAIR("grid_x=", grid_x); - SERIAL_ECHOPAIR(" grid_y=", grid_y); - SERIAL_ECHOPAIR(" floor_x=", floor_x); - SERIAL_ECHOPAIR(" floor_y=", floor_y); - SERIAL_ECHOPAIR(" ratio_x=", ratio_x); - SERIAL_ECHOPAIR(" ratio_y=", ratio_y); - SERIAL_ECHOPAIR(" z1=", z1); - SERIAL_ECHOPAIR(" z2=", z2); - SERIAL_ECHOPAIR(" z3=", z3); - SERIAL_ECHOPAIR(" z4=", z4); - SERIAL_ECHOPAIR(" left=", left); - SERIAL_ECHOPAIR(" right=", right); - SERIAL_ECHOLNPAIR(" offset=", offset); - */ - } - #endif // AUTO_BED_LEVELING_NONLINEAR - #endif // DELTA /** @@ -8018,10 +8015,6 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { inverse_kinematics(logical); - #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR) - if (!bed_leveling_in_progress) adjust_delta(logical); - #endif - //DEBUG_POS("prepare_kinematic_move_to", logical); //DEBUG_POS("prepare_kinematic_move_to", delta); @@ -8272,9 +8265,6 @@ void prepare_move_to_destination() { #if IS_KINEMATIC inverse_kinematics(arc_target); - #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR) - adjust_delta(arc_target); - #endif planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder); #else planner.buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], fr_mm_s, active_extruder); @@ -8284,9 +8274,6 @@ void prepare_move_to_destination() { // Ensure last segment arrives at target location. #if IS_KINEMATIC inverse_kinematics(logical); - #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_NONLINEAR) - adjust_delta(logical); - #endif planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], fr_mm_s, active_extruder); #else planner.buffer_line(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS], logical[E_AXIS], fr_mm_s, active_extruder); diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index a37dc1d4a0..cad1f3eaa9 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -541,6 +541,23 @@ void Planner::check_axes_activity() { ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM); lz = LOGICAL_Z_POSITION(dz); + #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR) + + float tmp[XYZ] = { lx, ly, 0 }; + + #if ENABLED(DELTA) + + float offset = nonlinear_z_offset(tmp); + lx += offset; + ly += offset; + lz += offset; + + #else + + lz += nonlinear_z_offset(tmp); + + #endif + #endif } @@ -562,6 +579,11 @@ void Planner::check_axes_activity() { ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM); lz = LOGICAL_Z_POSITION(dz); + #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR) + + float tmp[XYZ] = { lx, ly, 0 }; + lz -= nonlinear_z_offset(tmp); + #endif } diff --git a/Marlin/planner_bezier.cpp b/Marlin/planner_bezier.cpp index 15c8091635..ad46f89f0e 100644 --- a/Marlin/planner_bezier.cpp +++ b/Marlin/planner_bezier.cpp @@ -190,10 +190,7 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS] #if IS_KINEMATIC inverse_kinematics(bez_target); - #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE) - adjust_delta(bez_target); - #endif - planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], bez_target[E_AXIS], fr_mm_s, extruder); + planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], bez_target[E_AXIS], fr_mm_s, extruder); #else planner.buffer_line(bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS], bez_target[E_AXIS], fr_mm_s, extruder); #endif