MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Add JD_HANDLE_SMALL_SEGMENTS option (#18316)

Browse Source
This commit is contained in:
XDA-Bam 2020-06-16 01:54:00 +02:00 committed by GitHub
parent aea60d9450
commit a4b46eaf8f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 76 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Marlin/Configuration.h
View File

@ -815,6 +815,8 @@
*/ */
#if DISABLED(CLASSIC_JERK) #if DISABLED(CLASSIC_JERK)
#define JUNCTION_DEVIATION_MM 0.013 // (mm) Distance from real junction edge #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 #endif
/** /**

142
Marlin/src/module/planner.cpp
View File

@ -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); 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 ENABLED(JD_HANDLE_SMALL_SEGMENTS)
if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) {
#if ENABLED(JD_USE_MATH_ACOS) // For small moves with >135° junction (octagon) find speed for approximate arc
if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) {
#error "TODO: Inline maths with the MCU / FPU." #if ENABLED(JD_USE_MATH_ACOS)
#elif ENABLED(JD_USE_LOOKUP_TABLE) #error "TODO: Inline maths with the MCU / FPU."
// Fast acos approximation (max. error +-0.01 rads) #elif ENABLED(JD_USE_LOOKUP_TABLE)
// Based on LUT table and linear interpolation
/** // Fast acos approximation (max. error +-0.01 rads)
* // Generate the JD Lookup Table // Based on LUT table and linear interpolation
* 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; * // 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 int16_t idx = (t < 0.00000003f) ? 0 : __builtin_clz(uint16_t((1.0f - t) * jd_lut_tll)) - jd_lut_tll0; const float neg = junction_cos_theta < 0 ? -1 : 1,
t = neg * junction_cos_theta;
float junction_theta = t * pgm_read_float(&jd_lut_k[idx]) + pgm_read_float(&jd_lut_b[idx]); const int16_t idx = (t < 0.00000003f) ? 0 : __builtin_clz(uint16_t((1.0f - t) * jd_lut_tll)) - jd_lut_tll0;
if (neg > 0) junction_theta = RADIANS(180) - junction_theta; // acos(-t)
#else 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)
// Fast acos(-t) approximation (max. error +-0.033rad = 1.89°) #else
// 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, // Fast acos(-t) approximation (max. error +-0.033rad = 1.89°)
t = neg * junction_cos_theta, // Based on MinMax polynomial published by W. Randolph Franklin, see
asinx = 0.032843707f // https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
+ t * (-1.451838349f // acos( t) = pi / 2 - asin(x)
+ t * ( 29.66153956f // acos(-t) = pi - acos(t) ... pi / 2 + asin(x)
+ 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. 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)
#endif // NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0.
const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta; #endif
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 // Get the lowest speed

2
Marlin/src/module/planner.h
View File

@ -32,7 +32,7 @@
#include "../MarlinCore.h" #include "../MarlinCore.h"
#if HAS_JUNCTION_DEVIATION #if ENABLED(JD_HANDLE_SMALL_SEGMENTS)
// Enable this option for perfect accuracy but maximum // Enable this option for perfect accuracy but maximum
// computation. Should be fine on ARM processors. // computation. Should be fine on ARM processors.
//#define JD_USE_MATH_ACOS //#define JD_USE_MATH_ACOS