motion_control.cpp 8.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166
  1. /*
  2. motion_control.c - high level interface for issuing motion commands
  3. Part of Grbl
  4. Copyright (c) 2009-2011 Simen Svale Skogsrud
  5. Copyright (c) 2011 Sungeun K. Jeon
  6. Copyright (c) 2020 Brad Hochgesang
  7. Grbl is free software: you can redistribute it and/or modify
  8. it under the terms of the GNU General Public License as published by
  9. the Free Software Foundation, either version 3 of the License, or
  10. (at your option) any later version.
  11. Grbl is distributed in the hope that it will be useful,
  12. but WITHOUT ANY WARRANTY; without even the implied warranty of
  13. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  14. GNU General Public License for more details.
  15. You should have received a copy of the GNU General Public License
  16. along with Grbl. If not, see <http://www.gnu.org/licenses/>.
  17. */
  18. #include "Marlin.h"
  19. #include "stepper.h"
  20. #include "planner.h"
  21. // The arc is approximated by generating a huge number of tiny, linear segments. The length of each
  22. // segment is configured in settings.mm_per_arc_segment.
  23. void mc_arc(const float* position, float* target, const float* offset, float feed_rate, float radius, bool isclockwise, uint8_t extruder, uint16_t start_segment_idx)
  24. {
  25. float start_position[4];
  26. memcpy(start_position, position, sizeof(start_position));
  27. float r_axis_x = -offset[X_AXIS]; // Radius vector from center to current location
  28. float r_axis_y = -offset[Y_AXIS];
  29. float center_axis_x = start_position[X_AXIS] - r_axis_x;
  30. float center_axis_y = start_position[Y_AXIS] - r_axis_y;
  31. float travel_z = target[Z_AXIS] - start_position[Z_AXIS];
  32. float rt_x = target[X_AXIS] - center_axis_x;
  33. float rt_y = target[Y_AXIS] - center_axis_y;
  34. // 20200419 - Add a variable that will be used to hold the arc segment length
  35. float mm_per_arc_segment = cs.mm_per_arc_segment;
  36. // 20210109 - Add a variable to hold the n_arc_correction value
  37. unsigned char n_arc_correction = cs.n_arc_correction;
  38. // CCW angle between start_position and target from circle center. Only one atan2() trig computation required.
  39. float angular_travel_total = atan2(r_axis_x * rt_y - r_axis_y * rt_x, r_axis_x * rt_x + r_axis_y * rt_y);
  40. if (angular_travel_total < 0) { angular_travel_total += 2 * M_PI; }
  41. if (cs.min_arc_segments > 0)
  42. {
  43. // 20200417 - FormerLurker - Implement MIN_ARC_SEGMENTS if it is defined - from Marlin 2.0 implementation
  44. // Do this before converting the angular travel for clockwise rotation
  45. mm_per_arc_segment = radius * ((2.0f * M_PI) / cs.min_arc_segments);
  46. }
  47. if (cs.arc_segments_per_sec > 0)
  48. {
  49. // 20200417 - FormerLurker - Implement MIN_ARC_SEGMENTS if it is defined - from Marlin 2.0 implementation
  50. float mm_per_arc_segment_sec = (feed_rate / 60.0f) * (1.0f / cs.arc_segments_per_sec);
  51. if (mm_per_arc_segment_sec < mm_per_arc_segment)
  52. mm_per_arc_segment = mm_per_arc_segment_sec;
  53. }
  54. // Note: no need to check to see if min_mm_per_arc_segment is enabled or not (i.e. = 0), since mm_per_arc_segment can never be below 0.
  55. if (mm_per_arc_segment < cs.min_mm_per_arc_segment)
  56. {
  57. // 20200417 - FormerLurker - Implement MIN_MM_PER_ARC_SEGMENT if it is defined
  58. // This prevents a very high number of segments from being generated for curves of a short radius
  59. mm_per_arc_segment = cs.min_mm_per_arc_segment;
  60. }
  61. else if (mm_per_arc_segment > cs.mm_per_arc_segment) {
  62. // 20210113 - This can be implemented in an else if since we can't be below the min AND above the max at the same time.
  63. // 20200417 - FormerLurker - Implement MIN_MM_PER_ARC_SEGMENT if it is defined
  64. mm_per_arc_segment = cs.mm_per_arc_segment;
  65. }
  66. // Adjust the angular travel if the direction is clockwise
  67. if (isclockwise) { angular_travel_total -= 2 * M_PI; }
  68. //20141002:full circle for G03 did not work, e.g. G03 X80 Y80 I20 J0 F2000 is giving an Angle of zero so head is not moving
  69. //to compensate when start pos = target pos && angle is zero -> angle = 2Pi
  70. if (start_position[X_AXIS] == target[X_AXIS] && start_position[Y_AXIS] == target[Y_AXIS] && angular_travel_total == 0)
  71. {
  72. angular_travel_total += 2 * M_PI;
  73. }
  74. //end fix G03
  75. // 20200417 - FormerLurker - rename millimeters_of_travel to millimeters_of_travel_arc to better describe what we are
  76. // calculating here
  77. const float millimeters_of_travel_arc = hypot(angular_travel_total * radius, fabs(travel_z));
  78. if (millimeters_of_travel_arc < 0.001) { return; }
  79. // Calculate the number of arc segments
  80. unsigned short segments = static_cast<unsigned short>(ceil(millimeters_of_travel_arc / mm_per_arc_segment));
  81. /* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
  82. and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
  83. r_T = [cos(phi) -sin(phi);
  84. sin(phi) cos(phi] * r ;
  85. For arc generation, the center of the circle is the axis of rotation and the radius vector is
  86. defined from the circle center to the initial position. Each line segment is formed by successive
  87. vector rotations. This requires only two cos() and sin() computations to form the rotation
  88. matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
  89. all double numbers are single precision on the Arduino. (True double precision will not have
  90. round off issues for CNC applications.) Single precision error can accumulate to be greater than
  91. tool precision in some cases. Therefore, arc path correction is implemented.
  92. The small angle approximation was removed because of excessive errors for small circles (perhaps unique to
  93. 3d printing applications, causing significant path deviation and extrusion issues).
  94. Now there will be no corrections applied, but an accurate initial sin and cos will be calculated.
  95. This seems to work with a very high degree of accuracy and results in much simpler code.
  96. Finding a faster way to approximate sin, knowing that there can be substantial deviations from the true
  97. arc when using the previous approximation, would be beneficial.
  98. */
  99. // If there is only one segment, no need to do a bunch of work since this is a straight line!
  100. if (segments > 1 && start_segment_idx)
  101. {
  102. // Calculate theta per segments, and linear (z) travel per segment, e travel per segment
  103. // as well as the small angle approximation for sin and cos.
  104. const float theta_per_segment = angular_travel_total / segments,
  105. linear_per_segment = travel_z / (segments),
  106. segment_extruder_travel = (target[E_AXIS] - start_position[E_AXIS]) / (segments),
  107. sq_theta_per_segment = theta_per_segment * theta_per_segment,
  108. sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
  109. cos_T = 1 - 0.5f * sq_theta_per_segment;
  110. // Loop through all but one of the segments. The last one can be done simply
  111. // by moving to the target.
  112. for (uint16_t i = 1; i < segments; i++) {
  113. if (n_arc_correction-- == 0) {
  114. // Calculate the actual position for r_axis_x and r_axis_y
  115. const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
  116. r_axis_x = -offset[X_AXIS] * cos_Ti + offset[Y_AXIS] * sin_Ti;
  117. r_axis_y = -offset[X_AXIS] * sin_Ti - offset[Y_AXIS] * cos_Ti;
  118. // reset n_arc_correction
  119. n_arc_correction = cs.n_arc_correction;
  120. }
  121. else {
  122. // Calculate X and Y using the small angle approximation
  123. const float r_axisi = r_axis_x * sin_T + r_axis_y * cos_T;
  124. r_axis_x = r_axis_x * cos_T - r_axis_y * sin_T;
  125. r_axis_y = r_axisi;
  126. }
  127. // Update Position
  128. start_position[X_AXIS] = center_axis_x + r_axis_x;
  129. start_position[Y_AXIS] = center_axis_y + r_axis_y;
  130. start_position[Z_AXIS] += linear_per_segment;
  131. start_position[E_AXIS] += segment_extruder_travel;
  132. // Clamp to the calculated position.
  133. clamp_to_software_endstops(start_position);
  134. // Insert the segment into the buffer
  135. if (i >= start_segment_idx)
  136. plan_buffer_line(start_position[X_AXIS], start_position[Y_AXIS], start_position[Z_AXIS], start_position[E_AXIS], feed_rate, extruder, position, i);
  137. // Handle the situation where the planner is aborted hard.
  138. if (planner_aborted)
  139. return;
  140. }
  141. }
  142. // Clamp to the target position.
  143. clamp_to_software_endstops(target);
  144. // Ensure last segment arrives at target location.
  145. plan_buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, extruder, position, 0);
  146. }