|
@@ -2352,89 +2352,93 @@ bool Planner::_populate_block(block_t * const block, bool split_move,
|
2352
|
2352
|
|
2353
|
2353
|
vmax_junction_sqr = junction_acceleration * junction_deviation_mm * sin_theta_d2 / (1.0f - sin_theta_d2);
|
2354
|
2354
|
|
2355
|
|
- // For small moves with >135° junction (octagon) find speed for approximate arc
|
2356
|
|
- if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) {
|
2357
|
|
-
|
2358
|
|
- #if ENABLED(JD_USE_MATH_ACOS)
|
2359
|
|
-
|
2360
|
|
- #error "TODO: Inline maths with the MCU / FPU."
|
2361
|
|
-
|
2362
|
|
- #elif ENABLED(JD_USE_LOOKUP_TABLE)
|
2363
|
|
-
|
2364
|
|
- // Fast acos approximation (max. error +-0.01 rads)
|
2365
|
|
- // Based on LUT table and linear interpolation
|
2366
|
|
-
|
2367
|
|
- /**
|
2368
|
|
- * // Generate the JD Lookup Table
|
2369
|
|
- * constexpr float c = 1.00751495f; // Correction factor to center error around 0
|
2370
|
|
- * for (int i = 0; i < jd_lut_count - 1; ++i) {
|
2371
|
|
- * const float x0 = (sq(i) - 1) / sq(i),
|
2372
|
|
- * y0 = acos(x0) * (i == 0 ? 1 : c),
|
2373
|
|
- * x1 = i < jd_lut_count - 1 ? 0.5 * x0 + 0.5 : 0.999999f,
|
2374
|
|
- * y1 = acos(x1) * (i < jd_lut_count - 1 ? c : 1);
|
2375
|
|
- * jd_lut_k[i] = (y0 - y1) / (x0 - x1);
|
2376
|
|
- * jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1);
|
2377
|
|
- * }
|
2378
|
|
- *
|
2379
|
|
- * // Compute correction factor (Set c to 1.0f first!)
|
2380
|
|
- * float min = INFINITY, max = -min;
|
2381
|
|
- * for (float t = 0; t <= 1; t += 0.0003f) {
|
2382
|
|
- * const float e = acos(t) / approx(t);
|
2383
|
|
- * if (isfinite(e)) {
|
2384
|
|
- * if (e < min) min = e;
|
2385
|
|
- * if (e > max) max = e;
|
2386
|
|
- * }
|
2387
|
|
- * }
|
2388
|
|
- * fprintf(stderr, "%.9gf, ", (min + max) / 2);
|
2389
|
|
- */
|
2390
|
|
- static constexpr int16_t jd_lut_count = 16;
|
2391
|
|
- static constexpr uint16_t jd_lut_tll = _BV(jd_lut_count - 1);
|
2392
|
|
- static constexpr int16_t jd_lut_tll0 = __builtin_clz(jd_lut_tll) + 1; // i.e., 16 - jd_lut_count + 1
|
2393
|
|
- static constexpr float jd_lut_k[jd_lut_count] PROGMEM = {
|
2394
|
|
- -1.03145837f, -1.30760646f, -1.75205851f, -2.41705704f,
|
2395
|
|
- -3.37769222f, -4.74888992f, -6.69649887f, -9.45661736f,
|
2396
|
|
- -13.3640480f, -18.8928222f, -26.7136841f, -37.7754593f,
|
2397
|
|
- -53.4201813f, -75.5458374f, -106.836761f, -218.532821f };
|
2398
|
|
- static constexpr float jd_lut_b[jd_lut_count] PROGMEM = {
|
2399
|
|
- 1.57079637f, 1.70887053f, 2.04220939f, 2.62408352f,
|
2400
|
|
- 3.52467871f, 4.85302639f, 6.77020454f, 9.50875854f,
|
2401
|
|
- 13.4009285f, 18.9188995f, 26.7321243f, 37.7885055f,
|
2402
|
|
- 53.4293975f, 75.5523529f, 106.841369f, 218.534011f };
|
2403
|
|
-
|
2404
|
|
- const float neg = junction_cos_theta < 0 ? -1 : 1,
|
2405
|
|
- t = neg * junction_cos_theta;
|
2406
|
|
-
|
2407
|
|
- const int16_t idx = (t < 0.00000003f) ? 0 : __builtin_clz(uint16_t((1.0f - t) * jd_lut_tll)) - jd_lut_tll0;
|
2408
|
|
-
|
2409
|
|
- float junction_theta = t * pgm_read_float(&jd_lut_k[idx]) + pgm_read_float(&jd_lut_b[idx]);
|
2410
|
|
- if (neg > 0) junction_theta = RADIANS(180) - junction_theta; // acos(-t)
|
|
2355
|
+ #if ENABLED(JD_HANDLE_SMALL_SEGMENTS)
|
|
2356
|
+
|
|
2357
|
+ // For small moves with >135° junction (octagon) find speed for approximate arc
|
|
2358
|
+ if (block->millimeters < 1 && junction_cos_theta < -0.7071067812f) {
|
|
2359
|
+
|
|
2360
|
+ #if ENABLED(JD_USE_MATH_ACOS)
|
|
2361
|
+
|
|
2362
|
+ #error "TODO: Inline maths with the MCU / FPU."
|
|
2363
|
+
|
|
2364
|
+ #elif ENABLED(JD_USE_LOOKUP_TABLE)
|
|
2365
|
+
|
|
2366
|
+ // Fast acos approximation (max. error +-0.01 rads)
|
|
2367
|
+ // Based on LUT table and linear interpolation
|
|
2368
|
+
|
|
2369
|
+ /**
|
|
2370
|
+ * // Generate the JD Lookup Table
|
|
2371
|
+ * constexpr float c = 1.00751495f; // Correction factor to center error around 0
|
|
2372
|
+ * for (int i = 0; i < jd_lut_count - 1; ++i) {
|
|
2373
|
+ * const float x0 = (sq(i) - 1) / sq(i),
|
|
2374
|
+ * y0 = acos(x0) * (i == 0 ? 1 : c),
|
|
2375
|
+ * x1 = i < jd_lut_count - 1 ? 0.5 * x0 + 0.5 : 0.999999f,
|
|
2376
|
+ * y1 = acos(x1) * (i < jd_lut_count - 1 ? c : 1);
|
|
2377
|
+ * jd_lut_k[i] = (y0 - y1) / (x0 - x1);
|
|
2378
|
+ * jd_lut_b[i] = (y1 * x0 - y0 * x1) / (x0 - x1);
|
|
2379
|
+ * }
|
|
2380
|
+ *
|
|
2381
|
+ * // Compute correction factor (Set c to 1.0f first!)
|
|
2382
|
+ * float min = INFINITY, max = -min;
|
|
2383
|
+ * for (float t = 0; t <= 1; t += 0.0003f) {
|
|
2384
|
+ * const float e = acos(t) / approx(t);
|
|
2385
|
+ * if (isfinite(e)) {
|
|
2386
|
+ * if (e < min) min = e;
|
|
2387
|
+ * if (e > max) max = e;
|
|
2388
|
+ * }
|
|
2389
|
+ * }
|
|
2390
|
+ * fprintf(stderr, "%.9gf, ", (min + max) / 2);
|
|
2391
|
+ */
|
|
2392
|
+ static constexpr int16_t jd_lut_count = 16;
|
|
2393
|
+ static constexpr uint16_t jd_lut_tll = _BV(jd_lut_count - 1);
|
|
2394
|
+ static constexpr int16_t jd_lut_tll0 = __builtin_clz(jd_lut_tll) + 1; // i.e., 16 - jd_lut_count + 1
|
|
2395
|
+ static constexpr float jd_lut_k[jd_lut_count] PROGMEM = {
|
|
2396
|
+ -1.03145837f, -1.30760646f, -1.75205851f, -2.41705704f,
|
|
2397
|
+ -3.37769222f, -4.74888992f, -6.69649887f, -9.45661736f,
|
|
2398
|
+ -13.3640480f, -18.8928222f, -26.7136841f, -37.7754593f,
|
|
2399
|
+ -53.4201813f, -75.5458374f, -106.836761f, -218.532821f };
|
|
2400
|
+ static constexpr float jd_lut_b[jd_lut_count] PROGMEM = {
|
|
2401
|
+ 1.57079637f, 1.70887053f, 2.04220939f, 2.62408352f,
|
|
2402
|
+ 3.52467871f, 4.85302639f, 6.77020454f, 9.50875854f,
|
|
2403
|
+ 13.4009285f, 18.9188995f, 26.7321243f, 37.7885055f,
|
|
2404
|
+ 53.4293975f, 75.5523529f, 106.841369f, 218.534011f };
|
|
2405
|
+
|
|
2406
|
+ const float neg = junction_cos_theta < 0 ? -1 : 1,
|
|
2407
|
+ t = neg * junction_cos_theta;
|
|
2408
|
+
|
|
2409
|
+ const int16_t idx = (t < 0.00000003f) ? 0 : __builtin_clz(uint16_t((1.0f - t) * jd_lut_tll)) - jd_lut_tll0;
|
|
2410
|
+
|
|
2411
|
+ float junction_theta = t * pgm_read_float(&jd_lut_k[idx]) + pgm_read_float(&jd_lut_b[idx]);
|
|
2412
|
+ if (neg > 0) junction_theta = RADIANS(180) - junction_theta; // acos(-t)
|
|
2413
|
+
|
|
2414
|
+ #else
|
|
2415
|
+
|
|
2416
|
+ // Fast acos(-t) approximation (max. error +-0.033rad = 1.89°)
|
|
2417
|
+ // Based on MinMax polynomial published by W. Randolph Franklin, see
|
|
2418
|
+ // https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
|
|
2419
|
+ // acos( t) = pi / 2 - asin(x)
|
|
2420
|
+ // acos(-t) = pi - acos(t) ... pi / 2 + asin(x)
|
|
2421
|
+
|
|
2422
|
+ const float neg = junction_cos_theta < 0 ? -1 : 1,
|
|
2423
|
+ t = neg * junction_cos_theta,
|
|
2424
|
+ asinx = 0.032843707f
|
|
2425
|
+ + t * (-1.451838349f
|
|
2426
|
+ + t * ( 29.66153956f
|
|
2427
|
+ + t * (-131.1123477f
|
|
2428
|
+ + t * ( 262.8130562f
|
|
2429
|
+ + t * (-242.7199627f
|
|
2430
|
+ + t * ( 84.31466202f ) ))))),
|
|
2431
|
+ junction_theta = RADIANS(90) + neg * asinx; // acos(-t)
|
|
2432
|
+
|
|
2433
|
+ // NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0.
|
2411
|
2434
|
|
2412
|
|
- #else
|
2413
|
|
-
|
2414
|
|
- // Fast acos(-t) approximation (max. error +-0.033rad = 1.89°)
|
2415
|
|
- // Based on MinMax polynomial published by W. Randolph Franklin, see
|
2416
|
|
- // https://wrf.ecse.rpi.edu/Research/Short_Notes/arcsin/onlyelem.html
|
2417
|
|
- // acos( t) = pi / 2 - asin(x)
|
2418
|
|
- // acos(-t) = pi - acos(t) ... pi / 2 + asin(x)
|
2419
|
|
-
|
2420
|
|
- const float neg = junction_cos_theta < 0 ? -1 : 1,
|
2421
|
|
- t = neg * junction_cos_theta,
|
2422
|
|
- asinx = 0.032843707f
|
2423
|
|
- + t * (-1.451838349f
|
2424
|
|
- + t * ( 29.66153956f
|
2425
|
|
- + t * (-131.1123477f
|
2426
|
|
- + t * ( 262.8130562f
|
2427
|
|
- + t * (-242.7199627f
|
2428
|
|
- + t * ( 84.31466202f ) ))))),
|
2429
|
|
- junction_theta = RADIANS(90) + neg * asinx; // acos(-t)
|
2430
|
|
-
|
2431
|
|
- // NOTE: junction_theta bottoms out at 0.033 which avoids divide by 0.
|
|
2435
|
+ #endif
|
2432
|
2436
|
|
2433
|
|
- #endif
|
|
2437
|
+ const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta;
|
|
2438
|
+ NOMORE(vmax_junction_sqr, limit_sqr);
|
|
2439
|
+ }
|
2434
|
2440
|
|
2435
|
|
- const float limit_sqr = (block->millimeters * junction_acceleration) / junction_theta;
|
2436
|
|
- NOMORE(vmax_junction_sqr, limit_sqr);
|
2437
|
|
- }
|
|
2441
|
+ #endif // JD_HANDLE_SMALL_SEGMENTS
|
2438
|
2442
|
}
|
2439
|
2443
|
|
2440
|
2444
|
// Get the lowest speed
|