SunRed/Marlin-Artillery-M600
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity

More accurate Junction Deviation fast-acos (#17575)

Browse source
This commit is contained in:
XDA-Bam 2020-04-17 04:51:05 +02:00 committed by GitHub
parent 927e29b93e
commit bbe2cb75ad
Signed by: GitHub
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 16 additions and 3 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

19
Marlin/src/module/planner.cpp
View file

@ -2390,13 +2390,26 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive. sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive.
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) {
// Fast acos approximation, minus the error bar to be safe if (block->millimeters < 1) {
const float junction_theta = (RADIANS(-40) * sq(junction_cos_theta) - RADIANS(50)) * junction_cos_theta + RADIANS(90) - 0.18f; // Fast acos approximation (max. error +-0.033 rads)
// Based on MinMax polynomial published by W. Randolph Franklin, see
// https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
// (acos(x) = 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;
// If angle is greater than 135 degrees (octagon), find speed for approximate arc // If angle is greater than 135 degrees (octagon), find speed for approximate arc
if (junction_theta > RADIANS(135)) { if (junction_theta > RADIANS(135)) {
// NOTE: MinMax acos approximation and thereby also junction_theta top out at pi-0.033, which avoids division by 0
const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * junction_acceleration; const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * junction_acceleration;
NOMORE(vmax_junction_sqr, limit_sqr); NOMORE(vmax_junction_sqr, limit_sqr);
} }