andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Fix and combine JD condition

vanilla_fb_2.0.x
Scott Lahteine 4 years ago
parent
d571a4d389
commit
c55475e8e7
1 changed files with 79 additions and 79 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 158
      Marlin/src/module/planner.cpp

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

@ -2306,87 +2306,87 @@ 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);
if (block->millimeters < 1) {
const float neg = junction_cos_theta < 0 ? -1 : 1,
t = neg * junction_cos_theta;
// If angle is greater than 135 degrees (octagon), find speed for approximate arc
if (t > 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.00751317f; // 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 ? c : 1),
* x1 = 0.5 * x0 + 0.5,
* y1 = acos(x1) * c;
* jd_lut_k[i] = (y0 - y1) / (x0 - x1);
* jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1);
* }
* jd_lut_k[jd_lut_count - 1] = jd_lut_b[jd_lut_count - 1] = 0;
*
* // 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 = 15;
static constexpr uint16_t jd_lut_tll = 1 << jd_lut_count;
static constexpr int16_t jd_lut_tll0 = __builtin_clz(jd_lut_tll) + 1; // i.e., 16 - jd_lut_count
static constexpr float jd_lut_k[jd_lut_count] PROGMEM = {
-1.03146219f, -1.30760407f, -1.75205469f, -2.41705418f, -3.37768555f,
-4.74888229f, -6.69648552f, -9.45659828f, -13.3640289f, -18.8927879f,
-26.7136307f, -37.7754059f, -53.4200745f, -75.5457306f, 0.0f };
static constexpr float jd_lut_b[jd_lut_count] PROGMEM = {
1.57079637f, 1.70886743f, 2.04220533f, 2.62408018f, 3.52467203f,
4.85301876f, 6.77019119f, 9.50873947f, 13.4009094f, 18.9188652f,
26.7320709f, 37.7884521f, 53.4292908f, 75.5522461f, 0.0f };
const int16_t idx = (t == 0.0f) ? 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;
#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 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.
// 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.00751317f; // 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 ? c : 1),
* x1 = 0.5 * x0 + 0.5,
* y1 = acos(x1) * c;
* jd_lut_k[i] = (y0 - y1) / (x0 - x1);
* jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1);
* }
* jd_lut_k[jd_lut_count - 1] = jd_lut_b[jd_lut_count - 1] = 0;
*
* // 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 = 15;
static constexpr uint16_t jd_lut_tll = 1 << jd_lut_count;
static constexpr int16_t jd_lut_tll0 = __builtin_clz(jd_lut_tll) + 1; // i.e., 16 - jd_lut_count
static constexpr float jd_lut_k[jd_lut_count] PROGMEM = {
-1.03146219f, -1.30760407f, -1.75205469f, -2.41705418f, -3.37768555f,
-4.74888229f, -6.69648552f, -9.45659828f, -13.3640289f, -18.8927879f,
-26.7136307f, -37.7754059f, -53.4200745f, -75.5457306f, 0.0f };
static constexpr float jd_lut_b[jd_lut_count] PROGMEM = {
1.57079637f, 1.70886743f, 2.04220533f, 2.62408018f, 3.52467203f,
4.85301876f, 6.77019119f, 9.50873947f, 13.4009094f, 18.9188652f,
26.7320709f, 37.7884521f, 53.4292908f, 75.5522461f, 0.0f };
const float neg = junction_cos_theta < 0 ? -1 : 1,
t = neg * junction_cos_theta;
const int16_t idx = (t == 0.0f) ? 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)
#endif
#else
const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta;
NOMORE(vmax_junction_sqr, limit_sqr);
}
// 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
const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta;
NOMORE(vmax_junction_sqr, limit_sqr);
}
}

Write Preview
Loading…
Cancel
Save