From c437bb08f12f1c0535cc78a761b49a18f2dc2a12 Mon Sep 17 00:00:00 2001 From: Thomas Moore Date: Sun, 16 Sep 2018 22:24:15 -0400 Subject: [PATCH] Overhaul of the planner (#11578) - Move FWRETRACT to the planner - Combine leveling, skew, etc. in a single modifier method - Have kinematic and non-kinematic moves call one planner method --- Marlin/src/Marlin.cpp | 6 +- .../FLSUN/auto_calibrate/Configuration.h | 3 - .../delta/FLSUN/kossel/Configuration.h | 3 - .../delta/FLSUN/kossel_mini/Configuration.h | 3 - .../delta/Hatchbox_Alpha/Configuration.h | 3 - .../examples/delta/generic/Configuration.h | 3 - .../delta/kossel_mini/Configuration.h | 3 - .../examples/delta/kossel_pro/Configuration.h | 3 - .../examples/delta/kossel_xl/Configuration.h | 3 - Marlin/src/feature/bedlevel/bedlevel.cpp | 87 ++----- .../src/feature/bedlevel/ubl/ubl_motion.cpp | 73 ++---- Marlin/src/feature/fwretract.cpp | 47 ++-- Marlin/src/feature/fwretract.h | 3 +- Marlin/src/feature/pause.cpp | 2 +- Marlin/src/feature/power_loss_recovery.cpp | 27 ++ Marlin/src/feature/power_loss_recovery.h | 4 + Marlin/src/gcode/bedlevel/abl/G29.cpp | 2 +- Marlin/src/gcode/calibrate/G28.cpp | 6 +- Marlin/src/gcode/calibrate/M852.cpp | 2 +- Marlin/src/gcode/config/M200-M205.cpp | 11 +- Marlin/src/gcode/config/M92.cpp | 2 +- Marlin/src/gcode/geometry/G92.cpp | 2 +- Marlin/src/gcode/host/M114.cpp | 2 +- Marlin/src/gcode/motion/G2_G3.cpp | 81 ++---- Marlin/src/gcode/probe/G38.cpp | 4 +- Marlin/src/inc/Conditionals_post.h | 7 +- Marlin/src/lcd/ultralcd.cpp | 21 +- Marlin/src/module/configuration_store.cpp | 47 +++- Marlin/src/module/delta.cpp | 10 +- Marlin/src/module/delta.h | 11 +- Marlin/src/module/motion.cpp | 215 ++++++--------- Marlin/src/module/motion.h | 23 -- Marlin/src/module/planner.cpp | 245 ++++++++++++++---- Marlin/src/module/planner.h | 225 +++++++++++----- Marlin/src/module/planner_bezier.cpp | 12 +- Marlin/src/module/probe.cpp | 2 +- Marlin/src/module/scara.cpp | 2 +- Marlin/src/module/scara.h | 11 +- Marlin/src/module/tool_change.cpp | 36 +-- 39 files changed, 655 insertions(+), 597 deletions(-) diff --git a/Marlin/src/Marlin.cpp b/Marlin/src/Marlin.cpp index fda6f9279e..99502c6557 100644 --- a/Marlin/src/Marlin.cpp +++ b/Marlin/src/Marlin.cpp @@ -275,7 +275,7 @@ void quickstop_stepper() { planner.quick_stop(); planner.synchronize(); set_current_from_steppers_for_axis(ALL_AXES); - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); } void enable_all_steppers() { @@ -465,7 +465,7 @@ void manage_inactivity(const bool ignore_stepper_queue/*=false*/) { const float olde = current_position[E_AXIS]; current_position[E_AXIS] += EXTRUDER_RUNOUT_EXTRUDE; - planner.buffer_line_kinematic(current_position, MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder); + planner.buffer_line(current_position, MMM_TO_MMS(EXTRUDER_RUNOUT_SPEED), active_extruder); current_position[E_AXIS] = olde; planner.set_e_position_mm(olde); planner.synchronize(); @@ -766,7 +766,7 @@ void setup() { #endif // Vital to init stepper/planner equivalent for current_position - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); thermalManager.init(); // Initialize temperature loop diff --git a/Marlin/src/config/examples/delta/FLSUN/auto_calibrate/Configuration.h b/Marlin/src/config/examples/delta/FLSUN/auto_calibrate/Configuration.h index 8b80ca574f..21a4e83fe7 100644 --- a/Marlin/src/config/examples/delta/FLSUN/auto_calibrate/Configuration.h +++ b/Marlin/src/config/examples/delta/FLSUN/auto_calibrate/Configuration.h @@ -539,9 +539,6 @@ // and processor overload (too many expensive sqrt calls). #define DELTA_SEGMENTS_PER_SECOND 160 - // Convert feedrates to apply to the Effector instead of the Carriages - #define DELTA_FEEDRATE_SCALING - // After homing move down to a height where XY movement is unconstrained //#define DELTA_HOME_TO_SAFE_ZONE diff --git a/Marlin/src/config/examples/delta/FLSUN/kossel/Configuration.h b/Marlin/src/config/examples/delta/FLSUN/kossel/Configuration.h index 35fee82cca..7d4b40c991 100644 --- a/Marlin/src/config/examples/delta/FLSUN/kossel/Configuration.h +++ b/Marlin/src/config/examples/delta/FLSUN/kossel/Configuration.h @@ -539,9 +539,6 @@ // and processor overload (too many expensive sqrt calls). #define DELTA_SEGMENTS_PER_SECOND 160 - // Convert feedrates to apply to the Effector instead of the Carriages - #define DELTA_FEEDRATE_SCALING - // After homing move down to a height where XY movement is unconstrained //#define DELTA_HOME_TO_SAFE_ZONE diff --git a/Marlin/src/config/examples/delta/FLSUN/kossel_mini/Configuration.h b/Marlin/src/config/examples/delta/FLSUN/kossel_mini/Configuration.h index 5f456cb0d5..19df1a23b1 100644 --- a/Marlin/src/config/examples/delta/FLSUN/kossel_mini/Configuration.h +++ b/Marlin/src/config/examples/delta/FLSUN/kossel_mini/Configuration.h @@ -539,9 +539,6 @@ // and processor overload (too many expensive sqrt calls). #define DELTA_SEGMENTS_PER_SECOND 160 - // Convert feedrates to apply to the Effector instead of the Carriages - #define DELTA_FEEDRATE_SCALING - // After homing move down to a height where XY movement is unconstrained //#define DELTA_HOME_TO_SAFE_ZONE diff --git a/Marlin/src/config/examples/delta/Hatchbox_Alpha/Configuration.h b/Marlin/src/config/examples/delta/Hatchbox_Alpha/Configuration.h index debaf61bf6..c38c50628b 100644 --- a/Marlin/src/config/examples/delta/Hatchbox_Alpha/Configuration.h +++ b/Marlin/src/config/examples/delta/Hatchbox_Alpha/Configuration.h @@ -544,9 +544,6 @@ // and processor overload (too many expensive sqrt calls). #define DELTA_SEGMENTS_PER_SECOND 200 - // Convert feedrates to apply to the Effector instead of the Carriages - #define DELTA_FEEDRATE_SCALING - // After homing move down to a height where XY movement is unconstrained //#define DELTA_HOME_TO_SAFE_ZONE diff --git a/Marlin/src/config/examples/delta/generic/Configuration.h b/Marlin/src/config/examples/delta/generic/Configuration.h index 1d723398a1..53c6732aea 100644 --- a/Marlin/src/config/examples/delta/generic/Configuration.h +++ b/Marlin/src/config/examples/delta/generic/Configuration.h @@ -529,9 +529,6 @@ // and processor overload (too many expensive sqrt calls). #define DELTA_SEGMENTS_PER_SECOND 200 - // Convert feedrates to apply to the Effector instead of the Carriages - #define DELTA_FEEDRATE_SCALING - // After homing move down to a height where XY movement is unconstrained //#define DELTA_HOME_TO_SAFE_ZONE diff --git a/Marlin/src/config/examples/delta/kossel_mini/Configuration.h b/Marlin/src/config/examples/delta/kossel_mini/Configuration.h index 67a064fe86..a44ef02537 100644 --- a/Marlin/src/config/examples/delta/kossel_mini/Configuration.h +++ b/Marlin/src/config/examples/delta/kossel_mini/Configuration.h @@ -529,9 +529,6 @@ // and processor overload (too many expensive sqrt calls). #define DELTA_SEGMENTS_PER_SECOND 200 - // Convert feedrates to apply to the Effector instead of the Carriages - #define DELTA_FEEDRATE_SCALING - // After homing move down to a height where XY movement is unconstrained //#define DELTA_HOME_TO_SAFE_ZONE diff --git a/Marlin/src/config/examples/delta/kossel_pro/Configuration.h b/Marlin/src/config/examples/delta/kossel_pro/Configuration.h index 3e001ed64e..8ed691c270 100644 --- a/Marlin/src/config/examples/delta/kossel_pro/Configuration.h +++ b/Marlin/src/config/examples/delta/kossel_pro/Configuration.h @@ -515,9 +515,6 @@ // and processor overload (too many expensive sqrt calls). #define DELTA_SEGMENTS_PER_SECOND 160 - // Convert feedrates to apply to the Effector instead of the Carriages - #define DELTA_FEEDRATE_SCALING - // After homing move down to a height where XY movement is unconstrained //#define DELTA_HOME_TO_SAFE_ZONE diff --git a/Marlin/src/config/examples/delta/kossel_xl/Configuration.h b/Marlin/src/config/examples/delta/kossel_xl/Configuration.h index 59ab64b15c..e50867c7fe 100644 --- a/Marlin/src/config/examples/delta/kossel_xl/Configuration.h +++ b/Marlin/src/config/examples/delta/kossel_xl/Configuration.h @@ -533,9 +533,6 @@ // and processor overload (too many expensive sqrt calls). #define DELTA_SEGMENTS_PER_SECOND 160 - // Convert feedrates to apply to the Effector instead of the Carriages - #define DELTA_FEEDRATE_SCALING - // After homing move down to a height where XY movement is unconstrained //#define DELTA_HOME_TO_SAFE_ZONE diff --git a/Marlin/src/feature/bedlevel/bedlevel.cpp b/Marlin/src/feature/bedlevel/bedlevel.cpp index 90bbfe600e..5dc328c273 100644 --- a/Marlin/src/feature/bedlevel/bedlevel.cpp +++ b/Marlin/src/feature/bedlevel/bedlevel.cpp @@ -25,15 +25,12 @@ #if HAS_LEVELING #include "bedlevel.h" +#include "../../module/planner.h" #if ENABLED(MESH_BED_LEVELING) || ENABLED(PROBE_MANUALLY) #include "../../module/motion.h" #endif -#if PLANNER_LEVELING - #include "../../module/planner.h" -#endif - #if ENABLED(PROBE_MANUALLY) bool g29_in_progress = false; #endif @@ -79,74 +76,24 @@ void set_bed_leveling_enabled(const bool enable/*=true*/) { planner.synchronize(); - #if ENABLED(MESH_BED_LEVELING) + #if ENABLED(AUTO_BED_LEVELING_BILINEAR) + // Force bilinear_z_offset to re-calculate next time + const float reset[XYZ] = { -9999.999, -9999.999, 0 }; + (void)bilinear_z_offset(reset); + #endif - if (!enable) - planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]); + if (planner.leveling_active) { // leveling from on to off + // change unleveled current_position to physical current_position without moving steppers. + planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]); + planner.leveling_active = false; // disable only AFTER calling apply_leveling + } + else { // leveling from off to on + planner.leveling_active = true; // enable BEFORE calling unapply_leveling, otherwise ignored + // change physical current_position to unleveled current_position without moving steppers. + planner.unapply_leveling(current_position); + } - const bool enabling = enable && leveling_is_valid(); - planner.leveling_active = enabling; - if (enabling) planner.unapply_leveling(current_position); - - #elif ENABLED(AUTO_BED_LEVELING_UBL) - #if PLANNER_LEVELING - if (planner.leveling_active) { // leveling from on to off - // change unleveled current_position to physical current_position without moving steppers. - planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]); - planner.leveling_active = false; // disable only AFTER calling apply_leveling - } - else { // leveling from off to on - planner.leveling_active = true; // enable BEFORE calling unapply_leveling, otherwise ignored - // change physical current_position to unleveled current_position without moving steppers. - planner.unapply_leveling(current_position); - } - #else - // UBL equivalents for apply/unapply_leveling - #if ENABLED(SKEW_CORRECTION) - float pos[XYZ] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] }; - planner.skew(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS]); - #else - const float (&pos)[XYZE] = current_position; - #endif - if (planner.leveling_active) { - current_position[Z_AXIS] += ubl.get_z_correction(pos[X_AXIS], pos[Y_AXIS]); - planner.leveling_active = false; - } - else { - planner.leveling_active = true; - current_position[Z_AXIS] -= ubl.get_z_correction(pos[X_AXIS], pos[Y_AXIS]); - } - #endif - - #else // OLDSCHOOL_ABL - - #if ENABLED(AUTO_BED_LEVELING_BILINEAR) - // Force bilinear_z_offset to re-calculate next time - const float reset[XYZ] = { -9999.999, -9999.999, 0 }; - (void)bilinear_z_offset(reset); - #endif - - // Enable or disable leveling compensation in the planner - planner.leveling_active = enable; - - if (!enable) - // When disabling just get the current position from the steppers. - // This will yield the smallest error when first converted back to steps. - set_current_from_steppers_for_axis( - #if ABL_PLANAR - ALL_AXES - #else - Z_AXIS - #endif - ); - else - // When enabling, remove compensation from the current position, - // so compensation will give the right stepper counts. - planner.unapply_leveling(current_position); - - SYNC_PLAN_POSITION_KINEMATIC(); - - #endif // OLDSCHOOL_ABL + sync_plan_position(); } } diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp index 85ed86838e..f7add5f79e 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp @@ -49,12 +49,11 @@ * as possible to determine if this is the case. If this move is within the same cell, we will * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave */ - #if ENABLED(SKEW_CORRECTION) - // For skew correction just adjust the destination point and we're done + #if HAS_POSITION_MODIFIERS float start[XYZE] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS] }, end[XYZE] = { destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS] }; - planner.skew(start[X_AXIS], start[Y_AXIS], start[Z_AXIS]); - planner.skew(end[X_AXIS], end[Y_AXIS], end[Z_AXIS]); + planner.apply_modifiers(start); + planner.apply_modifiers(end); #else const float (&start)[XYZE] = current_position, (&end)[XYZE] = destination; @@ -364,47 +363,6 @@ #else // UBL_SEGMENTED - #if IS_SCARA // scale the feed rate from mm/s to degrees/s - static float scara_feed_factor, scara_oldA, scara_oldB; - #endif - - // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic, - // so we call buffer_segment directly here. Per-segmented leveling and kinematics performed first. - - inline void _O2 ubl_buffer_segment_raw(const float (&in_raw)[XYZE], const float &fr) { - - #if ENABLED(SKEW_CORRECTION) - float raw[XYZE] = { in_raw[X_AXIS], in_raw[Y_AXIS], in_raw[Z_AXIS] }; - planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); - #else - const float (&raw)[XYZE] = in_raw; - #endif - - #if ENABLED(DELTA) // apply delta inverse_kinematics - - DELTA_IK(raw); - planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder); - - #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw) - - inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE] - // should move the feedrate scaling to scara inverse_kinematics - - const float adiff = ABS(delta[A_AXIS] - scara_oldA), - bdiff = ABS(delta[B_AXIS] - scara_oldB); - scara_oldA = delta[A_AXIS]; - scara_oldB = delta[B_AXIS]; - float s_feedrate = MAX(adiff, bdiff) * scara_feed_factor; - - planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], s_feedrate, active_extruder); - - #else // CARTESIAN - - planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder); - - #endif - } - #if IS_SCARA #define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm #elif ENABLED(DELTA) @@ -449,10 +407,9 @@ NOLESS(segments, 1U); // must have at least one segment const float inv_segments = 1.0f / segments; // divide once, multiply thereafter - #if IS_SCARA // scale the feed rate from mm/s to degrees/s - scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate; - scara_oldA = planner.get_axis_position_degrees(A_AXIS); - scara_oldB = planner.get_axis_position_degrees(B_AXIS); + const float segment_xyz_mm = HYPOT(cartesian_xy_mm, total[Z_AXIS]) * inv_segments; // length of each segment + #if ENABLED(SCARA_FEEDRATE_SCALING) + const float inv_duration = feedrate / segment_xyz_mm; #endif const float diff[XYZE] = { @@ -476,9 +433,17 @@ if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling while (--segments) { LOOP_XYZE(i) raw[i] += diff[i]; - ubl_buffer_segment_raw(raw, feedrate); + planner.buffer_line(raw, feedrate, active_extruder, segment_xyz_mm + #if ENABLED(SCARA_FEEDRATE_SCALING) + , inv_duration + #endif + ); } - ubl_buffer_segment_raw(rtarget, feedrate); + planner.buffer_line(rtarget, feedrate, active_extruder, segment_xyz_mm + #if ENABLED(SCARA_FEEDRATE_SCALING) + , inv_duration + #endif + ); return false; // moved but did not set_current_from_destination(); } @@ -554,7 +519,11 @@ const float z = raw[Z_AXIS]; raw[Z_AXIS] += z_cxcy; - ubl_buffer_segment_raw(raw, feedrate); + planner.buffer_line(raw, feedrate, active_extruder, segment_xyz_mm + #if ENABLED(SCARA_FEEDRATE_SCALING) + , inv_duration + #endif + ); raw[Z_AXIS] = z; if (segments == 0) // done with last segment diff --git a/Marlin/src/feature/fwretract.cpp b/Marlin/src/feature/fwretract.cpp index a034c0f7af..a06f48ec05 100644 --- a/Marlin/src/feature/fwretract.cpp +++ b/Marlin/src/feature/fwretract.cpp @@ -54,6 +54,7 @@ float FWRetract::retract_length, // M207 S - G10 Retract len FWRetract::swap_retract_length, // M207 W - G10 Swap Retract length FWRetract::swap_retract_recover_length, // M208 W - G11 Swap Recover length FWRetract::swap_retract_recover_feedrate_mm_s, // M208 R - G11 Swap Recover feedrate + FWRetract::current_retract[EXTRUDERS], // Retract value used by planner FWRetract::current_hop; void FWRetract::reset() { @@ -73,6 +74,7 @@ void FWRetract::reset() { #if EXTRUDERS > 1 retracted_swap[i] = false; #endif + current_retract[i] = 0.0; } } @@ -84,9 +86,6 @@ void FWRetract::reset() { * * To simplify the logic, doubled retract/recover moves are ignored. * - * Note: Z lift is done transparently to the planner. Aborting - * a print between G10 and G11 may corrupt the Z position. - * * Note: Auto-retract will apply the set Z hop in addition to any Z hop * included in the G-code. Use M207 Z0 to to prevent double hop. */ @@ -95,9 +94,6 @@ void FWRetract::retract(const bool retracting , bool swapping /* =false */ #endif ) { - - static float current_hop = 0.0; // Total amount lifted, for use in recover - // Prevent two retracts or recovers in a row if (retracted[active_extruder] == retracting) return; @@ -129,48 +125,50 @@ void FWRetract::retract(const bool retracting //*/ const float old_feedrate_mm_s = feedrate_mm_s, - renormalize = RECIPROCAL(planner.e_factor[active_extruder]), - base_retract = swapping ? swap_retract_length : retract_length, - old_z = current_position[Z_AXIS], - old_e = current_position[E_AXIS]; + unscale_e = RECIPROCAL(planner.e_factor[active_extruder]), + unscale_fr = 100.0 / feedrate_percentage, // Disable feedrate scaling for retract moves + base_retract = swapping ? swap_retract_length : retract_length; // The current position will be the destination for E and Z moves set_destination_from_current(); if (retracting) { // Retract by moving from a faux E position back to the current E position - feedrate_mm_s = retract_feedrate_mm_s; - destination[E_AXIS] -= base_retract * renormalize; + feedrate_mm_s = retract_feedrate_mm_s * unscale_fr; + current_retract[active_extruder] = base_retract * unscale_e; prepare_move_to_destination(); // set_current_to_destination + planner.synchronize(); // Wait for move to complete // Is a Z hop set, and has the hop not yet been done? if (retract_zlift > 0.01 && !current_hop) { // Apply hop only once current_hop += retract_zlift; // Add to the hop total (again, only once) - destination[Z_AXIS] += retract_zlift; // Raise Z by the zlift (M207 Z) amount - feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS]; // Maximum Z feedrate + feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS] * unscale_fr; // Maximum Z feedrate prepare_move_to_destination(); // Raise up, set_current_to_destination + planner.synchronize(); // Wait for move to complete } } else { // If a hop was done and Z hasn't changed, undo the Z hop if (current_hop) { - current_position[Z_AXIS] += current_hop; // Restore the actual Z position - SYNC_PLAN_POSITION_KINEMATIC(); // Unspoof the position planner - feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS]; // Z feedrate to max + current_hop = 0.0; + feedrate_mm_s = planner.max_feedrate_mm_s[Z_AXIS] * unscale_fr; // Z feedrate to max prepare_move_to_destination(); // Lower Z, set_current_to_destination - current_hop = 0.0; // Clear the hop amount + planner.synchronize(); // Wait for move to complete } - destination[E_AXIS] += (base_retract + (swapping ? swap_retract_recover_length : retract_recover_length)) * renormalize; - feedrate_mm_s = swapping ? swap_retract_recover_feedrate_mm_s : retract_recover_feedrate_mm_s; + const float extra_recover = swapping ? swap_retract_recover_length : retract_recover_length; + if (extra_recover != 0.0) { + current_position[E_AXIS] -= extra_recover; // Adjust the current E position by the extra amount to recover + sync_plan_position_e(); // Sync the planner position so the extra amount is recovered + } + + current_retract[active_extruder] = 0.0; + feedrate_mm_s = (swapping ? swap_retract_recover_feedrate_mm_s : retract_recover_feedrate_mm_s) * unscale_fr; prepare_move_to_destination(); // Recover E, set_current_to_destination + planner.synchronize(); // Wait for move to complete } feedrate_mm_s = old_feedrate_mm_s; // Restore original feedrate - current_position[Z_AXIS] = old_z; // Restore Z and E positions - current_position[E_AXIS] = old_e; - SYNC_PLAN_POSITION_KINEMATIC(); // As if the move never took place - retracted[active_extruder] = retracting; // Active extruder now retracted / recovered // If swap retract/recover update the retracted_swap flag too @@ -194,7 +192,6 @@ void FWRetract::retract(const bool retracting SERIAL_ECHOLNPAIR("current_position[e] ", current_position[E_AXIS]); SERIAL_ECHOLNPAIR("current_hop ", current_hop); //*/ - } #endif // FWRETRACT diff --git a/Marlin/src/feature/fwretract.h b/Marlin/src/feature/fwretract.h index c99109130e..5483cd5924 100644 --- a/Marlin/src/feature/fwretract.h +++ b/Marlin/src/feature/fwretract.h @@ -46,7 +46,8 @@ public: swap_retract_length, // M207 W - G10 Swap Retract length swap_retract_recover_length, // M208 W - G11 Swap Recover length swap_retract_recover_feedrate_mm_s, // M208 R - G11 Swap Recover feedrate - current_hop; + current_retract[EXTRUDERS], // Retract value used by planner + current_hop; // Hop value used by planner FWRetract() { reset(); } diff --git a/Marlin/src/feature/pause.cpp b/Marlin/src/feature/pause.cpp index 62e66122ef..0af61060d6 100644 --- a/Marlin/src/feature/pause.cpp +++ b/Marlin/src/feature/pause.cpp @@ -120,7 +120,7 @@ static bool ensure_safe_temperature(const AdvancedPauseMode mode=ADVANCED_PAUSE_ static void do_pause_e_move(const float &length, const float &fr) { set_destination_from_current(); destination[E_AXIS] += length / planner.e_factor[active_extruder]; - planner.buffer_line_kinematic(destination, fr, active_extruder); + planner.buffer_line(destination, fr, active_extruder); set_current_from_destination(); planner.synchronize(); } diff --git a/Marlin/src/feature/power_loss_recovery.cpp b/Marlin/src/feature/power_loss_recovery.cpp index 424b4290e3..6613c0adf6 100644 --- a/Marlin/src/feature/power_loss_recovery.cpp +++ b/Marlin/src/feature/power_loss_recovery.cpp @@ -39,6 +39,10 @@ #include "../sd/cardreader.h" #include "../core/serial.h" +#if ENABLED(FWRETRACT) + #include "fwretract.h" +#endif + // Recovery data job_recovery_info_t job_recovery_info; JobRecoveryPhase job_recovery_phase = JOB_RECOVERY_IDLE; @@ -90,6 +94,15 @@ extern uint8_t commands_in_queue, cmd_queue_index_r; SERIAL_PROTOCOLPAIR("leveling: ", int(job_recovery_info.leveling)); SERIAL_PROTOCOLLNPAIR(" fade: ", int(job_recovery_info.fade)); #endif + #if ENABLED(FWRETRACT) + SERIAL_PROTOCOLPGM("retract: "); + for (int8_t e = 0; e < EXTRUDERS; e++) { + SERIAL_PROTOCOL(job_recovery_info.retract[e]); + if (e < EXTRUDERS - 1) SERIAL_CHAR(','); + } + SERIAL_EOL(); + SERIAL_PROTOCOLLNPAIR("retract_hop: ", job_recovery_info.retract_hop); + #endif SERIAL_PROTOCOLLNPAIR("cmd_queue_index_r: ", int(job_recovery_info.cmd_queue_index_r)); SERIAL_PROTOCOLLNPAIR("commands_in_queue: ", int(job_recovery_info.commands_in_queue)); if (recovery) @@ -160,6 +173,15 @@ void check_print_job_recovery() { } #endif + #if ENABLED(FWRETRACT) + for (uint8_t e = 0; e < EXTRUDERS; e++) { + if (job_recovery_info.retract[e] != 0.0) + fwretract.current_retract[e] = job_recovery_info.retract[e]; + fwretract.retracted[e] = true; + } + fwretract.current_hop = job_recovery_info.retract_hop; + #endif + dtostrf(job_recovery_info.current_position[Z_AXIS] + 2, 1, 3, str_1); dtostrf(job_recovery_info.current_position[E_AXIS] #if ENABLED(SAVE_EACH_CMD_MODE) @@ -256,6 +278,11 @@ void save_job_recovery_info() { ); #endif + #if ENABLED(FWRETRACT) + COPY(job_recovery_info.retract, fwretract.current_retract); + job_recovery_info.retract_hop = fwretract.current_hop; + #endif + // Commands in the queue job_recovery_info.cmd_queue_index_r = cmd_queue_index_r; job_recovery_info.commands_in_queue = commands_in_queue; diff --git a/Marlin/src/feature/power_loss_recovery.h b/Marlin/src/feature/power_loss_recovery.h index 46a8fd52de..24b0891a02 100644 --- a/Marlin/src/feature/power_loss_recovery.h +++ b/Marlin/src/feature/power_loss_recovery.h @@ -60,6 +60,10 @@ typedef struct { float fade; #endif + #if ENABLED(FWRETRACT) + float retract[EXTRUDERS], retract_hop; + #endif + // Command queue uint8_t cmd_queue_index_r, commands_in_queue; char command_queue[BUFSIZE][MAX_CMD_SIZE]; diff --git a/Marlin/src/gcode/bedlevel/abl/G29.cpp b/Marlin/src/gcode/bedlevel/abl/G29.cpp index c711b9deed..f6965f18e4 100644 --- a/Marlin/src/gcode/bedlevel/abl/G29.cpp +++ b/Marlin/src/gcode/bedlevel/abl/G29.cpp @@ -989,7 +989,7 @@ G29_TYPE GcodeSuite::G29() { KEEPALIVE_STATE(IN_HANDLER); if (planner.leveling_active) - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); #if HAS_BED_PROBE && defined(Z_AFTER_PROBING) move_z_after_probing(); diff --git a/Marlin/src/gcode/calibrate/G28.cpp b/Marlin/src/gcode/calibrate/G28.cpp index b8cfe9b78d..66054d00e5 100644 --- a/Marlin/src/gcode/calibrate/G28.cpp +++ b/Marlin/src/gcode/calibrate/G28.cpp @@ -101,7 +101,7 @@ if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Z_SAFE_HOMING >>>"); #endif - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); /** * Move the Z probe (or just the nozzle) to the safe homing point @@ -182,7 +182,7 @@ void GcodeSuite::G28(const bool always_home_all) { #if ENABLED(MARLIN_DEV_MODE) if (parser.seen('S')) { LOOP_XYZ(a) set_axis_is_at_home((AxisEnum)a); - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); SERIAL_ECHOLNPGM("Simulated Homing"); report_current_position(); #if ENABLED(DEBUG_LEVELING_FEATURE) @@ -357,7 +357,7 @@ void GcodeSuite::G28(const bool always_home_all) { } // home_all || homeZ #endif // Z_HOME_DIR < 0 - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); #endif // !DELTA (G28) diff --git a/Marlin/src/gcode/calibrate/M852.cpp b/Marlin/src/gcode/calibrate/M852.cpp index 4841cecbc0..927c0838e4 100644 --- a/Marlin/src/gcode/calibrate/M852.cpp +++ b/Marlin/src/gcode/calibrate/M852.cpp @@ -87,7 +87,7 @@ void GcodeSuite::M852() { // When skew is changed the current position changes if (setval) { set_current_from_steppers_for_axis(ALL_AXES); - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); report_current_position(); } diff --git a/Marlin/src/gcode/config/M200-M205.cpp b/Marlin/src/gcode/config/M200-M205.cpp index cd45047be6..132cfc9ae1 100644 --- a/Marlin/src/gcode/config/M200-M205.cpp +++ b/Marlin/src/gcode/config/M200-M205.cpp @@ -136,14 +136,17 @@ void GcodeSuite::M205() { const float junc_dev = parser.value_linear_units(); if (WITHIN(junc_dev, 0.01f, 0.3f)) { planner.junction_deviation_mm = junc_dev; - planner.recalculate_max_e_jerk(); + #if ENABLED(LIN_ADVANCE) + planner.recalculate_max_e_jerk(); + #endif } else { SERIAL_ERROR_START(); SERIAL_ERRORLNPGM("?J out of range (0.01 to 0.3)"); } } - #else + #endif + #if HAS_CLASSIC_JERK if (parser.seen('X')) planner.max_jerk[X_AXIS] = parser.value_linear_units(); if (parser.seen('Y')) planner.max_jerk[Y_AXIS] = parser.value_linear_units(); if (parser.seen('Z')) { @@ -153,6 +156,8 @@ void GcodeSuite::M205() { SERIAL_ECHOLNPGM("WARNING! Low Z Jerk may lead to unwanted pauses."); #endif } - if (parser.seen('E')) planner.max_jerk[E_AXIS] = parser.value_linear_units(); + #if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE) + if (parser.seen('E')) planner.max_jerk[E_AXIS] = parser.value_linear_units(); + #endif #endif } diff --git a/Marlin/src/gcode/config/M92.cpp b/Marlin/src/gcode/config/M92.cpp index 310b754aa3..2ac913e701 100644 --- a/Marlin/src/gcode/config/M92.cpp +++ b/Marlin/src/gcode/config/M92.cpp @@ -39,7 +39,7 @@ void GcodeSuite::M92() { const float value = parser.value_per_axis_unit((AxisEnum)(E_AXIS + TARGET_EXTRUDER)); if (value < 20) { float factor = planner.axis_steps_per_mm[E_AXIS + TARGET_EXTRUDER] / value; // increase e constants if M92 E14 is given for netfab. - #if DISABLED(JUNCTION_DEVIATION) + #if HAS_CLASSIC_JERK && (DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE)) planner.max_jerk[E_AXIS] *= factor; #endif planner.max_feedrate_mm_s[E_AXIS + TARGET_EXTRUDER] *= factor; diff --git a/Marlin/src/gcode/geometry/G92.cpp b/Marlin/src/gcode/geometry/G92.cpp index 933a6e1cdb..721c8b73c3 100644 --- a/Marlin/src/gcode/geometry/G92.cpp +++ b/Marlin/src/gcode/geometry/G92.cpp @@ -92,7 +92,7 @@ void GcodeSuite::G92() { COPY(coordinate_system[active_coordinate_system], position_shift); #endif - if (didXYZ) SYNC_PLAN_POSITION_KINEMATIC(); + if (didXYZ) sync_plan_position(); else if (didE) sync_plan_position_e(); report_current_position(); diff --git a/Marlin/src/gcode/host/M114.cpp b/Marlin/src/gcode/host/M114.cpp index e273c4e101..9d1a48b55b 100644 --- a/Marlin/src/gcode/host/M114.cpp +++ b/Marlin/src/gcode/host/M114.cpp @@ -56,7 +56,7 @@ float leveled[XYZ] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] }; - #if PLANNER_LEVELING + #if HAS_LEVELING SERIAL_PROTOCOLPGM("Leveled:"); planner.apply_leveling(leveled); report_xyz(leveled); diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp index 5e52d54c9b..0ae3f8574f 100644 --- a/Marlin/src/gcode/motion/G2_G3.cpp +++ b/Marlin/src/gcode/motion/G2_G3.cpp @@ -35,10 +35,6 @@ #include "../../module/scara.h" #endif -#if HAS_FEEDRATE_SCALING && ENABLED(AUTO_BED_LEVELING_BILINEAR) - #include "../../feature/bedlevel/abl/abl.h" -#endif - #if N_ARC_CORRECTION < 1 #undef N_ARC_CORRECTION #define N_ARC_CORRECTION 1 @@ -139,20 +135,12 @@ void plan_arc( const float fr_mm_s = MMS_SCALED(feedrate_mm_s); - millis_t next_idle_ms = millis() + 200UL; - - #if HAS_FEEDRATE_SCALING - // SCARA needs to scale the feed rate from mm/s to degrees/s - const float inv_segment_length = 1.0f / float(MM_PER_ARC_SEGMENT), - inverse_secs = inv_segment_length * fr_mm_s; - float oldA = planner.position_float[A_AXIS], - oldB = planner.position_float[B_AXIS] - #if ENABLED(DELTA_FEEDRATE_SCALING) - , oldC = planner.position_float[C_AXIS] - #endif - ; + #if ENABLED(SCARA_FEEDRATE_SCALING) + const float inv_duration = fr_mm_s / MM_PER_ARC_SEGMENT; #endif + millis_t next_idle_ms = millis() + 200UL; + #if N_ARC_CORRECTION > 1 int8_t arc_recalc_count = N_ARC_CORRECTION; #endif @@ -196,57 +184,32 @@ void plan_arc( clamp_to_software_endstops(raw); - #if HAS_FEEDRATE_SCALING - inverse_kinematics(raw); - ADJUST_DELTA(raw); + #if HAS_LEVELING && !PLANNER_LEVELING + planner.apply_leveling(raw); #endif - #if ENABLED(SCARA_FEEDRATE_SCALING) - // For SCARA scale the feed rate from mm/s to degrees/s - // i.e., Complete the angular vector in the given time. - if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder, MM_PER_ARC_SEGMENT)) - break; - oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; - #elif ENABLED(DELTA_FEEDRATE_SCALING) - // For DELTA scale the feed rate from Effector mm/s to Carriage mm/s - // i.e., Complete the linear vector in the given time. - if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs, active_extruder, MM_PER_ARC_SEGMENT)) - break; - oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldC = delta[C_AXIS]; - #elif HAS_UBL_AND_CURVES - float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] }; - planner.apply_leveling(pos); - if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT)) - break; - #else - if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder)) - break; - #endif + if (!planner.buffer_line(raw, fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT + #if ENABLED(SCARA_FEEDRATE_SCALING) + , inv_duration + #endif + )) + break; } // Ensure last segment arrives at target location. - #if HAS_FEEDRATE_SCALING - inverse_kinematics(cart); - ADJUST_DELTA(cart); + COPY(raw, cart); + + #if HAS_LEVELING && !PLANNER_LEVELING + planner.apply_leveling(raw); #endif - #if ENABLED(SCARA_FEEDRATE_SCALING) - const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB); - if (diff2) - planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], cart[Z_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder, MM_PER_ARC_SEGMENT); - #elif ENABLED(DELTA_FEEDRATE_SCALING) - const float diff2 = sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC); - if (diff2) - planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder, MM_PER_ARC_SEGMENT); - #elif HAS_UBL_AND_CURVES - float pos[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] }; - planner.apply_leveling(pos); - planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], cart[E_AXIS], fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT); - #else - planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder); - #endif + planner.buffer_line(raw, fr_mm_s, active_extruder, MM_PER_ARC_SEGMENT + #if ENABLED(SCARA_FEEDRATE_SCALING) + , inv_duration + #endif + ); - COPY(current_position, cart); + COPY(current_position, raw); } // plan_arc /** diff --git a/Marlin/src/gcode/probe/G38.cpp b/Marlin/src/gcode/probe/G38.cpp index bdbb635ef3..6bf09b8bac 100644 --- a/Marlin/src/gcode/probe/G38.cpp +++ b/Marlin/src/gcode/probe/G38.cpp @@ -56,7 +56,7 @@ static bool G38_run_probe() { endstops.hit_on_purpose(); set_current_from_steppers_for_axis(ALL_AXES); - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); if (G38_endstop_hit) { @@ -82,7 +82,7 @@ static bool G38_run_probe() { G38_move = false; set_current_from_steppers_for_axis(ALL_AXES); - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); #endif } diff --git a/Marlin/src/inc/Conditionals_post.h b/Marlin/src/inc/Conditionals_post.h index eea96adbe9..8e37f38ba8 100644 --- a/Marlin/src/inc/Conditionals_post.h +++ b/Marlin/src/inc/Conditionals_post.h @@ -49,6 +49,8 @@ #define IS_KINEMATIC (ENABLED(DELTA) || IS_SCARA) #define IS_CARTESIAN !IS_KINEMATIC +#define HAS_CLASSIC_JERK (IS_KINEMATIC || DISABLED(JUNCTION_DEVIATION)) + /** * Axis lengths and center */ @@ -1173,10 +1175,9 @@ #define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING)) #define HAS_AUTOLEVEL (HAS_ABL && DISABLED(PROBE_MANUALLY)) #define HAS_MESH (ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(MESH_BED_LEVELING)) -#define PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_SEGMENTED || ENABLED(SKEW_CORRECTION)) +#define PLANNER_LEVELING (HAS_LEVELING && DISABLED(AUTO_BED_LEVELING_UBL)) #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)) -#define HAS_UBL_AND_CURVES (ENABLED(AUTO_BED_LEVELING_UBL) && !PLANNER_LEVELING && (ENABLED(ARC_SUPPORT) || ENABLED(BEZIER_CURVE_SUPPORT))) -#define HAS_FEEDRATE_SCALING (ENABLED(SCARA_FEEDRATE_SCALING) || ENABLED(DELTA_FEEDRATE_SCALING)) +#define HAS_POSITION_MODIFIERS (ENABLED(FWRETRACT) || HAS_LEVELING || ENABLED(SKEW_CORRECTION)) #if ENABLED(AUTO_BED_LEVELING_UBL) #undef LCD_BED_LEVELING diff --git a/Marlin/src/lcd/ultralcd.cpp b/Marlin/src/lcd/ultralcd.cpp index 37cdb82ba9..943edc153e 100644 --- a/Marlin/src/lcd/ultralcd.cpp +++ b/Marlin/src/lcd/ultralcd.cpp @@ -841,7 +841,7 @@ void lcd_quick_feedback(const bool clear_buttons) { } inline void line_to_current_z() { - planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[Z_AXIS]), active_extruder); + planner.buffer_line(current_position, MMM_TO_MMS(manual_feedrate_mm_m[Z_AXIS]), active_extruder); } inline void line_to_z(const float &z) { @@ -1892,7 +1892,7 @@ void lcd_quick_feedback(const bool clear_buttons) { break; #endif } - planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[X_AXIS]), active_extruder); + planner.buffer_line(current_position, MMM_TO_MMS(manual_feedrate_mm_m[X_AXIS]), active_extruder); line_to_z(0.0); if (++bed_corner > 3 #if ENABLED(LEVEL_CENTER_TOO) @@ -2432,7 +2432,7 @@ void lcd_quick_feedback(const bool clear_buttons) { void ubl_map_move_to_xy() { current_position[X_AXIS] = pgm_read_float(&ubl._mesh_index_to_xpos[x_plot]); current_position[Y_AXIS] = pgm_read_float(&ubl._mesh_index_to_ypos[y_plot]); - planner.buffer_line_kinematic(current_position, MMM_TO_MMS(XY_PROBE_SPEED), active_extruder); + planner.buffer_line(current_position, MMM_TO_MMS(XY_PROBE_SPEED), active_extruder); } /** @@ -2911,7 +2911,7 @@ void lcd_quick_feedback(const bool clear_buttons) { #else - planner.buffer_line_kinematic(current_position, MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_axis == E_AXIS ? manual_move_e_index : active_extruder); + planner.buffer_line(current_position, MMM_TO_MMS(manual_feedrate_mm_m[manual_move_axis]), manual_move_axis == E_AXIS ? manual_move_e_index : active_extruder); manual_move_axis = (int8_t)NO_AXIS; #endif @@ -3746,8 +3746,13 @@ void lcd_quick_feedback(const bool clear_buttons) { MENU_BACK(MSG_ADVANCED_SETTINGS); #if ENABLED(JUNCTION_DEVIATION) - MENU_ITEM_EDIT_CALLBACK(float43, MSG_JUNCTION_DEVIATION, &planner.junction_deviation_mm, 0.01f, 0.3f, planner.recalculate_max_e_jerk); - #else + #if ENABLED(LIN_ADVANCE) + MENU_ITEM_EDIT_CALLBACK(float43, MSG_JUNCTION_DEVIATION, &planner.junction_deviation_mm, 0.01f, 0.3f, planner.recalculate_max_e_jerk); + #else + MENU_ITEM_EDIT(float43, MSG_JUNCTION_DEVIATION, &planner.junction_deviation_mm, 0.01f, 0.3f); + #endif + #endif + #if HAS_CLASSIC_JERK MENU_MULTIPLIER_ITEM_EDIT(float3, MSG_VA_JERK, &planner.max_jerk[A_AXIS], 1, 990); MENU_MULTIPLIER_ITEM_EDIT(float3, MSG_VB_JERK, &planner.max_jerk[B_AXIS], 1, 990); #if ENABLED(DELTA) @@ -3755,7 +3760,9 @@ void lcd_quick_feedback(const bool clear_buttons) { #else MENU_MULTIPLIER_ITEM_EDIT(float52sign, MSG_VC_JERK, &planner.max_jerk[C_AXIS], 0.1f, 990); #endif - MENU_MULTIPLIER_ITEM_EDIT(float3, MSG_VE_JERK, &planner.max_jerk[E_AXIS], 1, 990); + #if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE) + MENU_MULTIPLIER_ITEM_EDIT(float3, MSG_VE_JERK, &planner.max_jerk[E_AXIS], 1, 990); + #endif #endif END_MENU(); diff --git a/Marlin/src/module/configuration_store.cpp b/Marlin/src/module/configuration_store.cpp index 1ddd9bbbd4..3703d27b37 100644 --- a/Marlin/src/module/configuration_store.cpp +++ b/Marlin/src/module/configuration_store.cpp @@ -428,12 +428,20 @@ void MarlinSettings::postprocess() { EEPROM_WRITE(planner.min_feedrate_mm_s); EEPROM_WRITE(planner.min_travel_feedrate_mm_s); - #if ENABLED(JUNCTION_DEVIATION) + #if HAS_CLASSIC_JERK + EEPROM_WRITE(planner.max_jerk); + #if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE) + dummy = float(DEFAULT_EJERK); + EEPROM_WRITE(dummy); + #endif + #else const float planner_max_jerk[] = { float(DEFAULT_XJERK), float(DEFAULT_YJERK), float(DEFAULT_ZJERK), float(DEFAULT_EJERK) }; EEPROM_WRITE(planner_max_jerk); + #endif + + #if ENABLED(JUNCTION_DEVIATION) EEPROM_WRITE(planner.junction_deviation_mm); #else - EEPROM_WRITE(planner.max_jerk); dummy = 0.02f; EEPROM_WRITE(dummy); #endif @@ -1062,11 +1070,18 @@ void MarlinSettings::postprocess() { EEPROM_READ(planner.min_feedrate_mm_s); EEPROM_READ(planner.min_travel_feedrate_mm_s); - #if ENABLED(JUNCTION_DEVIATION) + #if HAS_CLASSIC_JERK + EEPROM_READ(planner.max_jerk); + #if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE) + EEPROM_READ(dummy); + #endif + #else for (uint8_t q = 4; q--;) EEPROM_READ(dummy); + #endif + + #if ENABLED(JUNCTION_DEVIATION) EEPROM_READ(planner.junction_deviation_mm); #else - EEPROM_READ(planner.max_jerk); EEPROM_READ(dummy); #endif @@ -1808,11 +1823,15 @@ void MarlinSettings::reset(PORTARG_SOLO) { #if ENABLED(JUNCTION_DEVIATION) planner.junction_deviation_mm = float(JUNCTION_DEVIATION_MM); - #else + #endif + + #if HAS_CLASSIC_JERK planner.max_jerk[X_AXIS] = DEFAULT_XJERK; planner.max_jerk[Y_AXIS] = DEFAULT_YJERK; planner.max_jerk[Z_AXIS] = DEFAULT_ZJERK; - planner.max_jerk[E_AXIS] = DEFAULT_EJERK; + #if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE) + planner.max_jerk[E_AXIS] = DEFAULT_EJERK; + #endif #endif #if HAS_HOME_OFFSET @@ -2243,11 +2262,12 @@ void MarlinSettings::reset(PORTARG_SOLO) { SERIAL_ECHOPGM_P(port, "Advanced: B S T"); #if ENABLED(JUNCTION_DEVIATION) SERIAL_ECHOPGM_P(port, " J"); - #else - SERIAL_ECHOPGM_P(port, " X Y Z"); #endif - #if DISABLED(JUNCTION_DEVIATION) || ENABLED(LIN_ADVANCE) - SERIAL_ECHOPGM_P(port, " E"); + #if HAS_CLASSIC_JERK + SERIAL_ECHOPGM_P(port, " X Y Z"); + #if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE) + SERIAL_ECHOPGM_P(port, " E"); + #endif #endif SERIAL_EOL_P(port); } @@ -2258,11 +2278,14 @@ void MarlinSettings::reset(PORTARG_SOLO) { #if ENABLED(JUNCTION_DEVIATION) SERIAL_ECHOPAIR_P(port, " J", LINEAR_UNIT(planner.junction_deviation_mm)); - #else + #endif + #if HAS_CLASSIC_JERK SERIAL_ECHOPAIR_P(port, " X", LINEAR_UNIT(planner.max_jerk[X_AXIS])); SERIAL_ECHOPAIR_P(port, " Y", LINEAR_UNIT(planner.max_jerk[Y_AXIS])); SERIAL_ECHOPAIR_P(port, " Z", LINEAR_UNIT(planner.max_jerk[Z_AXIS])); - SERIAL_ECHOPAIR_P(port, " E", LINEAR_UNIT(planner.max_jerk[E_AXIS])); + #if DISABLED(JUNCTION_DEVIATION) || DISABLED(LIN_ADVANCE) + SERIAL_ECHOPAIR_P(port, " E", LINEAR_UNIT(planner.max_jerk[E_AXIS])); + #endif #endif SERIAL_EOL_P(port); diff --git a/Marlin/src/module/delta.cpp b/Marlin/src/module/delta.cpp index ba4c018f93..f0832d13a5 100644 --- a/Marlin/src/module/delta.cpp +++ b/Marlin/src/module/delta.cpp @@ -101,7 +101,7 @@ void recalc_delta_settings() { SERIAL_ECHOLNPAIR(" C:", delta[C_AXIS]); \ }while(0) -void inverse_kinematics(const float raw[XYZ]) { +void inverse_kinematics(const float (&raw)[XYZ]) { #if HAS_HOTEND_OFFSET // Delta hotend offsets must be applied in Cartesian space with no "spoofing" const float pos[XYZ] = { @@ -224,6 +224,7 @@ void home_delta() { #endif // Init the current position of all carriages to 0,0,0 ZERO(current_position); + ZERO(destination); sync_plan_position(); // Disable stealthChop if used. Enable diag1 pin on driver. @@ -232,9 +233,8 @@ void home_delta() { #endif // Move all carriages together linearly until an endstop is hit. - current_position[X_AXIS] = current_position[Y_AXIS] = current_position[Z_AXIS] = (delta_height + 10); - feedrate_mm_s = homing_feedrate(X_AXIS); - line_to_current_position(); + destination[Z_AXIS] = (delta_height + 10); + buffer_line_to_destination(homing_feedrate(X_AXIS)); planner.synchronize(); // Re-enable stealthChop if used. Disable diag1 pin on driver. @@ -256,7 +256,7 @@ void home_delta() { // give the impression that they are the same. LOOP_XYZ(i) set_axis_is_at_home((AxisEnum)i); - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("<<< home_delta", current_position); diff --git a/Marlin/src/module/delta.h b/Marlin/src/module/delta.h index 2dde72f801..52e2c59acc 100644 --- a/Marlin/src/module/delta.h +++ b/Marlin/src/module/delta.h @@ -24,8 +24,7 @@ * delta.h - Delta-specific functions */ -#ifndef __DELTA_H__ -#define __DELTA_H__ +#pragma once extern float delta_height, delta_endstop_adj[ABC], @@ -78,7 +77,11 @@ void recalc_delta_settings(); delta[C_AXIS] = DELTA_Z(V, C_AXIS); \ }while(0) -void inverse_kinematics(const float raw[XYZ]); +void inverse_kinematics(const float (&raw)[XYZ]); +FORCE_INLINE void inverse_kinematics(const float (&raw)[XYZE]) { + const float raw_xyz[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] }; + inverse_kinematics(raw_xyz); +} /** * Calculate the highest Z position where the @@ -118,5 +121,3 @@ FORCE_INLINE void forward_kinematics_DELTA(const float (&point)[ABC]) { } void home_delta(); - -#endif // __DELTA_H__ diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index dee9983b1b..06db3caf04 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -75,7 +75,7 @@ bool relative_mode; // = false; * Cartesian Current Position * Used to track the native machine position as moves are queued. * Used by 'buffer_line_to_current_position' to do a move after changing it. - * Used by 'SYNC_PLAN_POSITION_KINEMATIC' to update 'planner.position'. + * Used by 'sync_plan_position' to update 'planner.position'. */ float current_position[XYZE] = { 0 }; @@ -218,15 +218,22 @@ void get_cartesian_from_steppers() { * may have been applied. * * To prevent small shifts in axis position always call - * SYNC_PLAN_POSITION_KINEMATIC after updating axes with this. + * sync_plan_position after updating axes with this. * * To keep hosts in sync, always call report_current_position * after updating the current_position. */ void set_current_from_steppers_for_axis(const AxisEnum axis) { get_cartesian_from_steppers(); - #if PLANNER_LEVELING - planner.unapply_leveling(cartes); + + #if HAS_POSITION_MODIFIERS + float pos[XYZE] = { cartes[X_AXIS], cartes[Y_AXIS], cartes[Z_AXIS], current_position[E_AXIS] }; + planner.unapply_modifiers(pos + #if HAS_LEVELING + , true + #endif + ); + const float (&cartes)[XYZE] = pos; #endif if (axis == ALL_AXES) COPY(current_position, cartes); @@ -252,13 +259,6 @@ void buffer_line_to_destination(const float fr_mm_s) { #if IS_KINEMATIC - void sync_plan_position_kinematic() { - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) DEBUG_POS("sync_plan_position_kinematic", current_position); - #endif - planner.set_position_mm_kinematic(current_position); - } - /** * Calculate delta, start a line, and set current_position to destination */ @@ -277,7 +277,7 @@ void buffer_line_to_destination(const float fr_mm_s) { && current_position[E_AXIS] == destination[E_AXIS] ) return; - planner.buffer_line_kinematic(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder); + planner.buffer_line(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s), active_extruder); #endif set_current_from_destination(); @@ -538,7 +538,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, // If the move is only in Z/E don't split up the move if (!xdiff && !ydiff) { - planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder); + planner.buffer_line(rtarget, _feedrate_mm_s, active_extruder); return false; // caller will update current_position } @@ -580,53 +580,22 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, ydiff * inv_segments, zdiff * inv_segments, ediff * inv_segments - }; - - #if !HAS_FEEDRATE_SCALING - const float cartesian_segment_mm = cartesian_mm * inv_segments; + }, + cartesian_segment_mm = cartesian_mm * inv_segments; + + #if ENABLED(SCARA_FEEDRATE_SCALING) + const float inv_duration = _feedrate_mm_s / cartesian_segment_mm; #endif /* SERIAL_ECHOPAIR("mm=", cartesian_mm); SERIAL_ECHOPAIR(" seconds=", seconds); SERIAL_ECHOPAIR(" segments=", segments); - #if !HAS_FEEDRATE_SCALING - SERIAL_ECHOPAIR(" segment_mm=", cartesian_segment_mm); - #endif + SERIAL_ECHOPAIR(" segment_mm=", cartesian_segment_mm); SERIAL_EOL(); //*/ - #if HAS_FEEDRATE_SCALING - // SCARA needs to scale the feed rate from mm/s to degrees/s - // i.e., Complete the angular vector in the given time. - const float segment_length = cartesian_mm * inv_segments, - inv_segment_length = 1.0f / segment_length, // 1/mm/segs - inverse_secs = inv_segment_length * _feedrate_mm_s; - - float oldA = planner.position_float[A_AXIS], - oldB = planner.position_float[B_AXIS] - #if ENABLED(DELTA_FEEDRATE_SCALING) - , oldC = planner.position_float[C_AXIS] - #endif - ; - - /* - SERIAL_ECHOPGM("Scaled kinematic move: "); - SERIAL_ECHOPAIR(" segment_length (inv)=", segment_length); - SERIAL_ECHOPAIR(" (", inv_segment_length); - SERIAL_ECHOPAIR(") _feedrate_mm_s=", _feedrate_mm_s); - SERIAL_ECHOPAIR(" inverse_secs=", inverse_secs); - SERIAL_ECHOPAIR(" oldA=", oldA); - SERIAL_ECHOPAIR(" oldB=", oldB); - #if ENABLED(DELTA_FEEDRATE_SCALING) - SERIAL_ECHOPAIR(" oldC=", oldC); - #endif - SERIAL_EOL(); - safe_delay(5); - //*/ - #endif - - // Get the current position as starting point + // Get the current position as starting point float raw[XYZE]; COPY(raw, current_position); @@ -642,78 +611,20 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, LOOP_XYZE(i) raw[i] += segment_distance[i]; - #if ENABLED(DELTA) && HOTENDS < 2 - DELTA_IK(raw); // Delta can inline its kinematics - #else - inverse_kinematics(raw); - #endif - ADJUST_DELTA(raw); // Adjust Z if bed leveling is enabled - - #if ENABLED(SCARA_FEEDRATE_SCALING) - // For SCARA scale the feed rate from mm/s to degrees/s - // i.e., Complete the angular vector in the given time. - if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder, segment_length)) - break; - /* - SERIAL_ECHO(segments); - SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]); - SERIAL_ECHOPAIR(" A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]); - SERIAL_ECHOLNPAIR(" F", HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs * 60); - safe_delay(5); - //*/ - oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; - #elif ENABLED(DELTA_FEEDRATE_SCALING) - // For DELTA scale the feed rate from Effector mm/s to Carriage mm/s - // i.e., Complete the linear vector in the given time. - if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs, active_extruder, segment_length)) - break; - /* - SERIAL_ECHO(segments); - SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]); - SERIAL_ECHOPAIR(" A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]); SERIAL_ECHOPAIR(" C=", delta[C_AXIS]); - SERIAL_ECHOLNPAIR(" F", SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs * 60); - safe_delay(5); - //*/ - oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldC = delta[C_AXIS]; - #else - if (!planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm)) - break; - #endif + if (!planner.buffer_line(raw, _feedrate_mm_s, active_extruder, cartesian_segment_mm + #if ENABLED(SCARA_FEEDRATE_SCALING) + , inv_duration + #endif + )) + break; } // Ensure last segment arrives at target location. - #if HAS_FEEDRATE_SCALING - inverse_kinematics(rtarget); - ADJUST_DELTA(rtarget); - #endif - - #if ENABLED(SCARA_FEEDRATE_SCALING) - const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB); - if (diff2) { - planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], rtarget[Z_AXIS], rtarget[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder, segment_length); - /* - SERIAL_ECHOPAIR("final: A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]); - SERIAL_ECHOPAIR(" adiff=", delta[A_AXIS] - oldA); SERIAL_ECHOPAIR(" bdiff=", delta[B_AXIS] - oldB); - SERIAL_ECHOLNPAIR(" F", SQRT(diff2) * inverse_secs * 60); - SERIAL_EOL(); - safe_delay(5); - //*/ - } - #elif ENABLED(DELTA_FEEDRATE_SCALING) - const float diff2 = sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC); - if (diff2) { - planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], rtarget[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder, segment_length); - /* - SERIAL_ECHOPAIR("final: A=", delta[A_AXIS]); SERIAL_ECHOPAIR(" B=", delta[B_AXIS]); SERIAL_ECHOPAIR(" C=", delta[C_AXIS]); - SERIAL_ECHOPAIR(" adiff=", delta[A_AXIS] - oldA); SERIAL_ECHOPAIR(" bdiff=", delta[B_AXIS] - oldB); SERIAL_ECHOPAIR(" cdiff=", delta[C_AXIS] - oldC); - SERIAL_ECHOLNPAIR(" F", SQRT(diff2) * inverse_secs * 60); - SERIAL_EOL(); - safe_delay(5); - //*/ - } - #else - planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder, cartesian_segment_mm); - #endif + planner.buffer_line(rtarget, _feedrate_mm_s, active_extruder, cartesian_segment_mm + #if ENABLED(SCARA_FEEDRATE_SCALING) + , inv_duration + #endif + ); return false; // caller will update current_position } @@ -736,7 +647,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, // If the move is only in Z/E don't split up the move if (!xdiff && !ydiff) { - planner.buffer_line_kinematic(destination, fr_mm_s, active_extruder); + planner.buffer_line(destination, fr_mm_s, active_extruder); return; } @@ -766,6 +677,10 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, ediff * inv_segments }; + #if ENABLED(SCARA_FEEDRATE_SCALING) + const float inv_duration = _feedrate_mm_s / cartesian_segment_mm; + #endif + // SERIAL_ECHOPAIR("mm=", cartesian_mm); // SERIAL_ECHOLNPAIR(" segments=", segments); // SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm); @@ -783,13 +698,21 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, idle(); } LOOP_XYZE(i) raw[i] += segment_distance[i]; - if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm)) + if (!planner.buffer_line(raw, fr_mm_s, active_extruder, cartesian_segment_mm + #if ENABLED(SCARA_FEEDRATE_SCALING) + , inv_duration + #endif + )) break; } // Since segment_distance is only approximate, // the final move must be to the exact destination. - planner.buffer_line_kinematic(destination, fr_mm_s, active_extruder, cartesian_segment_mm); + planner.buffer_line(destination, fr_mm_s, active_extruder, cartesian_segment_mm + #if ENABLED(SCARA_FEEDRATE_SCALING) + , inv_duration + #endif + ); } #endif // SEGMENT_LEVELED_MOVES @@ -922,7 +845,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, planner.max_feedrate_mm_s[X_AXIS], 1) ) break; planner.synchronize(); - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); extruder_duplication_enabled = true; active_extruder_parked = false; #if ENABLED(DEBUG_LEVELING_FEATURE) @@ -1092,7 +1015,7 @@ inline float get_homing_bump_feedrate(const AxisEnum axis) { /** * Home an individual linear axis */ -static void do_homing_move(const AxisEnum axis, const float distance, const float fr_mm_s=0.0) { +void do_homing_move(const AxisEnum axis, const float distance, const float fr_mm_s=0.0) { #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) { @@ -1139,18 +1062,29 @@ static void do_homing_move(const AxisEnum axis, const float distance, const floa #endif } - // Tell the planner the axis is at 0 - current_position[axis] = 0; - #if IS_SCARA - SYNC_PLAN_POSITION_KINEMATIC(); - current_position[axis] = distance; - inverse_kinematics(current_position); - planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder); - #else + // Tell the planner the axis is at 0 + current_position[axis] = 0; sync_plan_position(); - current_position[axis] = distance; // Set delta/cartesian axes directly - planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder); + current_position[axis] = distance; + planner.buffer_line(current_position, fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder); + #else + float target[ABCE] = { planner.get_axis_position_mm(A_AXIS), planner.get_axis_position_mm(B_AXIS), planner.get_axis_position_mm(C_AXIS), planner.get_axis_position_mm(E_AXIS) }; + target[axis] = 0; + planner.set_machine_position_mm(target); + target[axis] = distance; + + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + const float delta_mm_cart[XYZE] = {0, 0, 0, 0}; + #endif + + // Set delta/cartesian axes directly + planner.buffer_segment(target + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + , delta_mm_cart + #endif + , fr_mm_s ? fr_mm_s : homing_feedrate(axis), active_extruder + ); #endif planner.synchronize(); @@ -1349,7 +1283,14 @@ void homeaxis(const AxisEnum axis) { if (axis == Z_AXIS && set_bltouch_deployed(true)) return; #endif - do_homing_move(axis, 1.5f * max_length(axis) * axis_home_dir); + do_homing_move(axis, 1.5f * max_length( + #if ENABLED(DELTA) + Z_AXIS + #else + axis + #endif + ) * axis_home_dir + ); #if HOMING_Z_WITH_PROBE && ENABLED(BLTOUCH) // BLTOUCH needs to be stowed after trigger to rearm itself @@ -1481,7 +1422,7 @@ void homeaxis(const AxisEnum axis) { #if IS_SCARA set_axis_is_at_home(axis); - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); #elif ENABLED(DELTA) diff --git a/Marlin/src/module/motion.h b/Marlin/src/module/motion.h index a680c01c9e..7d05a0ebd4 100644 --- a/Marlin/src/module/motion.h +++ b/Marlin/src/module/motion.h @@ -129,13 +129,6 @@ void set_current_from_steppers_for_axis(const AxisEnum axis); void sync_plan_position(); void sync_plan_position_e(); -#if IS_KINEMATIC - void sync_plan_position_kinematic(); - #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position_kinematic() -#else - #define SYNC_PLAN_POSITION_KINEMATIC() sync_plan_position() -#endif - /** * Move the planner to the current position from wherever it last moved * (or from wherever it has been told it is located). @@ -354,20 +347,4 @@ void homeaxis(const AxisEnum axis); void set_home_offset(const AxisEnum axis, const float v); #endif -#if ENABLED(AUTO_BED_LEVELING_BILINEAR) - #if ENABLED(DELTA) - #define ADJUST_DELTA(V) \ - if (planner.leveling_active) { \ - const float zadj = bilinear_z_offset(V); \ - delta[A_AXIS] += zadj; \ - delta[B_AXIS] += zadj; \ - delta[C_AXIS] += zadj; \ - } - #else - #define ADJUST_DELTA(V) if (planner.leveling_active) { delta[Z_AXIS] += bilinear_z_offset(V); } - #endif -#else - #define ADJUST_DELTA(V) NOOP -#endif - #endif // MOTION_H diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index fe286a2acf..98a40fcca1 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -133,8 +133,13 @@ float Planner::max_feedrate_mm_s[XYZE_N], // (mm/s) M203 XYZE - Max speeds float Planner::max_e_jerk; #endif #endif -#else - float Planner::max_jerk[XYZE]; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration. +#endif +#if HAS_CLASSIC_JERK + #if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE) + float Planner::max_jerk[XYZ]; // (mm/s^2) M205 XYZ - The largest speed change requiring no acceleration. + #else + float Planner::max_jerk[XYZE]; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration. + #endif #endif #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) @@ -220,6 +225,10 @@ float Planner::previous_speed[NUM_AXIS], float Planner::position_float[XYZE]; // Needed for accurate maths. Steps cannot be used! #endif +#if IS_KINEMATIC + float Planner::position_cart[XYZE]; +#endif + #if ENABLED(ULTRA_LCD) volatile uint32_t Planner::block_buffer_runtime_us = 0; #endif @@ -235,6 +244,9 @@ void Planner::init() { #if HAS_POSITION_FLOAT ZERO(position_float); #endif + #if IS_KINEMATIC + ZERO(position_cart); + #endif ZERO(previous_speed); previous_nominal_speed_sqr = 0; #if ABL_PLANAR @@ -1354,17 +1366,12 @@ void Planner::check_axes_activity() { } #endif -#if PLANNER_LEVELING || HAS_UBL_AND_CURVES +#if HAS_LEVELING /** * rx, ry, rz - Cartesian positions in mm * Leveled XYZ on completion */ void Planner::apply_leveling(float &rx, float &ry, float &rz) { - - #if ENABLED(SKEW_CORRECTION) - skew(rx, ry, rz); - #endif - if (!leveling_active) return; #if ABL_PLANAR @@ -1406,10 +1413,6 @@ void Planner::check_axes_activity() { #endif } -#endif - -#if PLANNER_LEVELING - void Planner::unapply_leveling(float raw[XYZ]) { if (leveling_active) { @@ -1456,7 +1459,23 @@ void Planner::check_axes_activity() { #endif } -#endif // PLANNER_LEVELING +#endif // HAS_LEVELING + +#if ENABLED(FWRETRACT) + /** + * rz, e - Cartesian positions in mm + */ + void Planner::apply_retract(float &rz, float &e) { + rz += fwretract.current_hop; + e -= fwretract.current_retract[active_extruder]; + } + + void Planner::unapply_retract(float &rz, float &e) { + rz -= fwretract.current_hop; + e += fwretract.current_retract[active_extruder]; + } + +#endif void Planner::quick_stop() { @@ -1554,6 +1573,7 @@ void Planner::synchronize() { * Add a new linear movement to the planner queue (in terms of steps). * * target - target position in steps units + * target_float - target position in direct (mm, degrees) units. optional * fr_mm_s - (target) speed of the move * extruder - target extruder * millimeters - the length of the movement, if known @@ -1562,7 +1582,10 @@ void Planner::synchronize() { */ bool Planner::_buffer_steps(const int32_t (&target)[XYZE] #if HAS_POSITION_FLOAT - , const float (&target_float)[XYZE] + , const float (&target_float)[ABCE] + #endif + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + , const float (&delta_mm_cart)[XYZE] #endif , float fr_mm_s, const uint8_t extruder, const float &millimeters ) { @@ -1579,6 +1602,9 @@ bool Planner::_buffer_steps(const int32_t (&target)[XYZE] #if HAS_POSITION_FLOAT , target_float #endif + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + , delta_mm_cart + #endif , fr_mm_s, extruder, millimeters )) { // Movement was not queued, probably because it was too short. @@ -1618,9 +1644,12 @@ bool Planner::_buffer_steps(const int32_t (&target)[XYZE] * Returns true is movement is acceptable, false otherwise */ bool Planner::_populate_block(block_t * const block, bool split_move, - const int32_t (&target)[XYZE] + const int32_t (&target)[ABCE] #if HAS_POSITION_FLOAT - , const float (&target_float)[XYZE] + , const float (&target_float)[ABCE] + #endif + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + , const float (&delta_mm_cart)[XYZE] #endif , float fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/ ) { @@ -2243,12 +2272,21 @@ bool Planner::_populate_block(block_t * const block, bool split_move, // Unit vector of previous path line segment static float previous_unit_vec[XYZE]; - float unit_vec[] = { - delta_mm[A_AXIS] * inverse_millimeters, - delta_mm[B_AXIS] * inverse_millimeters, - delta_mm[C_AXIS] * inverse_millimeters, - delta_mm[E_AXIS] * inverse_millimeters - }; + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + float unit_vec[] = { + delta_mm_cart[X_AXIS] * inverse_millimeters, + delta_mm_cart[Y_AXIS] * inverse_millimeters, + delta_mm_cart[Z_AXIS] * inverse_millimeters, + delta_mm_cart[E_AXIS] * inverse_millimeters + }; + #else + float unit_vec[] = { + delta_mm[X_AXIS] * inverse_millimeters, + delta_mm[Y_AXIS] * inverse_millimeters, + delta_mm[Z_AXIS] * inverse_millimeters, + delta_mm[E_AXIS] * inverse_millimeters + }; + #endif // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) { @@ -2302,7 +2340,9 @@ bool Planner::_populate_block(block_t * const block, bool split_move, COPY(previous_unit_vec, unit_vec); - #else // Classic Jerk Limiting + #endif + + #if HAS_CLASSIC_JERK /** * Adapted from Průša MKS firmware @@ -2317,7 +2357,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move, float safe_speed = nominal_speed; uint8_t limited = 0; - LOOP_XYZE(i) { + #if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE) + LOOP_XYZ(i) + #else + LOOP_XYZE(i) + #endif + { const float jerk = ABS(current_speed[i]), // cs : Starting from zero, change in speed for this axis maxj = max_jerk[i]; // mj : The max jerk setting for this axis if (jerk > maxj) { // cs > mj : New current speed too fast? @@ -2349,7 +2394,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move, // Now limit the jerk in all axes. const float smaller_speed_factor = vmax_junction / previous_nominal_speed; - LOOP_XYZE(axis) { + #if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE) + LOOP_XYZ(axis) + #else + LOOP_XYZE(axis) + #endif + { // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop. float v_exit = previous_speed[axis] * smaller_speed_factor, v_entry = current_speed[axis]; @@ -2381,7 +2431,12 @@ bool Planner::_populate_block(block_t * const block, bool split_move, vmax_junction = safe_speed; previous_safe_speed = safe_speed; - vmax_junction_sqr = sq(vmax_junction); + + #if ENABLED(JUNCTION_DEVIATION) + vmax_junction_sqr = MIN(vmax_junction_sqr, sq(vmax_junction)); + #else + vmax_junction_sqr = sq(vmax_junction); + #endif #endif // Classic Jerk Limiting @@ -2466,7 +2521,12 @@ void Planner::buffer_sync_block() { * extruder - target extruder * millimeters - the length of the movement, if known */ -bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/) { +bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + , const float (&delta_mm_cart)[XYZE] + #endif + , const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/ +) { // If we are cleaning, do not accept queuing of movements if (cleaning_buffer_counter) return false; @@ -2534,6 +2594,9 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con #if HAS_POSITION_FLOAT , target_float #endif + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + , delta_mm_cart + #endif , fr_mm_s, extruder, millimeters ) ) return false; @@ -2543,24 +2606,84 @@ bool Planner::buffer_segment(const float &a, const float &b, const float &c, con } // buffer_segment() /** - * Directly set the planner XYZ position (and stepper positions) + * Add a new linear movement to the buffer. + * The target is cartesian, it's translated to delta/scara if + * needed. + * + * + * rx,ry,rz,e - target position in mm or degrees + * fr_mm_s - (target) speed of the move (mm/s) + * extruder - target extruder + * millimeters - the length of the movement, if known + * inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled) + */ +bool Planner::buffer_line(const float &rx, const float &ry, const float &rz, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters + #if ENABLED(SCARA_FEEDRATE_SCALING) + , const float &inv_duration + #endif +) { + float raw[XYZE] = { rx, ry, rz, e }; + #if HAS_POSITION_MODIFIERS + apply_modifiers(raw); + #endif + + #if IS_KINEMATIC + const float delta_mm_cart[] = { + rx - position_cart[X_AXIS], + ry - position_cart[Y_AXIS], + rz - position_cart[Z_AXIS] + #if ENABLED(JUNCTION_DEVIATION) + , e - position_cart[E_AXIS] + #endif + }; + + float mm = millimeters; + if (mm == 0.0) + mm = (delta_mm_cart[X_AXIS] != 0.0 || delta_mm_cart[Y_AXIS] != 0.0) ? SQRT(sq(delta_mm_cart[X_AXIS]) + sq(delta_mm_cart[Y_AXIS]) + sq(delta_mm_cart[Z_AXIS])) : fabs(delta_mm_cart[Z_AXIS]); + + inverse_kinematics(raw); + + #if ENABLED(SCARA_FEEDRATE_SCALING) + // For SCARA scale the feed rate from mm/s to degrees/s + // i.e., Complete the angular vector in the given time. + const float duration_recip = inv_duration ? inv_duration : fr_mm_s / mm, + feedrate = HYPOT(delta[A_AXIS] - position_float[A_AXIS], delta[B_AXIS] - position_float[B_AXIS]) * duration_recip; + #else + const float feedrate = fr_mm_s; + #endif + if (buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS] + #if ENABLED(JUNCTION_DEVIATION) + , delta_mm_cart + #endif + , feedrate, extruder, mm + )) { + position_cart[X_AXIS] = rx; + position_cart[Y_AXIS] = ry; + position_cart[Z_AXIS] = rz; + position_cart[E_AXIS] = e; + return true; + } + else + return false; + #else + return buffer_segment(raw, fr_mm_s, extruder, millimeters); + #endif +} // buffer_line() + +/** + * Directly set the planner ABC position (and stepper positions) * converting mm (or angles for SCARA) into steps. * - * On CORE machines stepper ABC will be translated from the given XYZ. + * The provided ABC position is in machine units. */ -void Planner::_set_position_mm(const float &a, const float &b, const float &c, const float &e) { +void Planner::set_machine_position_mm(const float &a, const float &b, const float &c, const float &e) { #if ENABLED(DISTINCT_E_FACTORS) last_extruder = active_extruder; #endif position[A_AXIS] = LROUND(a * axis_steps_per_mm[A_AXIS]); position[B_AXIS] = LROUND(b * axis_steps_per_mm[B_AXIS]); - position[C_AXIS] = LROUND(axis_steps_per_mm[C_AXIS] * (c +( - #if !IS_KINEMATIC && ENABLED(AUTO_BED_LEVELING_UBL) - leveling_active ? ubl.get_z_correction(a, b) : - #endif - 0) - )); + position[C_AXIS] = LROUND(c * axis_steps_per_mm[C_AXIS]); position[E_AXIS] = LROUND(e * axis_steps_per_mm[_EINDEX]); #if HAS_POSITION_FLOAT position_float[A_AXIS] = a; @@ -2577,44 +2700,54 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c stepper.set_position(position[A_AXIS], position[B_AXIS], position[C_AXIS], position[E_AXIS]); } -void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) { - #if PLANNER_LEVELING - float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] }; - apply_leveling(raw); - #else - const float (&raw)[XYZE] = cart; +void Planner::set_position_mm(const float &rx, const float &ry, const float &rz, const float &e) { + float raw[XYZE] = { rx, ry, rz, e }; + #if HAS_POSITION_MODIFIERS + apply_modifiers(raw + #if HAS_LEVELING + , true + #endif + ); #endif #if IS_KINEMATIC + position_cart[X_AXIS] = rx; + position_cart[Y_AXIS] = ry; + position_cart[Z_AXIS] = rz; + position_cart[E_AXIS] = e; + inverse_kinematics(raw); - _set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS]); + set_machine_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS]); #else - _set_position_mm(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS]); + set_machine_position_mm(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS]); #endif } /** * Setters for planner position (also setting stepper position). */ -void Planner::set_position_mm(const AxisEnum axis, const float &v) { +void Planner::set_e_position_mm(const float &e) { #if ENABLED(DISTINCT_E_FACTORS) - const uint8_t axis_index = axis + (axis == E_AXIS ? active_extruder : 0); + const uint8_t axis_index = E_AXIS + active_extruder; last_extruder = active_extruder; #else - const uint8_t axis_index = axis; + const uint8_t axis_index = E_AXIS; #endif - position[axis] = LROUND(axis_steps_per_mm[axis_index] * (v + ( - #if ENABLED(AUTO_BED_LEVELING_UBL) - axis == Z_AXIS && leveling_active ? ubl.get_z_correction(current_position[X_AXIS], current_position[Y_AXIS]) : - #endif - 0) - )); + #if ENABLED(FWRETRACT) + float e_new = e - fwretract.current_retract[active_extruder]; + #else + const float e_new = e; + #endif + position[E_AXIS] = LROUND(axis_steps_per_mm[axis_index] * e_new); #if HAS_POSITION_FLOAT - position_float[axis] = v; + position_float[E_AXIS] = e_new; + #endif + #if IS_KINEMATIC + position_cart[E_AXIS] = e; #endif if (has_blocks_queued()) buffer_sync_block(); else - stepper.set_position(axis, position[axis]); + stepper.set_position(E_AXIS, position[E_AXIS]); } // Recalculate the steps/s^2 acceleration rates, based on the mm/s^2 @@ -2638,7 +2771,7 @@ void Planner::reset_acceleration_rates() { // Recalculate position, steps_to_mm if axis_steps_per_mm changes! void Planner::refresh_positioning() { LOOP_XYZE_N(i) steps_to_mm[i] = 1.0f / axis_steps_per_mm[i]; - set_position_mm_kinematic(current_position); + set_position_mm(current_position); reset_acceleration_rates(); } diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 48f392a65b..8b745d5de8 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -45,6 +45,10 @@ #include "../libs/vector_3.h" #endif +#if ENABLED(FWRETRACT) + #include "../feature/fwretract.h" +#endif + enum BlockFlagBit : char { // Recalculate trapezoids on entry junction. For optimization. BLOCK_BIT_RECALCULATE, @@ -151,7 +155,7 @@ typedef struct { } block_t; -#define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || HAS_FEEDRATE_SCALING) +#define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || ENABLED(SCARA_FEEDRATE_SCALING)) #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1)) @@ -210,14 +214,22 @@ class Planner { #if ENABLED(JUNCTION_DEVIATION) static float junction_deviation_mm; // (mm) M205 J #if ENABLED(LIN_ADVANCE) - #if ENABLED(DISTINCT_E_FACTORS) - static float max_e_jerk[EXTRUDERS]; // Calculated from junction_deviation_mm - #else - static float max_e_jerk; - #endif + static float max_e_jerk // Calculated from junction_deviation_mm + #if ENABLED(DISTINCT_E_FACTORS) + [EXTRUDERS] + #endif + ; #endif - #else - static float max_jerk[XYZE]; // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration. + #endif + + #if HAS_CLASSIC_JERK + static float max_jerk[ + #if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE) + XYZ // (mm/s^2) M205 XYZ - The largest speed change requiring no acceleration. + #else + XYZE // (mm/s^2) M205 XYZE - The largest speed change requiring no acceleration. + #endif + ]; #endif #if HAS_LEVELING @@ -240,6 +252,10 @@ class Planner { static float position_float[XYZE]; #endif + #if IS_KINEMATIC + static float position_cart[XYZE]; + #endif + #if ENABLED(SKEW_CORRECTION) #if ENABLED(SKEW_CORRECTION_GCODE) static float xy_skew_factor; @@ -410,6 +426,8 @@ class Planner { } } } + FORCE_INLINE static void skew(float (&raw)[XYZ]) { skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); } + FORCE_INLINE static void skew(float (&raw)[XYZE]) { skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); } FORCE_INLINE static void unskew(float &cx, float &cy, const float &cz) { if (WITHIN(cx, X_MIN_POS, X_MAX_POS) && WITHIN(cy, Y_MIN_POS, Y_MAX_POS)) { @@ -420,29 +438,76 @@ class Planner { } } } + FORCE_INLINE static void unskew(float (&raw)[XYZ]) { unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); } + FORCE_INLINE static void unskew(float (&raw)[XYZE]) { unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); } #endif // SKEW_CORRECTION - #if PLANNER_LEVELING || HAS_UBL_AND_CURVES + #if HAS_LEVELING /** * Apply leveling to transform a cartesian position * as it will be given to the planner and steppers. */ static void apply_leveling(float &rx, float &ry, float &rz); FORCE_INLINE static void apply_leveling(float (&raw)[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); } + FORCE_INLINE static void apply_leveling(float (&raw)[XYZE]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); } + + static void unapply_leveling(float raw[XYZ]); #endif - #if PLANNER_LEVELING - #define ARG_X float rx - #define ARG_Y float ry - #define ARG_Z float rz - static void unapply_leveling(float raw[XYZ]); - #else - #define ARG_X const float &rx - #define ARG_Y const float &ry - #define ARG_Z const float &rz + #if ENABLED(FWRETRACT) + static void apply_retract(float &rz, float &e); + FORCE_INLINE static void apply_retract(float (&raw)[XYZE]) { apply_retract(raw[Z_AXIS], raw[E_AXIS]); } + static void unapply_retract(float &rz, float &e); + FORCE_INLINE static void unapply_retract(float (&raw)[XYZE]) { unapply_retract(raw[Z_AXIS], raw[E_AXIS]); } #endif + #if HAS_POSITION_MODIFIERS + FORCE_INLINE static void apply_modifiers(float (&pos)[XYZE] + #if HAS_LEVELING + , bool leveling = + #if PLANNER_LEVELING + true + #else + false + #endif + #endif + ) { + #if ENABLED(SKEW_CORRECTION) + skew(pos); + #endif + #if HAS_LEVELING + if (leveling) + apply_leveling(pos); + #endif + #if ENABLED(FWRETRACT) + apply_retract(pos); + #endif + } + + FORCE_INLINE static void unapply_modifiers(float (&pos)[XYZE] + #if HAS_LEVELING + , bool leveling = + #if PLANNER_LEVELING + true + #else + false + #endif + #endif + ) { + #if ENABLED(FWRETRACT) + unapply_retract(pos); + #endif + #if HAS_LEVELING + if (leveling) + unapply_leveling(pos); + #endif + #if ENABLED(SKEW_CORRECTION) + unskew(pos); + #endif + } + #endif // HAS_POSITION_MODIFIERS + // Number of moves currently in the planner including the busy block, if any FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); } @@ -489,7 +554,10 @@ class Planner { */ static bool _buffer_steps(const int32_t (&target)[XYZE] #if HAS_POSITION_FLOAT - , const float (&target_float)[XYZE] + , const float (&target_float)[ABCE] + #endif + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + , const float (&delta_mm_cart)[XYZE] #endif , float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0 ); @@ -511,6 +579,9 @@ class Planner { #if HAS_POSITION_FLOAT , const float (&target_float)[XYZE] #endif + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + , const float (&delta_mm_cart)[XYZE] + #endif , float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0 ); @@ -520,6 +591,13 @@ class Planner { */ static void buffer_sync_block(); + #if IS_KINEMATIC + private: + + // Allow do_homing_move to access internal functions, such as buffer_segment. + friend void do_homing_move(const AxisEnum, const float, const float); + #endif + /** * Planner::buffer_segment * @@ -532,74 +610,83 @@ class Planner { * extruder - target extruder * millimeters - the length of the movement, if known */ - static bool buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0); - - static void _set_position_mm(const float &a, const float &b, const float &c, const float &e); - - /** - * Add a new linear movement to the buffer. - * The target is NOT translated to delta/scara - * - * Leveling will be applied to input on cartesians. - * Kinematic machines should call buffer_line_kinematic (for leveled moves). - * (Cartesians may also call buffer_line_kinematic.) - * - * rx,ry,rz,e - target position in mm or degrees - * fr_mm_s - (target) speed of the move (mm/s) - * extruder - target extruder - * millimeters - the length of the movement, if known - */ - FORCE_INLINE static bool buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) { - #if PLANNER_LEVELING && IS_CARTESIAN - apply_leveling(rx, ry, rz); + static bool buffer_segment(const float &a, const float &b, const float &c, const float &e + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + , const float (&delta_mm_cart)[XYZE] #endif - return buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters); + , const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0 + ); + + FORCE_INLINE static bool buffer_segment(const float (&abce)[ABCE] + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + , const float (&delta_mm_cart)[XYZE] + #endif + , const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0 + ) { + return buffer_segment(abce[A_AXIS], abce[B_AXIS], abce[C_AXIS], abce[E_AXIS] + #if IS_KINEMATIC && ENABLED(JUNCTION_DEVIATION) + , delta_mm_cart + #endif + , fr_mm_s, extruder, millimeters); } + public: + /** * Add a new linear movement to the buffer. * The target is cartesian, it's translated to delta/scara if * needed. * - * cart - x,y,z,e CARTESIAN target in mm + * + * rx,ry,rz,e - target position in mm or degrees * fr_mm_s - (target) speed of the move (mm/s) * extruder - target extruder * millimeters - the length of the movement, if known + * inv_duration - the reciprocal if the duration of the movement, if known (kinematic only if feeedrate scaling is enabled) */ - FORCE_INLINE static bool buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) { - #if PLANNER_LEVELING - float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] }; - apply_leveling(raw); - #else - const float (&raw)[XYZE] = cart; + static bool buffer_line(const float &rx, const float &ry, const float &rz, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters=0.0 + #if ENABLED(SCARA_FEEDRATE_SCALING) + , const float &inv_duration=0.0 #endif - #if IS_KINEMATIC - inverse_kinematics(raw); - return buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters); - #else - return buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters); + ); + + FORCE_INLINE static bool buffer_line(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters=0.0 + #if ENABLED(SCARA_FEEDRATE_SCALING) + , const float &inv_duration=0.0 #endif + ) { + return buffer_line(cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters + #if ENABLED(SCARA_FEEDRATE_SCALING) + , inv_duration + #endif + ); } /** * Set the planner.position and individual stepper positions. * Used by G92, G28, G29, and other procedures. + * + * The supplied position is in the cartesian coordinate space and is + * translated in to machine space as needed. Modifiers such as leveling + * and skew are also applied. * * Multiplies by axis_steps_per_mm[] and does necessary conversion * for COREXY / COREXZ / COREYZ to set the corresponding stepper positions. * * Clears previous speed values. */ - FORCE_INLINE static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) { - #if PLANNER_LEVELING && IS_CARTESIAN - apply_leveling(rx, ry, rz); - #endif - _set_position_mm(rx, ry, rz, e); - } - static void set_position_mm_kinematic(const float (&cart)[XYZE]); - static void set_position_mm(const AxisEnum axis, const float &v); - FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); } - FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); } + static void set_position_mm(const float &rx, const float &ry, const float &rz, const float &e); + FORCE_INLINE static void set_position_mm(const float (&cart)[XYZE]) { set_position_mm(cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS], cart[E_AXIS]); } + static void set_e_position_mm(const float &e); + + /** + * Set the planner.position and individual stepper positions. + * + * The supplied position is in machine space, and no additional + * conversions are applied. + */ + static void set_machine_position_mm(const float &a, const float &b, const float &c, const float &e); + FORCE_INLINE static void set_machine_position_mm(const float (&abce)[ABCE]) { set_machine_position_mm(abce[A_AXIS], abce[B_AXIS], abce[C_AXIS], abce[E_AXIS]); } /** * Get an axis position according to stepper position(s) @@ -756,16 +843,14 @@ class Planner { static void autotemp_M104_M109(); #endif - #if ENABLED(JUNCTION_DEVIATION) + #if ENABLED(JUNCTION_DEVIATION) && ENABLED(LIN_ADVANCE) FORCE_INLINE static void recalculate_max_e_jerk() { #define GET_MAX_E_JERK(N) SQRT(SQRT(0.5) * junction_deviation_mm * (N) * RECIPROCAL(1.0 - SQRT(0.5))) - #if ENABLED(LIN_ADVANCE) - #if ENABLED(DISTINCT_E_FACTORS) - for (uint8_t i = 0; i < EXTRUDERS; i++) - max_e_jerk[i] = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS + i]); - #else - max_e_jerk = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS]); - #endif + #if ENABLED(DISTINCT_E_FACTORS) + for (uint8_t i = 0; i < EXTRUDERS; i++) + max_e_jerk[i] = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS + i]); + #else + max_e_jerk = GET_MAX_E_JERK(max_acceleration_mm_per_s2[E_AXIS]); #endif } #endif diff --git a/Marlin/src/module/planner_bezier.cpp b/Marlin/src/module/planner_bezier.cpp index 6ff6808a79..a547e23086 100644 --- a/Marlin/src/module/planner_bezier.cpp +++ b/Marlin/src/module/planner_bezier.cpp @@ -190,15 +190,15 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS] bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t); clamp_to_software_endstops(bez_target); - #if HAS_UBL_AND_CURVES - float pos[XYZ] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS] }; + #if HAS_LEVELING && !PLANNER_LEVELING + float pos[XYZE] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS], bez_target[E_AXIS] }; planner.apply_leveling(pos); - if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], bez_target[E_AXIS], fr_mm_s, active_extruder)) - break; #else - if (!planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder)) - break; + const float (&pos)[XYZE] = bez_target; #endif + + if (!planner.buffer_line(pos, fr_mm_s, active_extruder, step)) + break; } } diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp index ed2ff0b9aa..c4d4bc22d4 100644 --- a/Marlin/src/module/probe.cpp +++ b/Marlin/src/module/probe.cpp @@ -537,7 +537,7 @@ static bool do_probe_move(const float z, const float fr_mm_s) { set_current_from_steppers_for_axis(Z_AXIS); // Tell the planner where we actually are - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("<<< do_probe_move", current_position); diff --git a/Marlin/src/module/scara.cpp b/Marlin/src/module/scara.cpp index 222bf9cbe4..225e4a25d8 100644 --- a/Marlin/src/module/scara.cpp +++ b/Marlin/src/module/scara.cpp @@ -104,7 +104,7 @@ void forward_kinematics_SCARA(const float &a, const float &b) { * Maths and first version by QHARLEY. * Integrated into Marlin and slightly restructured by Joachim Cerny. */ -void inverse_kinematics(const float raw[XYZ]) { +void inverse_kinematics(const float (&raw)[XYZ]) { static float C2, S2, SK1, SK2, THETA, PSI; diff --git a/Marlin/src/module/scara.h b/Marlin/src/module/scara.h index 501c46dc28..fc7049997a 100644 --- a/Marlin/src/module/scara.h +++ b/Marlin/src/module/scara.h @@ -24,8 +24,7 @@ * scara.h - SCARA-specific functions */ -#ifndef __SCARA_H__ -#define __SCARA_H__ +#pragma once #include "../core/macros.h" @@ -38,9 +37,11 @@ float constexpr L1 = SCARA_LINKAGE_1, L2 = SCARA_LINKAGE_2, void scara_set_axis_is_at_home(const AxisEnum axis); -void inverse_kinematics(const float raw[XYZ]); +void inverse_kinematics(const float (&raw)[XYZ]); +FORCE_INLINE void inverse_kinematics(const float (&raw)[XYZE]) { + const float raw_xyz[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] }; + inverse_kinematics(raw_xyz); +} void forward_kinematics_SCARA(const float &a, const float &b); void scara_report_positions(); - -#endif // __SCARA_H__ diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp index 7c4b52aa8b..a1cb42aa23 100644 --- a/Marlin/src/module/tool_change.cpp +++ b/Marlin/src/module/tool_change.cpp @@ -134,7 +134,7 @@ #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("(1) Raise Z-Axis", current_position); #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder); planner.synchronize(); // STEP 2 @@ -145,7 +145,7 @@ DEBUG_POS("Moving ParkPos", current_position); } #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder); planner.synchronize(); // STEP 3 @@ -163,7 +163,7 @@ #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("Move away from parked extruder", current_position); #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder); planner.synchronize(); // STEP 5 @@ -178,12 +178,12 @@ // STEP 6 current_position[X_AXIS] = grabpos + (tmp_extruder ? -10 : 10); - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder); current_position[X_AXIS] = grabpos; #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("(6) Unpark extruder", current_position); #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS]/2, active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS]/2, active_extruder); planner.synchronize(); // Step 7 @@ -191,7 +191,7 @@ #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("(7) Move midway between hotends", current_position); #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder); planner.synchronize(); #if ENABLED(DEBUG_LEVELING_FEATURE) SERIAL_ECHOLNPGM("Autopark done."); @@ -241,7 +241,7 @@ #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("(1) Raise Z-Axis", current_position); #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder); planner.synchronize(); // STEP 2 @@ -252,14 +252,14 @@ DEBUG_POS("Move X SwitchPos", current_position); } #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder); planner.synchronize(); current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - SWITCHING_TOOLHEAD_Y_SECURITY; #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos + Security", current_position); #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); planner.synchronize(); // STEP 3 @@ -273,14 +273,14 @@ #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos", current_position); #endif - planner.buffer_line_kinematic(current_position,(planner.max_feedrate_mm_s[Y_AXIS] * 0.5), active_extruder); + planner.buffer_line(current_position,(planner.max_feedrate_mm_s[Y_AXIS] * 0.5), active_extruder); planner.synchronize(); safe_delay(200); current_position[Y_AXIS] -= SWITCHING_TOOLHEAD_Y_CLEAR; #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position); #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead + planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead planner.synchronize(); // STEP 4 @@ -291,13 +291,13 @@ #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("Move to new toolhead X", current_position); #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[X_AXIS], active_extruder); planner.synchronize(); current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - SWITCHING_TOOLHEAD_Y_SECURITY; #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos + Security", current_position); #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); planner.synchronize(); // STEP 5 @@ -308,7 +308,7 @@ #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos", current_position); #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS] * 0.5, active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS] * 0.5, active_extruder); planner.synchronize(); safe_delay(200); @@ -319,7 +319,7 @@ #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position); #endif - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead + planner.buffer_line(current_position, planner.max_feedrate_mm_s[Y_AXIS], active_extruder); // move away from docked toolhead planner.synchronize(); // STEP 6 @@ -524,7 +524,7 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n #if ENABLED(SWITCHING_NOZZLE) // Always raise by at least 1 to avoid workpiece current_position[Z_AXIS] += MAX(-zdiff, 0.0) + 1; - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder); move_nozzle_servo(tmp_extruder); #endif #endif @@ -549,7 +549,7 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n #endif // !DUAL_X_CARRIAGE // Tell the planner the new "current position" - SYNC_PLAN_POSITION_KINEMATIC(); + sync_plan_position(); #if ENABLED(DELTA) //LOOP_XYZ(i) update_software_endstops(i); // or modify the constrain function @@ -563,7 +563,7 @@ void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool n #if DISABLED(SWITCHING_NOZZLE) // Do a small lift to avoid the workpiece in the move back (below) current_position[Z_AXIS] += 1.0; - planner.buffer_line_kinematic(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder); + planner.buffer_line(current_position, planner.max_feedrate_mm_s[Z_AXIS], active_extruder); #endif #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("Move back", destination);