bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Fix and combine JD condition

Browse Source
This commit is contained in:
Scott Lahteine 2020-05-04 19:01:54 -05:00
parent d571a4d389
commit c55475e8e7
1 changed files with 67 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2306,12 +2306,8 @@ 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);
if (block->millimeters < 1) { // For small moves with >135° junction (octagon) find speed for approximate arc
const float neg = junction_cos_theta < 0 ? -1 : 1, if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) {
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) #if ENABLED(JD_USE_MATH_ACOS)
@ -2358,10 +2354,13 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
4.85301876f, 6.77019119f, 9.50873947f, 13.4009094f, 18.9188652f, 4.85301876f, 6.77019119f, 9.50873947f, 13.4009094f, 18.9188652f,
26.7320709f, 37.7884521f, 53.4292908f, 75.5522461f, 0.0f }; 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; 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]); 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; if (neg > 0) junction_theta = RADIANS(180) - junction_theta; // acos(-t)
#else #else
@ -2371,7 +2370,9 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
// acos( t) = pi / 2 - asin(x) // acos( t) = pi / 2 - asin(x)
// acos(-t) = pi - acos(t) ... pi / 2 + asin(x) // acos(-t) = pi - acos(t) ... pi / 2 + asin(x)
const float asinx = 0.032843707f const float neg = junction_cos_theta < 0 ? -1 : 1,
t = neg * junction_cos_theta,
asinx = 0.032843707f
+ t * (-1.451838349f + t * (-1.451838349f
+ t * ( 29.66153956f + t * ( 29.66153956f
+ t * (-131.1123477f + t * (-131.1123477f
@ -2388,7 +2389,6 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
NOMORE(vmax_junction_sqr, limit_sqr); NOMORE(vmax_junction_sqr, limit_sqr);
} }
} }
}
// Get the lowest speed // Get the lowest speed
vmax_junction_sqr = _MIN(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr); vmax_junction_sqr = _MIN(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr);