From 8eaac0dab37b4376ac06f1fefd469cdd6bc80673 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 30 Jun 2018 13:44:27 -0500 Subject: [PATCH] Add delta feedrate scaling (#11153) --- .../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/gcode/motion/G2_G3.cpp | 32 +++++++++-- Marlin/src/inc/Conditionals_post.h | 1 + Marlin/src/module/motion.cpp | 56 +++++++++++++++---- Marlin/src/module/planner.cpp | 6 +- Marlin/src/module/planner.h | 2 +- 13 files changed, 100 insertions(+), 21 deletions(-) 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 10084ef04..60ae2d46a 100644 --- a/Marlin/src/config/examples/delta/FLSUN/auto_calibrate/Configuration.h +++ b/Marlin/src/config/examples/delta/FLSUN/auto_calibrate/Configuration.h @@ -522,6 +522,9 @@ // 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 82ab7866d..5ee4b81c9 100644 --- a/Marlin/src/config/examples/delta/FLSUN/kossel/Configuration.h +++ b/Marlin/src/config/examples/delta/FLSUN/kossel/Configuration.h @@ -522,6 +522,9 @@ // 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 c95791a12..d1e4654d2 100644 --- a/Marlin/src/config/examples/delta/FLSUN/kossel_mini/Configuration.h +++ b/Marlin/src/config/examples/delta/FLSUN/kossel_mini/Configuration.h @@ -522,6 +522,9 @@ // 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 52429cb74..5955b14da 100644 --- a/Marlin/src/config/examples/delta/Hatchbox_Alpha/Configuration.h +++ b/Marlin/src/config/examples/delta/Hatchbox_Alpha/Configuration.h @@ -527,6 +527,9 @@ // 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 863ab7b3a..b844f4a03 100644 --- a/Marlin/src/config/examples/delta/generic/Configuration.h +++ b/Marlin/src/config/examples/delta/generic/Configuration.h @@ -512,6 +512,9 @@ // 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 b0a8155ee..0cfa45c12 100644 --- a/Marlin/src/config/examples/delta/kossel_mini/Configuration.h +++ b/Marlin/src/config/examples/delta/kossel_mini/Configuration.h @@ -512,6 +512,9 @@ // 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 1a5190c12..d897f8ef8 100644 --- a/Marlin/src/config/examples/delta/kossel_pro/Configuration.h +++ b/Marlin/src/config/examples/delta/kossel_pro/Configuration.h @@ -498,6 +498,9 @@ // 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 0d742089b..f909345e0 100644 --- a/Marlin/src/config/examples/delta/kossel_xl/Configuration.h +++ b/Marlin/src/config/examples/delta/kossel_xl/Configuration.h @@ -516,6 +516,9 @@ // 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/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp index 4c2c5ac91..46cd9daa3 100644 --- a/Marlin/src/gcode/motion/G2_G3.cpp +++ b/Marlin/src/gcode/motion/G2_G3.cpp @@ -35,7 +35,7 @@ #include "../../module/scara.h" #endif -#if ENABLED(SCARA_FEEDRATE_SCALING) && ENABLED(AUTO_BED_LEVELING_BILINEAR) +#if HAS_FEEDRATE_SCALING && ENABLED(AUTO_BED_LEVELING_BILINEAR) #include "../../feature/bedlevel/abl/abl.h" #endif @@ -141,12 +141,16 @@ void plan_arc( millis_t next_idle_ms = millis() + 200UL; - #if ENABLED(SCARA_FEEDRATE_SCALING) + #if HAS_FEEDRATE_SCALING // SCARA needs to scale the feed rate from mm/s to degrees/s const float inv_segment_length = 1.0 / (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]; + oldB = planner.position_float[B_AXIS] + #if ENABLED(DELTA_FEEDRATE_SCALING) + , oldC = planner.position_float[C_AXIS] + #endif + ; #endif #if N_ARC_CORRECTION > 1 @@ -192,14 +196,23 @@ void plan_arc( clamp_to_software_endstops(raw); + #if HAS_FEEDRATE_SCALING + inverse_kinematics(raw); + ADJUST_DELTA(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. - inverse_kinematics(raw); - ADJUST_DELTA(raw); 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)) 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)) + 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); @@ -212,12 +225,19 @@ void plan_arc( } // Ensure last segment arrives at target location. - #if ENABLED(SCARA_FEEDRATE_SCALING) + #if HAS_FEEDRATE_SCALING inverse_kinematics(cart); ADJUST_DELTA(cart); + #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); + #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); #elif HAS_UBL_AND_CURVES float pos[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] }; planner.apply_leveling(pos); diff --git a/Marlin/src/inc/Conditionals_post.h b/Marlin/src/inc/Conditionals_post.h index 8a9f5e895..417fc59d9 100644 --- a/Marlin/src/inc/Conditionals_post.h +++ b/Marlin/src/inc/Conditionals_post.h @@ -1053,6 +1053,7 @@ #define PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_SEGMENTED || ENABLED(SKEW_CORRECTION)) #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)) #if ENABLED(AUTO_BED_LEVELING_UBL) #undef LCD_BED_LEVELING diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index e8e3ee5bb..edacc94d6 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -581,7 +581,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, ediff * inv_segments }; - #if DISABLED(SCARA_FEEDRATE_SCALING) + #if !HAS_FEEDRATE_SCALING const float cartesian_segment_mm = cartesian_mm * inv_segments; #endif @@ -589,14 +589,13 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, SERIAL_ECHOPAIR("mm=", cartesian_mm); SERIAL_ECHOPAIR(" seconds=", seconds); SERIAL_ECHOPAIR(" segments=", segments); - #if DISABLED(SCARA_FEEDRATE_SCALING) - SERIAL_ECHOLNPAIR(" segment_mm=", cartesian_segment_mm); - #else - SERIAL_EOL(); + #if !HAS_FEEDRATE_SCALING + SERIAL_ECHOPAIR(" segment_mm=", cartesian_segment_mm); #endif + SERIAL_EOL(); //*/ - #if ENABLED(SCARA_FEEDRATE_SCALING) + #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, @@ -604,7 +603,11 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, inverse_secs = inv_segment_length * _feedrate_mm_s; float oldA = planner.position_float[A_AXIS], - oldB = planner.position_float[B_AXIS]; + oldB = planner.position_float[B_AXIS] + #if ENABLED(DELTA_FEEDRATE_SCALING) + , oldC = planner.position_float[C_AXIS] + #endif + ; /* SERIAL_ECHOPGM("Scaled kinematic move: "); @@ -613,7 +616,11 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, SERIAL_ECHOPAIR(") _feedrate_mm_s=", _feedrate_mm_s); SERIAL_ECHOPAIR(" inverse_secs=", inverse_secs); SERIAL_ECHOPAIR(" oldA=", oldA); - SERIAL_ECHOLNPAIR(" oldB=", oldB); + SERIAL_ECHOPAIR(" oldB=", oldB); + #if ENABLED(DELTA_FEEDRATE_SCALING) + SERIAL_ECHOPAIR(" oldC=", oldC); + #endif + SERIAL_EOL(); safe_delay(5); //*/ #endif @@ -654,6 +661,19 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, 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)) + 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; @@ -661,17 +681,31 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, } // Ensure last segment arrives at target location. - #if ENABLED(SCARA_FEEDRATE_SCALING) + #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); - /* 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_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); + /* + 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); //*/ diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 4152d2e0d..7da5403c6 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -1567,9 +1567,9 @@ bool Planner::_buffer_steps(const int32_t (&target)[XYZE] // Fill the block with the specified movement if (!_populate_block(block, false, target - #if HAS_POSITION_FLOAT - , target_float - #endif + #if HAS_POSITION_FLOAT + , target_float + #endif , fr_mm_s, extruder, millimeters )) { // Movement was not queued, probably because it was too short. diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 96382014c..c3a912b46 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -149,7 +149,7 @@ typedef struct { } block_t; -#define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || ENABLED(SCARA_FEEDRATE_SCALING)) +#define HAS_POSITION_FLOAT (ENABLED(LIN_ADVANCE) || HAS_FEEDRATE_SCALING) #define BLOCK_MOD(n) ((n)&(BLOCK_BUFFER_SIZE-1))