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.

G2_G3.cpp 14KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340
  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. #include "../../inc/MarlinConfig.h"
  23. #if ENABLED(ARC_SUPPORT)
  24. #include "../gcode.h"
  25. #include "../../module/motion.h"
  26. #include "../../module/planner.h"
  27. #include "../../module/temperature.h"
  28. #if ENABLED(DELTA)
  29. #include "../../module/delta.h"
  30. #elif ENABLED(SCARA)
  31. #include "../../module/scara.h"
  32. #endif
  33. #if HAS_FEEDRATE_SCALING && ENABLED(AUTO_BED_LEVELING_BILINEAR)
  34. #include "../../feature/bedlevel/abl/abl.h"
  35. #endif
  36. #if N_ARC_CORRECTION < 1
  37. #undef N_ARC_CORRECTION
  38. #define N_ARC_CORRECTION 1
  39. #endif
  40. /**
  41. * Plan an arc in 2 dimensions
  42. *
  43. * The arc is approximated by generating many small linear segments.
  44. * The length of each segment is configured in MM_PER_ARC_SEGMENT (Default 1mm)
  45. * Arcs should only be made relatively large (over 5mm), as larger arcs with
  46. * larger segments will tend to be more efficient. Your slicer should have
  47. * options for G2/G3 arc generation. In future these options may be GCode tunable.
  48. */
  49. void plan_arc(
  50. const float (&cart)[XYZE], // Destination position
  51. const float (&offset)[2], // Center of rotation relative to current_position
  52. const uint8_t clockwise // Clockwise?
  53. ) {
  54. #if ENABLED(CNC_WORKSPACE_PLANES)
  55. AxisEnum p_axis, q_axis, l_axis;
  56. switch (gcode.workspace_plane) {
  57. default:
  58. case GcodeSuite::PLANE_XY: p_axis = X_AXIS; q_axis = Y_AXIS; l_axis = Z_AXIS; break;
  59. case GcodeSuite::PLANE_ZX: p_axis = Z_AXIS; q_axis = X_AXIS; l_axis = Y_AXIS; break;
  60. case GcodeSuite::PLANE_YZ: p_axis = Y_AXIS; q_axis = Z_AXIS; l_axis = X_AXIS; break;
  61. }
  62. #else
  63. constexpr AxisEnum p_axis = X_AXIS, q_axis = Y_AXIS, l_axis = Z_AXIS;
  64. #endif
  65. // Radius vector from center to current location
  66. float r_P = -offset[0], r_Q = -offset[1];
  67. const float radius = HYPOT(r_P, r_Q),
  68. center_P = current_position[p_axis] - r_P,
  69. center_Q = current_position[q_axis] - r_Q,
  70. rt_X = cart[p_axis] - center_P,
  71. rt_Y = cart[q_axis] - center_Q,
  72. linear_travel = cart[l_axis] - current_position[l_axis],
  73. extruder_travel = cart[E_AXIS] - current_position[E_AXIS];
  74. // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
  75. float angular_travel = ATAN2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y);
  76. if (angular_travel < 0) angular_travel += RADIANS(360);
  77. if (clockwise) angular_travel -= RADIANS(360);
  78. // Make a circle if the angular rotation is 0 and the target is current position
  79. if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis])
  80. angular_travel = RADIANS(360);
  81. const float flat_mm = radius * angular_travel,
  82. mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm);
  83. if (mm_of_travel < 0.001f) return;
  84. uint16_t segments = FLOOR(mm_of_travel / (MM_PER_ARC_SEGMENT));
  85. if (segments == 0) segments = 1;
  86. /**
  87. * Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
  88. * and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
  89. * r_T = [cos(phi) -sin(phi);
  90. * sin(phi) cos(phi)] * r ;
  91. *
  92. * For arc generation, the center of the circle is the axis of rotation and the radius vector is
  93. * defined from the circle center to the initial position. Each line segment is formed by successive
  94. * vector rotations. This requires only two cos() and sin() computations to form the rotation
  95. * matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
  96. * all double numbers are single precision on the Arduino. (True double precision will not have
  97. * round off issues for CNC applications.) Single precision error can accumulate to be greater than
  98. * tool precision in some cases. Therefore, arc path correction is implemented.
  99. *
  100. * Small angle approximation may be used to reduce computation overhead further. This approximation
  101. * holds for everything, but very small circles and large MM_PER_ARC_SEGMENT values. In other words,
  102. * theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
  103. * to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
  104. * numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
  105. * issue for CNC machines with the single precision Arduino calculations.
  106. *
  107. * This approximation also allows plan_arc to immediately insert a line segment into the planner
  108. * without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
  109. * a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead.
  110. * This is important when there are successive arc motions.
  111. */
  112. // Vector rotation matrix values
  113. float raw[XYZE];
  114. const float theta_per_segment = angular_travel / segments,
  115. linear_per_segment = linear_travel / segments,
  116. extruder_per_segment = extruder_travel / segments,
  117. sin_T = theta_per_segment,
  118. cos_T = 1 - 0.5f * sq(theta_per_segment); // Small angle approximation
  119. // Initialize the linear axis
  120. raw[l_axis] = current_position[l_axis];
  121. // Initialize the extruder axis
  122. raw[E_AXIS] = current_position[E_AXIS];
  123. const float fr_mm_s = MMS_SCALED(feedrate_mm_s);
  124. millis_t next_idle_ms = millis() + 200UL;
  125. #if HAS_FEEDRATE_SCALING
  126. // SCARA needs to scale the feed rate from mm/s to degrees/s
  127. const float inv_segment_length = 1.0f / float(MM_PER_ARC_SEGMENT),
  128. inverse_secs = inv_segment_length * fr_mm_s;
  129. float oldA = planner.position_float[A_AXIS],
  130. oldB = planner.position_float[B_AXIS]
  131. #if ENABLED(DELTA_FEEDRATE_SCALING)
  132. , oldC = planner.position_float[C_AXIS]
  133. #endif
  134. ;
  135. #endif
  136. #if N_ARC_CORRECTION > 1
  137. int8_t arc_recalc_count = N_ARC_CORRECTION;
  138. #endif
  139. for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
  140. thermalManager.manage_heater();
  141. if (ELAPSED(millis(), next_idle_ms)) {
  142. next_idle_ms = millis() + 200UL;
  143. idle();
  144. }
  145. #if N_ARC_CORRECTION > 1
  146. if (--arc_recalc_count) {
  147. // Apply vector rotation matrix to previous r_P / 1
  148. const float r_new_Y = r_P * sin_T + r_Q * cos_T;
  149. r_P = r_P * cos_T - r_Q * sin_T;
  150. r_Q = r_new_Y;
  151. }
  152. else
  153. #endif
  154. {
  155. #if N_ARC_CORRECTION > 1
  156. arc_recalc_count = N_ARC_CORRECTION;
  157. #endif
  158. // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
  159. // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
  160. // To reduce stuttering, the sin and cos could be computed at different times.
  161. // For now, compute both at the same time.
  162. const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
  163. r_P = -offset[0] * cos_Ti + offset[1] * sin_Ti;
  164. r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti;
  165. }
  166. // Update raw location
  167. raw[p_axis] = center_P + r_P;
  168. raw[q_axis] = center_Q + r_Q;
  169. raw[l_axis] += linear_per_segment;
  170. raw[E_AXIS] += extruder_per_segment;
  171. clamp_to_software_endstops(raw);
  172. #if HAS_FEEDRATE_SCALING
  173. inverse_kinematics(raw);
  174. ADJUST_DELTA(raw);
  175. #endif
  176. #if ENABLED(SCARA_FEEDRATE_SCALING)
  177. // For SCARA scale the feed rate from mm/s to degrees/s
  178. // i.e., Complete the angular vector in the given time.
  179. if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder))
  180. break;
  181. oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
  182. #elif ENABLED(DELTA_FEEDRATE_SCALING)
  183. // For DELTA scale the feed rate from Effector mm/s to Carriage mm/s
  184. // i.e., Complete the linear vector in the given time.
  185. if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], SQRT(sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC)) * inverse_secs, active_extruder))
  186. break;
  187. oldA = delta[A_AXIS]; oldB = delta[B_AXIS]; oldC = delta[C_AXIS];
  188. #elif HAS_UBL_AND_CURVES
  189. float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
  190. planner.apply_leveling(pos);
  191. if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder))
  192. break;
  193. #else
  194. if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder))
  195. break;
  196. #endif
  197. }
  198. // Ensure last segment arrives at target location.
  199. #if HAS_FEEDRATE_SCALING
  200. inverse_kinematics(cart);
  201. ADJUST_DELTA(cart);
  202. #endif
  203. #if ENABLED(SCARA_FEEDRATE_SCALING)
  204. const float diff2 = HYPOT2(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB);
  205. if (diff2)
  206. planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], cart[Z_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder);
  207. #elif ENABLED(DELTA_FEEDRATE_SCALING)
  208. const float diff2 = sq(delta[A_AXIS] - oldA) + sq(delta[B_AXIS] - oldB) + sq(delta[C_AXIS] - oldC);
  209. if (diff2)
  210. planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], SQRT(diff2) * inverse_secs, active_extruder);
  211. #elif HAS_UBL_AND_CURVES
  212. float pos[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
  213. planner.apply_leveling(pos);
  214. planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], cart[E_AXIS], fr_mm_s, active_extruder);
  215. #else
  216. planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder);
  217. #endif
  218. COPY(current_position, cart);
  219. } // plan_arc
  220. /**
  221. * G2: Clockwise Arc
  222. * G3: Counterclockwise Arc
  223. *
  224. * This command has two forms: IJ-form and R-form.
  225. *
  226. * - I specifies an X offset. J specifies a Y offset.
  227. * At least one of the IJ parameters is required.
  228. * X and Y can be omitted to do a complete circle.
  229. * The given XY is not error-checked. The arc ends
  230. * based on the angle of the destination.
  231. * Mixing I or J with R will throw an error.
  232. *
  233. * - R specifies the radius. X or Y is required.
  234. * Omitting both X and Y will throw an error.
  235. * X or Y must differ from the current XY.
  236. * Mixing R with I or J will throw an error.
  237. *
  238. * - P specifies the number of full circles to do
  239. * before the specified arc move.
  240. *
  241. * Examples:
  242. *
  243. * G2 I10 ; CW circle centered at X+10
  244. * G3 X20 Y12 R14 ; CCW circle with r=14 ending at X20 Y12
  245. */
  246. void GcodeSuite::G2_G3(const bool clockwise) {
  247. if (MOTION_CONDITIONS) {
  248. #if ENABLED(SF_ARC_FIX)
  249. const bool relative_mode_backup = relative_mode;
  250. relative_mode = true;
  251. #endif
  252. get_destination_from_command();
  253. #if ENABLED(SF_ARC_FIX)
  254. relative_mode = relative_mode_backup;
  255. #endif
  256. float arc_offset[2] = { 0, 0 };
  257. if (parser.seenval('R')) {
  258. const float r = parser.value_linear_units(),
  259. p1 = current_position[X_AXIS], q1 = current_position[Y_AXIS],
  260. p2 = destination[X_AXIS], q2 = destination[Y_AXIS];
  261. if (r && (p2 != p1 || q2 != q1)) {
  262. const float e = clockwise ^ (r < 0) ? -1 : 1, // clockwise -1/1, counterclockwise 1/-1
  263. dx = p2 - p1, dy = q2 - q1, // X and Y differences
  264. d = HYPOT(dx, dy), // Linear distance between the points
  265. dinv = 1/d, // Inverse of d
  266. h = SQRT(sq(r) - sq(d * 0.5f)), // Distance to the arc pivot-point
  267. mx = (p1 + p2) * 0.5f, my = (q1 + q2) * 0.5f,// Point between the two points
  268. sx = -dy * dinv, sy = dx * dinv, // Slope of the perpendicular bisector
  269. cx = mx + e * h * sx, cy = my + e * h * sy; // Pivot-point of the arc
  270. arc_offset[0] = cx - p1;
  271. arc_offset[1] = cy - q1;
  272. }
  273. }
  274. else {
  275. if (parser.seenval('I')) arc_offset[0] = parser.value_linear_units();
  276. if (parser.seenval('J')) arc_offset[1] = parser.value_linear_units();
  277. }
  278. if (arc_offset[0] || arc_offset[1]) {
  279. #if ENABLED(ARC_P_CIRCLES)
  280. // P indicates number of circles to do
  281. int8_t circles_to_do = parser.byteval('P');
  282. if (!WITHIN(circles_to_do, 0, 100)) {
  283. SERIAL_ERROR_START();
  284. SERIAL_ERRORLNPGM(MSG_ERR_ARC_ARGS);
  285. }
  286. while (circles_to_do--)
  287. plan_arc(current_position, arc_offset, clockwise);
  288. #endif
  289. // Send the arc to the planner
  290. plan_arc(destination, arc_offset, clockwise);
  291. reset_stepper_timeout();
  292. }
  293. else {
  294. // Bad arguments
  295. SERIAL_ERROR_START();
  296. SERIAL_ERRORLNPGM(MSG_ERR_ARC_ARGS);
  297. }
  298. }
  299. }
  300. #endif // ARC_SUPPORT