|
@@ -2390,13 +2390,26 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
2390
|
2390
|
sin_theta_d2 = SQRT(0.5f * (1.0f - junction_cos_theta)); // Trig half angle identity. Always positive.
|
2391
|
2391
|
|
2392
|
2392
|
vmax_junction_sqr = (junction_acceleration * junction_deviation_mm * sin_theta_d2) / (1.0f - sin_theta_d2);
|
2393
|
|
- if (block->millimeters < 1) {
|
2394
|
2393
|
|
2395
|
|
- // Fast acos approximation, minus the error bar to be safe
|
2396
|
|
- const float junction_theta = (RADIANS(-40) * sq(junction_cos_theta) - RADIANS(50)) * junction_cos_theta + RADIANS(90) - 0.18f;
|
|
2394
|
+ if (block->millimeters < 1) {
|
|
2395
|
+ // Fast acos approximation (max. error +-0.033 rads)
|
|
2396
|
+ // Based on MinMax polynomial published by W. Randolph Franklin, see
|
|
2397
|
+ // https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
|
|
2398
|
+ // (acos(x) = pi / 2 - asin(x))
|
|
2399
|
+
|
|
2400
|
+ const float neg = junction_cos_theta < 0 ? -1 : 1,
|
|
2401
|
+ t = neg * junction_cos_theta,
|
|
2402
|
+ asinx = 0.032843707f
|
|
2403
|
+ + t * (-1.451838349f
|
|
2404
|
+ + t * ( 29.66153956f
|
|
2405
|
+ + t * (-131.1123477f
|
|
2406
|
+ + t * ( 262.8130562f
|
|
2407
|
+ + t * (-242.7199627f + t * 84.31466202f) )))),
|
|
2408
|
+ junction_theta = RADIANS(90) - neg * asinx;
|
2397
|
2409
|
|
2398
|
2410
|
// If angle is greater than 135 degrees (octagon), find speed for approximate arc
|
2399
|
2411
|
if (junction_theta > RADIANS(135)) {
|
|
2412
|
+ // NOTE: MinMax acos approximation and thereby also junction_theta top out at pi-0.033, which avoids division by 0
|
2400
|
2413
|
const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * junction_acceleration;
|
2401
|
2414
|
NOMORE(vmax_junction_sqr, limit_sqr);
|
2402
|
2415
|
}
|