My Marlin configs for Fabrikator Mini and CTC i3 Pro B
Ви не можете вибрати більше 25 тем Теми мають розпочинатися з літери або цифри, можуть містити дефіси (-) і не повинні перевищувати 35 символів.

planner_bezier.cpp 8.0KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203
  1. /**
  2. * Marlin 3D Printer Firmware
  3. * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
  4. *
  5. * Based on Sprinter and grbl.
  6. * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
  7. *
  8. * This program is free software: you can redistribute it and/or modify
  9. * it under the terms of the GNU General Public License as published by
  10. * the Free Software Foundation, either version 3 of the License, or
  11. * (at your option) any later version.
  12. *
  13. * This program is distributed in the hope that it will be useful,
  14. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  16. * GNU General Public License for more details.
  17. *
  18. * You should have received a copy of the GNU General Public License
  19. * along with this program. If not, see <http://www.gnu.org/licenses/>.
  20. *
  21. */
  22. /**
  23. * planner_bezier.cpp
  24. *
  25. * Compute and buffer movement commands for bezier curves
  26. *
  27. */
  28. #include "Marlin.h"
  29. #if ENABLED(BEZIER_CURVE_SUPPORT)
  30. #include "planner.h"
  31. #include "language.h"
  32. #include "temperature.h"
  33. // See the meaning in the documentation of cubic_b_spline().
  34. #define MIN_STEP 0.002
  35. #define MAX_STEP 0.1
  36. #define SIGMA 0.1
  37. /* Compute the linear interpolation between to real numbers.
  38. */
  39. inline static float interp(float a, float b, float t) { return (1.0 - t) * a + t * b; }
  40. /**
  41. * Compute a Bézier curve using the De Casteljau's algorithm (see
  42. * https://en.wikipedia.org/wiki/De_Casteljau%27s_algorithm), which is
  43. * easy to code and has good numerical stability (very important,
  44. * since Arudino works with limited precision real numbers).
  45. */
  46. inline static float eval_bezier(float a, float b, float c, float d, float t) {
  47. float iab = interp(a, b, t);
  48. float ibc = interp(b, c, t);
  49. float icd = interp(c, d, t);
  50. float iabc = interp(iab, ibc, t);
  51. float ibcd = interp(ibc, icd, t);
  52. float iabcd = interp(iabc, ibcd, t);
  53. return iabcd;
  54. }
  55. /**
  56. * We approximate Euclidean distance with the sum of the coordinates
  57. * offset (so-called "norm 1"), which is quicker to compute.
  58. */
  59. inline static float dist1(float x1, float y1, float x2, float y2) { return fabs(x1 - x2) + fabs(y1 - y2); }
  60. /**
  61. * The algorithm for computing the step is loosely based on the one in Kig
  62. * (See https://sources.debian.net/src/kig/4:15.08.3-1/misc/kigpainter.cpp/#L759)
  63. * However, we do not use the stack.
  64. *
  65. * The algorithm goes as it follows: the parameters t runs from 0.0 to
  66. * 1.0 describing the curve, which is evaluated by eval_bezier(). At
  67. * each iteration we have to choose a step, i.e., the increment of the
  68. * t variable. By default the step of the previous iteration is taken,
  69. * and then it is enlarged or reduced depending on how straight the
  70. * curve locally is. The step is always clamped between MIN_STEP/2 and
  71. * 2*MAX_STEP. MAX_STEP is taken at the first iteration.
  72. *
  73. * For some t, the step value is considered acceptable if the curve in
  74. * the interval [t, t+step] is sufficiently straight, i.e.,
  75. * sufficiently close to linear interpolation. In practice the
  76. * following test is performed: the distance between eval_bezier(...,
  77. * t+step/2) is evaluated and compared with 0.5*(eval_bezier(...,
  78. * t)+eval_bezier(..., t+step)). If it is smaller than SIGMA, then the
  79. * step value is considered acceptable, otherwise it is not. The code
  80. * seeks to find the larger step value which is considered acceptable.
  81. *
  82. * At every iteration the recorded step value is considered and then
  83. * iteratively halved until it becomes acceptable. If it was already
  84. * acceptable in the beginning (i.e., no halving were done), then
  85. * maybe it was necessary to enlarge it; then it is iteratively
  86. * doubled while it remains acceptable. The last acceptable value
  87. * found is taken, provided that it is between MIN_STEP and MAX_STEP
  88. * and does not bring t over 1.0.
  89. *
  90. * Caveat: this algorithm is not perfect, since it can happen that a
  91. * step is considered acceptable even when the curve is not linear at
  92. * all in the interval [t, t+step] (but its mid point coincides "by
  93. * chance" with the midpoint according to the parametrization). This
  94. * kind of glitches can be eliminated with proper first derivative
  95. * estimates; however, given the improbability of such configurations,
  96. * the mitigation offered by MIN_STEP and the small computational
  97. * power available on Arduino, I think it is not wise to implement it.
  98. */
  99. void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS], const float offset[4], float fr_mm_s, uint8_t extruder) {
  100. // Absolute first and second control points are recovered.
  101. float first0 = position[X_AXIS] + offset[0];
  102. float first1 = position[Y_AXIS] + offset[1];
  103. float second0 = target[X_AXIS] + offset[2];
  104. float second1 = target[Y_AXIS] + offset[3];
  105. float t = 0.0;
  106. float bez_target[4];
  107. bez_target[X_AXIS] = position[X_AXIS];
  108. bez_target[Y_AXIS] = position[Y_AXIS];
  109. float step = MAX_STEP;
  110. millis_t next_idle_ms = millis() + 200UL;
  111. while (t < 1.0) {
  112. thermalManager.manage_heater();
  113. millis_t now = millis();
  114. if (ELAPSED(now, next_idle_ms)) {
  115. next_idle_ms = now + 200UL;
  116. idle();
  117. }
  118. // First try to reduce the step in order to make it sufficiently
  119. // close to a linear interpolation.
  120. bool did_reduce = false;
  121. float new_t = t + step;
  122. NOMORE(new_t, 1.0);
  123. float new_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], new_t);
  124. float new_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], new_t);
  125. for (;;) {
  126. if (new_t - t < (MIN_STEP)) break;
  127. float candidate_t = 0.5 * (t + new_t);
  128. float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t);
  129. float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t);
  130. float interp_pos0 = 0.5 * (bez_target[X_AXIS] + new_pos0);
  131. float interp_pos1 = 0.5 * (bez_target[Y_AXIS] + new_pos1);
  132. if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break;
  133. new_t = candidate_t;
  134. new_pos0 = candidate_pos0;
  135. new_pos1 = candidate_pos1;
  136. did_reduce = true;
  137. }
  138. // If we did not reduce the step, maybe we should enlarge it.
  139. if (!did_reduce) for (;;) {
  140. if (new_t - t > MAX_STEP) break;
  141. float candidate_t = t + 2.0 * (new_t - t);
  142. if (candidate_t >= 1.0) break;
  143. float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t);
  144. float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t);
  145. float interp_pos0 = 0.5 * (bez_target[X_AXIS] + candidate_pos0);
  146. float interp_pos1 = 0.5 * (bez_target[Y_AXIS] + candidate_pos1);
  147. if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break;
  148. new_t = candidate_t;
  149. new_pos0 = candidate_pos0;
  150. new_pos1 = candidate_pos1;
  151. }
  152. // Check some postcondition; they are disabled in the actual
  153. // Marlin build, but if you test the same code on a computer you
  154. // may want to check they are respect.
  155. /*
  156. assert(new_t <= 1.0);
  157. if (new_t < 1.0) {
  158. assert(new_t - t >= (MIN_STEP) / 2.0);
  159. assert(new_t - t <= (MAX_STEP) * 2.0);
  160. }
  161. */
  162. step = new_t - t;
  163. t = new_t;
  164. // Compute and send new position
  165. bez_target[X_AXIS] = new_pos0;
  166. bez_target[Y_AXIS] = new_pos1;
  167. // FIXME. The following two are wrong, since the parameter t is
  168. // not linear in the distance.
  169. bez_target[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t);
  170. bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
  171. clamp_to_software_endstops(bez_target);
  172. #if IS_KINEMATIC
  173. inverse_kinematics(bez_target);
  174. #if ENABLED(DELTA) && ENABLED(AUTO_BED_LEVELING_FEATURE)
  175. adjust_delta(bez_target);
  176. #endif
  177. planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], bez_target[E_AXIS], fr_mm_s, extruder);
  178. #else
  179. planner.buffer_line(bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS], bez_target[E_AXIS], fr_mm_s, extruder);
  180. #endif
  181. }
  182. }
  183. #endif // BEZIER_CURVE_SUPPORT