My Marlin configs for Fabrikator Mini and CTC i3 Pro B
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

delta.cpp 9.5KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265
  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. * delta.cpp
  24. */
  25. #include "../inc/MarlinConfig.h"
  26. #if ENABLED(DELTA)
  27. #include "delta.h"
  28. #include "motion.h"
  29. // For homing:
  30. #include "stepper.h"
  31. #include "endstops.h"
  32. #include "../lcd/ultralcd.h"
  33. #include "../Marlin.h"
  34. // Initialized by settings.load()
  35. float delta_endstop_adj[ABC] = { 0 },
  36. delta_radius,
  37. delta_diagonal_rod,
  38. delta_segments_per_second,
  39. delta_calibration_radius,
  40. delta_tower_angle_trim[ABC];
  41. float delta_tower[ABC][2],
  42. delta_diagonal_rod_2_tower[ABC],
  43. delta_clip_start_height = Z_MAX_POS;
  44. float delta_safe_distance_from_top();
  45. /**
  46. * Recalculate factors used for delta kinematics whenever
  47. * settings have been changed (e.g., by M665).
  48. */
  49. void recalc_delta_settings(const float radius, const float diagonal_rod, const float tower_angle_trim[ABC]) {
  50. const float trt[ABC] = DELTA_RADIUS_TRIM_TOWER,
  51. drt[ABC] = DELTA_DIAGONAL_ROD_TRIM_TOWER;
  52. delta_tower[A_AXIS][X_AXIS] = cos(RADIANS(210 + tower_angle_trim[A_AXIS])) * (radius + trt[A_AXIS]); // front left tower
  53. delta_tower[A_AXIS][Y_AXIS] = sin(RADIANS(210 + tower_angle_trim[A_AXIS])) * (radius + trt[A_AXIS]);
  54. delta_tower[B_AXIS][X_AXIS] = cos(RADIANS(330 + tower_angle_trim[B_AXIS])) * (radius + trt[B_AXIS]); // front right tower
  55. delta_tower[B_AXIS][Y_AXIS] = sin(RADIANS(330 + tower_angle_trim[B_AXIS])) * (radius + trt[B_AXIS]);
  56. delta_tower[C_AXIS][X_AXIS] = cos(RADIANS( 90 + tower_angle_trim[C_AXIS])) * (radius + trt[C_AXIS]); // back middle tower
  57. delta_tower[C_AXIS][Y_AXIS] = sin(RADIANS( 90 + tower_angle_trim[C_AXIS])) * (radius + trt[C_AXIS]);
  58. delta_diagonal_rod_2_tower[A_AXIS] = sq(diagonal_rod + drt[A_AXIS]);
  59. delta_diagonal_rod_2_tower[B_AXIS] = sq(diagonal_rod + drt[B_AXIS]);
  60. delta_diagonal_rod_2_tower[C_AXIS] = sq(diagonal_rod + drt[C_AXIS]);
  61. }
  62. /**
  63. * Delta Inverse Kinematics
  64. *
  65. * Calculate the tower positions for a given machine
  66. * position, storing the result in the delta[] array.
  67. *
  68. * This is an expensive calculation, requiring 3 square
  69. * roots per segmented linear move, and strains the limits
  70. * of a Mega2560 with a Graphical Display.
  71. *
  72. * Suggested optimizations include:
  73. *
  74. * - Disable the home_offset (M206) and/or position_shift (G92)
  75. * features to remove up to 12 float additions.
  76. *
  77. * - Use a fast-inverse-sqrt function and add the reciprocal.
  78. * (see above)
  79. */
  80. #if ENABLED(DELTA_FAST_SQRT) && defined(__AVR__)
  81. /**
  82. * Fast inverse sqrt from Quake III Arena
  83. * See: https://en.wikipedia.org/wiki/Fast_inverse_square_root
  84. */
  85. float Q_rsqrt(float number) {
  86. long i;
  87. float x2, y;
  88. const float threehalfs = 1.5f;
  89. x2 = number * 0.5f;
  90. y = number;
  91. i = * ( long * ) &y; // evil floating point bit level hacking
  92. i = 0x5F3759DF - ( i >> 1 ); // what the f***?
  93. y = * ( float * ) &i;
  94. y = y * ( threehalfs - ( x2 * y * y ) ); // 1st iteration
  95. // y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed
  96. return y;
  97. }
  98. #endif
  99. #define DELTA_DEBUG() do { \
  100. SERIAL_ECHOPAIR("cartesian X:", raw[X_AXIS]); \
  101. SERIAL_ECHOPAIR(" Y:", raw[Y_AXIS]); \
  102. SERIAL_ECHOLNPAIR(" Z:", raw[Z_AXIS]); \
  103. SERIAL_ECHOPAIR("delta A:", delta[A_AXIS]); \
  104. SERIAL_ECHOPAIR(" B:", delta[B_AXIS]); \
  105. SERIAL_ECHOLNPAIR(" C:", delta[C_AXIS]); \
  106. }while(0)
  107. void inverse_kinematics(const float raw[XYZ]) {
  108. DELTA_RAW_IK();
  109. // DELTA_DEBUG();
  110. }
  111. /**
  112. * Calculate the highest Z position where the
  113. * effector has the full range of XY motion.
  114. */
  115. float delta_safe_distance_from_top() {
  116. float cartesian[XYZ] = { 0, 0, 0 };
  117. inverse_kinematics(cartesian);
  118. float distance = delta[A_AXIS];
  119. cartesian[Y_AXIS] = DELTA_PRINTABLE_RADIUS;
  120. inverse_kinematics(cartesian);
  121. return FABS(distance - delta[A_AXIS]);
  122. }
  123. /**
  124. * Delta Forward Kinematics
  125. *
  126. * See the Wikipedia article "Trilateration"
  127. * https://en.wikipedia.org/wiki/Trilateration
  128. *
  129. * Establish a new coordinate system in the plane of the
  130. * three carriage points. This system has its origin at
  131. * tower1, with tower2 on the X axis. Tower3 is in the X-Y
  132. * plane with a Z component of zero.
  133. * We will define unit vectors in this coordinate system
  134. * in our original coordinate system. Then when we calculate
  135. * the Xnew, Ynew and Znew values, we can translate back into
  136. * the original system by moving along those unit vectors
  137. * by the corresponding values.
  138. *
  139. * Variable names matched to Marlin, c-version, and avoid the
  140. * use of any vector library.
  141. *
  142. * by Andreas Hardtung 2016-06-07
  143. * based on a Java function from "Delta Robot Kinematics V3"
  144. * by Steve Graves
  145. *
  146. * The result is stored in the cartes[] array.
  147. */
  148. void forward_kinematics_DELTA(float z1, float z2, float z3) {
  149. // Create a vector in old coordinates along x axis of new coordinate
  150. float p12[3] = { delta_tower[B_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[B_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z2 - z1 };
  151. // Get the Magnitude of vector.
  152. float d = SQRT( sq(p12[0]) + sq(p12[1]) + sq(p12[2]) );
  153. // Create unit vector by dividing by magnitude.
  154. float ex[3] = { p12[0] / d, p12[1] / d, p12[2] / d };
  155. // Get the vector from the origin of the new system to the third point.
  156. float p13[3] = { delta_tower[C_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[C_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z3 - z1 };
  157. // Use the dot product to find the component of this vector on the X axis.
  158. float i = ex[0] * p13[0] + ex[1] * p13[1] + ex[2] * p13[2];
  159. // Create a vector along the x axis that represents the x component of p13.
  160. float iex[3] = { ex[0] * i, ex[1] * i, ex[2] * i };
  161. // Subtract the X component from the original vector leaving only Y. We use the
  162. // variable that will be the unit vector after we scale it.
  163. float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2] };
  164. // The magnitude of Y component
  165. float j = SQRT( sq(ey[0]) + sq(ey[1]) + sq(ey[2]) );
  166. // Convert to a unit vector
  167. ey[0] /= j; ey[1] /= j; ey[2] /= j;
  168. // The cross product of the unit x and y is the unit z
  169. // float[] ez = vectorCrossProd(ex, ey);
  170. float ez[3] = {
  171. ex[1] * ey[2] - ex[2] * ey[1],
  172. ex[2] * ey[0] - ex[0] * ey[2],
  173. ex[0] * ey[1] - ex[1] * ey[0]
  174. };
  175. // We now have the d, i and j values defined in Wikipedia.
  176. // Plug them into the equations defined in Wikipedia for Xnew, Ynew and Znew
  177. float Xnew = (delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[B_AXIS] + sq(d)) / (d * 2),
  178. Ynew = ((delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[C_AXIS] + HYPOT2(i, j)) / 2 - i * Xnew) / j,
  179. Znew = SQRT(delta_diagonal_rod_2_tower[A_AXIS] - HYPOT2(Xnew, Ynew));
  180. // Start from the origin of the old coordinates and add vectors in the
  181. // old coords that represent the Xnew, Ynew and Znew to find the point
  182. // in the old system.
  183. cartes[X_AXIS] = delta_tower[A_AXIS][X_AXIS] + ex[0] * Xnew + ey[0] * Ynew - ez[0] * Znew;
  184. cartes[Y_AXIS] = delta_tower[A_AXIS][Y_AXIS] + ex[1] * Xnew + ey[1] * Ynew - ez[1] * Znew;
  185. cartes[Z_AXIS] = z1 + ex[2] * Xnew + ey[2] * Ynew - ez[2] * Znew;
  186. }
  187. /**
  188. * A delta can only safely home all axes at the same time
  189. * This is like quick_home_xy() but for 3 towers.
  190. */
  191. bool home_delta() {
  192. #if ENABLED(DEBUG_LEVELING_FEATURE)
  193. if (DEBUGGING(LEVELING)) DEBUG_POS(">>> home_delta", current_position);
  194. #endif
  195. // Init the current position of all carriages to 0,0,0
  196. ZERO(current_position);
  197. sync_plan_position();
  198. // Move all carriages together linearly until an endstop is hit.
  199. current_position[X_AXIS] = current_position[Y_AXIS] = current_position[Z_AXIS] = (DELTA_HEIGHT + home_offset[Z_AXIS] + 10);
  200. feedrate_mm_s = homing_feedrate(X_AXIS);
  201. line_to_current_position();
  202. stepper.synchronize();
  203. // If an endstop was not hit, then damage can occur if homing is continued.
  204. // This can occur if the delta height (DELTA_HEIGHT + home_offset[Z_AXIS]) is
  205. // not set correctly.
  206. if (!(Endstops::endstop_hit_bits & (_BV(X_MAX) | _BV(Y_MAX) | _BV(Z_MAX)))) {
  207. LCD_MESSAGEPGM(MSG_ERR_HOMING_FAILED);
  208. SERIAL_ERROR_START();
  209. SERIAL_ERRORLNPGM(MSG_ERR_HOMING_FAILED);
  210. return false;
  211. }
  212. endstops.hit_on_purpose(); // clear endstop hit flags
  213. // At least one carriage has reached the top.
  214. // Now re-home each carriage separately.
  215. HOMEAXIS(A);
  216. HOMEAXIS(B);
  217. HOMEAXIS(C);
  218. // Set all carriages to their home positions
  219. // Do this here all at once for Delta, because
  220. // XYZ isn't ABC. Applying this per-tower would
  221. // give the impression that they are the same.
  222. LOOP_XYZ(i) set_axis_is_at_home((AxisEnum)i);
  223. SYNC_PLAN_POSITION_KINEMATIC();
  224. #if ENABLED(DEBUG_LEVELING_FEATURE)
  225. if (DEBUGGING(LEVELING)) DEBUG_POS("<<< home_delta", current_position);
  226. #endif
  227. return true;
  228. }
  229. #endif // DELTA