Browse Source

Fix G2/G3 P<circles> E and Z motion (#19797)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
uwedamm 4 years ago
parent
commit
a596969049
No account linked to committer's email address
2 changed files with 30 additions and 13 deletions
  1. 2
    2
      Marlin/src/gcode/bedlevel/G26.cpp
  2. 28
    11
      Marlin/src/gcode/motion/G2_G3.cpp

+ 2
- 2
Marlin/src/gcode/bedlevel/G26.cpp View File

@@ -136,7 +136,7 @@
136 136
 #define G26_ERR true
137 137
 
138 138
 #if ENABLED(ARC_SUPPORT)
139
-  void plan_arc(const xyze_pos_t &cart, const ab_float_t &offset, const uint8_t clockwise);
139
+  void plan_arc(const xyze_pos_t&, const ab_float_t&, const bool, const uint8_t);
140 140
 #endif
141 141
 
142 142
 constexpr float g26_e_axis_feedrate = 0.025;
@@ -783,7 +783,7 @@ void GcodeSuite::G26() {
783 783
 
784 784
         const feedRate_t old_feedrate = feedrate_mm_s;
785 785
         feedrate_mm_s = PLANNER_XY_FEEDRATE() * 0.1f;
786
-        plan_arc(endpoint, arc_offset, false);  // Draw a counter-clockwise arc
786
+        plan_arc(endpoint, arc_offset, false, 0);  // Draw a counter-clockwise arc
787 787
         feedrate_mm_s = old_feedrate;
788 788
         destination = current_position;
789 789
 

+ 28
- 11
Marlin/src/gcode/motion/G2_G3.cpp View File

@@ -52,7 +52,8 @@
52 52
 void plan_arc(
53 53
   const xyze_pos_t &cart,   // Destination position
54 54
   const ab_float_t &offset, // Center of rotation relative to current_position
55
-  const uint8_t clockwise   // Clockwise?
55
+  const bool clockwise,     // Clockwise?
56
+  const uint8_t circles     // Take the scenic route
56 57
 ) {
57 58
   #if ENABLED(CNC_WORKSPACE_PLANES)
58 59
     AxisEnum p_axis, q_axis, l_axis;
@@ -74,9 +75,7 @@ void plan_arc(
74 75
               center_Q = current_position[q_axis] - rvec.b,
75 76
               rt_X = cart[p_axis] - center_P,
76 77
               rt_Y = cart[q_axis] - center_Q,
77
-              start_L = current_position[l_axis],
78
-              linear_travel = cart[l_axis] - start_L,
79
-              extruder_travel = cart.e - current_position.e;
78
+              start_L = current_position[l_axis];
80 79
 
81 80
   // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
82 81
   float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);
@@ -90,13 +89,32 @@ void plan_arc(
90 89
   if (clockwise) angular_travel -= RADIANS(360);
91 90
 
92 91
   // Make a circle if the angular rotation is 0 and the target is current position
93
-  if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis]) {
92
+  if (NEAR_ZERO(angular_travel) && NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {
94 93
     angular_travel = RADIANS(360);
95 94
     #ifdef MIN_ARC_SEGMENTS
96 95
       min_segments = MIN_ARC_SEGMENTS;
97 96
     #endif
98 97
   }
99 98
 
99
+  float linear_travel = cart[l_axis] - start_L,
100
+        extruder_travel = cart.e - current_position.e;
101
+
102
+  // If circling around...
103
+  if (ENABLED(ARC_P_CIRCLES) && circles) {
104
+    const float total_angular = angular_travel + circles * RADIANS(360),  // Total rotation with all circles and remainder
105
+              part_per_circle = RADIANS(360) / total_angular,             // Each circle's part of the total
106
+                 l_per_circle = linear_travel * part_per_circle,          // L movement per circle
107
+                 e_per_circle = extruder_travel * part_per_circle;        // E movement per circle
108
+    xyze_pos_t temp_position = current_position;                          // for plan_arc to compare to current_position
109
+    for (uint16_t n = circles; n--;) {
110
+      temp_position.e += e_per_circle;                                    // Destination E axis
111
+      temp_position[l_axis] += l_per_circle;                              // Destination L axis
112
+      plan_arc(temp_position, offset, clockwise, 0);                      // Plan a single whole circle
113
+    }
114
+    linear_travel = cart[l_axis] - current_position[l_axis];
115
+    extruder_travel = cart.e - current_position.e;
116
+  }
117
+
100 118
   const float flat_mm = radius * angular_travel,
101 119
               mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm);
102 120
   if (mm_of_travel < 0.001f) return;
@@ -150,7 +168,7 @@ void plan_arc(
150 168
               linear_per_segment = linear_travel / segments,
151 169
               extruder_per_segment = extruder_travel / segments,
152 170
               sq_theta_per_segment = sq(theta_per_segment),
153
-              sin_T = theta_per_segment - sq_theta_per_segment*theta_per_segment/6,
171
+              sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
154 172
               cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
155 173
 
156 174
   // Initialize the linear axis
@@ -320,16 +338,15 @@ void GcodeSuite::G2_G3(const bool clockwise) {
320 338
 
321 339
       #if ENABLED(ARC_P_CIRCLES)
322 340
         // P indicates number of circles to do
323
-        int8_t circles_to_do = parser.byteval('P');
341
+        const int8_t circles_to_do = parser.byteval('P');
324 342
         if (!WITHIN(circles_to_do, 0, 100))
325 343
           SERIAL_ERROR_MSG(STR_ERR_ARC_ARGS);
326
-
327
-        while (circles_to_do--)
328
-          plan_arc(current_position, arc_offset, clockwise);
344
+      #else
345
+        constexpr uint8_t circles_to_do = 0;
329 346
       #endif
330 347
 
331 348
       // Send the arc to the planner
332
-      plan_arc(destination, arc_offset, clockwise);
349
+      plan_arc(destination, arc_offset, clockwise, circles_to_do);
333 350
       reset_stepper_timeout();
334 351
     }
335 352
     else

Loading…
Cancel
Save