From a4b46eaf8f45ef9f8c212b9f17692682cae08aaa Mon Sep 17 00:00:00 2001 From: XDA-Bam <1209896+XDA-Bam@users.noreply.github.com> Date: Tue, 16 Jun 2020 01:54:00 +0200 Subject: [PATCH] Add JD_HANDLE_SMALL_SEGMENTS option (#18316) --- Marlin/Configuration.h | 2 + Marlin/src/module/planner.cpp | 164 +++++++++++++++++----------------- Marlin/src/module/planner.h | 2 +- 3 files changed, 87 insertions(+), 81 deletions(-) diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h index 6aac20dd1e..2a4e958d9b 100644 --- a/Marlin/Configuration.h +++ b/Marlin/Configuration.h @@ -815,6 +815,8 @@ */ #if DISABLED(CLASSIC_JERK) #define JUNCTION_DEVIATION_MM 0.013 // (mm) Distance from real junction edge + #define JD_HANDLE_SMALL_SEGMENTS // Use curvature estimation instead of just the junction angle + // for small segments (< 1mm) with large junction angles (> 135°). #endif /** diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index bc6a067f59..2703cd64cf 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -2352,89 +2352,93 @@ bool Planner::_populate_block(block_t * const block, bool split_move, vmax_junction_sqr = junction_acceleration * junction_deviation_mm * sin_theta_d2 / (1.0f - sin_theta_d2); - // For small moves with >135° junction (octagon) find speed for approximate arc - if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) { - - #if ENABLED(JD_USE_MATH_ACOS) - - #error "TODO: Inline maths with the MCU / FPU." - - #elif ENABLED(JD_USE_LOOKUP_TABLE) - - // Fast acos approximation (max. error +-0.01 rads) - // Based on LUT table and linear interpolation - - /** - * // Generate the JD Lookup Table - * constexpr float c = 1.00751495f; // Correction factor to center error around 0 - * for (int i = 0; i < jd_lut_count - 1; ++i) { - * const float x0 = (sq(i) - 1) / sq(i), - * y0 = acos(x0) * (i == 0 ? 1 : c), - * x1 = i < jd_lut_count - 1 ? 0.5 * x0 + 0.5 : 0.999999f, - * y1 = acos(x1) * (i < jd_lut_count - 1 ? c : 1); - * jd_lut_k[i] = (y0 - y1) / (x0 - x1); - * jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1); - * } - * - * // Compute correction factor (Set c to 1.0f first!) - * float min = INFINITY, max = -min; - * for (float t = 0; t <= 1; t += 0.0003f) { - * const float e = acos(t) / approx(t); - * if (isfinite(e)) { - * if (e < min) min = e; - * if (e > max) max = e; - * } - * } - * fprintf(stderr, "%.9gf, ", (min + max) / 2); - */ - static constexpr int16_t jd_lut_count = 16; - static constexpr uint16_t jd_lut_tll = _BV(jd_lut_count - 1); - static constexpr int16_t jd_lut_tll0 = __builtin_clz(jd_lut_tll) + 1; // i.e., 16 - jd_lut_count + 1 - static constexpr float jd_lut_k[jd_lut_count] PROGMEM = { - -1.03145837f, -1.30760646f, -1.75205851f, -2.41705704f, - -3.37769222f, -4.74888992f, -6.69649887f, -9.45661736f, - -13.3640480f, -18.8928222f, -26.7136841f, -37.7754593f, - -53.4201813f, -75.5458374f, -106.836761f, -218.532821f }; - static constexpr float jd_lut_b[jd_lut_count] PROGMEM = { - 1.57079637f, 1.70887053f, 2.04220939f, 2.62408352f, - 3.52467871f, 4.85302639f, 6.77020454f, 9.50875854f, - 13.4009285f, 18.9188995f, 26.7321243f, 37.7885055f, - 53.4293975f, 75.5523529f, 106.841369f, 218.534011f }; - - const float neg = junction_cos_theta < 0 ? -1 : 1, - t = neg * junction_cos_theta; - - const int16_t idx = (t < 0.00000003f) ? 0 : __builtin_clz(uint16_t((1.0f - t) * jd_lut_tll)) - jd_lut_tll0; - - float junction_theta = t * pgm_read_float(&jd_lut_k[idx]) + pgm_read_float(&jd_lut_b[idx]); - if (neg > 0) junction_theta = RADIANS(180) - junction_theta; // acos(-t) + #if ENABLED(JD_HANDLE_SMALL_SEGMENTS) + + // For small moves with >135° junction (octagon) find speed for approximate arc + if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) { + + #if ENABLED(JD_USE_MATH_ACOS) + + #error "TODO: Inline maths with the MCU / FPU." + + #elif ENABLED(JD_USE_LOOKUP_TABLE) + + // Fast acos approximation (max. error +-0.01 rads) + // Based on LUT table and linear interpolation + + /** + * // Generate the JD Lookup Table + * constexpr float c = 1.00751495f; // Correction factor to center error around 0 + * for (int i = 0; i < jd_lut_count - 1; ++i) { + * const float x0 = (sq(i) - 1) / sq(i), + * y0 = acos(x0) * (i == 0 ? 1 : c), + * x1 = i < jd_lut_count - 1 ? 0.5 * x0 + 0.5 : 0.999999f, + * y1 = acos(x1) * (i < jd_lut_count - 1 ? c : 1); + * jd_lut_k[i] = (y0 - y1) / (x0 - x1); + * jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1); + * } + * + * // Compute correction factor (Set c to 1.0f first!) + * float min = INFINITY, max = -min; + * for (float t = 0; t <= 1; t += 0.0003f) { + * const float e = acos(t) / approx(t); + * if (isfinite(e)) { + * if (e < min) min = e; + * if (e > max) max = e; + * } + * } + * fprintf(stderr, "%.9gf, ", (min + max) / 2); + */ + static constexpr int16_t jd_lut_count = 16; + static constexpr uint16_t jd_lut_tll = _BV(jd_lut_count - 1); + static constexpr int16_t jd_lut_tll0 = __builtin_clz(jd_lut_tll) + 1; // i.e., 16 - jd_lut_count + 1 + static constexpr float jd_lut_k[jd_lut_count] PROGMEM = { + -1.03145837f, -1.30760646f, -1.75205851f, -2.41705704f, + -3.37769222f, -4.74888992f, -6.69649887f, -9.45661736f, + -13.3640480f, -18.8928222f, -26.7136841f, -37.7754593f, + -53.4201813f, -75.5458374f, -106.836761f, -218.532821f }; + static constexpr float jd_lut_b[jd_lut_count] PROGMEM = { + 1.57079637f, 1.70887053f, 2.04220939f, 2.62408352f, + 3.52467871f, 4.85302639f, 6.77020454f, 9.50875854f, + 13.4009285f, 18.9188995f, 26.7321243f, 37.7885055f, + 53.4293975f, 75.5523529f, 106.841369f, 218.534011f }; + + const float neg = junction_cos_theta < 0 ? -1 : 1, + t = neg * junction_cos_theta; + + const int16_t idx = (t < 0.00000003f) ? 0 : __builtin_clz(uint16_t((1.0f - t) * jd_lut_tll)) - jd_lut_tll0; + + float junction_theta = t * pgm_read_float(&jd_lut_k[idx]) + pgm_read_float(&jd_lut_b[idx]); + if (neg > 0) junction_theta = RADIANS(180) - junction_theta; // acos(-t) + + #else + + // Fast acos(-t) approximation (max. error +-0.033rad = 1.89°) + // Based on MinMax polynomial published by W. Randolph Franklin, see + // https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html + // acos( t) = pi / 2 - asin(x) + // acos(-t) = pi - acos(t) ... pi / 2 + asin(x) + + const float neg = junction_cos_theta < 0 ? -1 : 1, + t = neg * junction_cos_theta, + asinx = 0.032843707f + + t * (-1.451838349f + + t * ( 29.66153956f + + t * (-131.1123477f + + t * ( 262.8130562f + + t * (-242.7199627f + + t * ( 84.31466202f ) ))))), + junction_theta = RADIANS(90) + neg * asinx; // acos(-t) + + // NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0. - #else - - // Fast acos(-t) approximation (max. error +-0.033rad = 1.89°) - // Based on MinMax polynomial published by W. Randolph Franklin, see - // https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html - // acos( t) = pi / 2 - asin(x) - // acos(-t) = pi - acos(t) ... pi / 2 + asin(x) - - const float neg = junction_cos_theta < 0 ? -1 : 1, - t = neg * junction_cos_theta, - asinx = 0.032843707f - + t * (-1.451838349f - + t * ( 29.66153956f - + t * (-131.1123477f - + t * ( 262.8130562f - + t * (-242.7199627f - + t * ( 84.31466202f ) ))))), - junction_theta = RADIANS(90) + neg * asinx; // acos(-t) - - // NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0. + #endif - #endif + const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta; + NOMORE(vmax_junction_sqr, limit_sqr); + } - const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta; - NOMORE(vmax_junction_sqr, limit_sqr); - } + #endif // JD_HANDLE_SMALL_SEGMENTS } // Get the lowest speed diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index 33b5da9fe9..07bead3caf 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -32,7 +32,7 @@ #include "../MarlinCore.h" -#if HAS_JUNCTION_DEVIATION +#if ENABLED(JD_HANDLE_SMALL_SEGMENTS) // Enable this option for perfect accuracy but maximum // computation. Should be fine on ARM processors. //#define JD_USE_MATH_ACOS