Merge pull request #3751 from AnHardt/fix-arcs

Idle during long arcs

Marlin/Configuration_adv.h

@@ -455,6 +455,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/Marlin_main.cpp

@@ -506,7 +506,9 @@ void stop();
 void get_available_commands();
 void process_next_command();
 
-void plan_arc(float target[NUM_AXIS], float* offset, uint8_t clockwise);
+#if ENABLED(ARC_SUPPORT)
+  void plan_arc(float target[NUM_AXIS], float* offset, uint8_t clockwise);
+#endif
 
 void serial_echopair_P(const char* s_P, int v)           { serialprintPGM(s_P); SERIAL_ECHO(v); }
 void serial_echopair_P(const char* s_P, long v)          { serialprintPGM(s_P); SERIAL_ECHO(v); }
@@ -2461,32 +2463,34 @@ inline void gcode_G0_G1() {
  * G2: Clockwise Arc
  * G3: Counterclockwise Arc
  */
-inline void gcode_G2_G3(bool clockwise) {
+#if ENABLED(ARC_SUPPORT)
-  if (IsRunning()) {
+  inline void gcode_G2_G3(bool clockwise) {
+    if (IsRunning()) {
 
-    #if ENABLED(SF_ARC_FIX)
+      #if ENABLED(SF_ARC_FIX)
-      bool relative_mode_backup = relative_mode;
+        bool relative_mode_backup = relative_mode;
-      relative_mode = true;
+        relative_mode = true;
-    #endif
+      #endif
 
-    gcode_get_destination();
+      gcode_get_destination();
 
-    #if ENABLED(SF_ARC_FIX)
+      #if ENABLED(SF_ARC_FIX)
-      relative_mode = relative_mode_backup;
+        relative_mode = relative_mode_backup;
-    #endif
+      #endif
 
-    // Center of arc as offset from current_position
+      // Center of arc as offset from current_position
-    float arc_offset[2] = {
+      float arc_offset[2] = {
-      code_seen('I') ? code_value() : 0,
+        code_seen('I') ? code_value() : 0,
-      code_seen('J') ? code_value() : 0
+        code_seen('J') ? code_value() : 0
-    };
+      };
 
-    // Send an arc to the planner
+      // Send an arc to the planner
-    plan_arc(destination, arc_offset, clockwise);
+      plan_arc(destination, arc_offset, clockwise);
 
-    refresh_cmd_timeout();
+      refresh_cmd_timeout();
+    }
   }
-}
+#endif
 
 /**
  * G4: Dwell S<seconds> or P<milliseconds>
@@ -6484,7 +6488,7 @@ void process_next_command() {
         break;
 
       // G2, G3
-      #if DISABLED(SCARA)
+      #if ENABLED(ARC_SUPPORT) && DISABLED(SCARA)
         case 2: // G2  - CW ARC
         case 3: // G3  - CCW ARC
           gcode_G2_G3(codenum == 2);
@@ -7423,147 +7427,157 @@ void prepare_move() {
   set_current_to_destination();
 }
 
-/**
+#if ENABLED(ARC_SUPPORT)
- * Plan an arc in 2 dimensions
- *
- * The arc is approximated by generating many small linear segments.
- * The length of each segment is configured in MM_PER_ARC_SEGMENT (Default 1mm)
- * Arcs should only be made relatively large (over 5mm), as larger arcs with
- * larger segments will tend to be more efficient. Your slicer should have
- * options for G2/G3 arc generation. In future these options may be GCode tunable.
- */
-void plan_arc(
-  float target[NUM_AXIS], // Destination position
-  float* offset,          // Center of rotation relative to current_position
-  uint8_t clockwise       // Clockwise?
-) {
-
-  float radius = hypot(offset[X_AXIS], offset[Y_AXIS]),
-        center_X = current_position[X_AXIS] + offset[X_AXIS],
-        center_Y = current_position[Y_AXIS] + offset[Y_AXIS],
-        linear_travel = target[Z_AXIS] - current_position[Z_AXIS],
-        extruder_travel = target[E_AXIS] - current_position[E_AXIS],
-        r_X = -offset[X_AXIS],  // Radius vector from center to current location
-        r_Y = -offset[Y_AXIS],
-        rt_X = target[X_AXIS] - center_X,
-        rt_Y = target[Y_AXIS] - center_Y;
-
-  // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
-  float angular_travel = atan2(r_X * rt_Y - r_Y * rt_X, r_X * rt_X + r_Y * rt_Y);
-  if (angular_travel < 0) angular_travel += RADIANS(360);
-  if (clockwise) angular_travel -= RADIANS(360);
-
-  // Make a circle if the angular rotation is 0
-  if (angular_travel == 0 && current_position[X_AXIS] == target[X_AXIS] && current_position[Y_AXIS] == target[Y_AXIS])
-    angular_travel += RADIANS(360);
-
-  float mm_of_travel = hypot(angular_travel * radius, fabs(linear_travel));
-  if (mm_of_travel < 0.001) return;
-  uint16_t segments = floor(mm_of_travel / (MM_PER_ARC_SEGMENT));
-  if (segments == 0) segments = 1;
-
-  float theta_per_segment = angular_travel / segments;
-  float linear_per_segment = linear_travel / segments;
-  float extruder_per_segment = extruder_travel / segments;
-
   /**
-   * Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
+   * Plan an arc in 2 dimensions
-   * and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
-   *     r_T = [cos(phi) -sin(phi);
-   *            sin(phi)  cos(phi] * r ;
    *
-   * For arc generation, the center of the circle is the axis of rotation and the radius vector is
+   * The arc is approximated by generating many small linear segments.
-   * defined from the circle center to the initial position. Each line segment is formed by successive
+   * The length of each segment is configured in MM_PER_ARC_SEGMENT (Default 1mm)
-   * vector rotations. This requires only two cos() and sin() computations to form the rotation
+   * Arcs should only be made relatively large (over 5mm), as larger arcs with
-   * matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
+   * larger segments will tend to be more efficient. Your slicer should have
-   * all double numbers are single precision on the Arduino. (True double precision will not have
+   * options for G2/G3 arc generation. In future these options may be GCode tunable.
-   * round off issues for CNC applications.) Single precision error can accumulate to be greater than
-   * tool precision in some cases. Therefore, arc path correction is implemented.
-   *
-   * Small angle approximation may be used to reduce computation overhead further. This approximation
-   * holds for everything, but very small circles and large MM_PER_ARC_SEGMENT values. In other words,
-   * theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
-   * to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
-   * numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
-   * issue for CNC machines with the single precision Arduino calculations.
-   *
-   * This approximation also allows plan_arc to immediately insert a line segment into the planner
-   * without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
-   * a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead.
-   * This is important when there are successive arc motions.
    */
-  // Vector rotation matrix values
+  void plan_arc(
-  float cos_T = 1 - 0.5 * theta_per_segment * theta_per_segment; // Small angle approximation
+    float target[NUM_AXIS], // Destination position
-  float sin_T = theta_per_segment;
+    float* offset,          // Center of rotation relative to current_position
+    uint8_t clockwise       // Clockwise?
+  ) {
 
-  float arc_target[NUM_AXIS];
+    float radius = hypot(offset[X_AXIS], offset[Y_AXIS]),
-  float sin_Ti, cos_Ti, r_new_Y;
+          center_X = current_position[X_AXIS] + offset[X_AXIS],
-  uint16_t i;
+          center_Y = current_position[Y_AXIS] + offset[Y_AXIS],
-  int8_t count = 0;
+          linear_travel = target[Z_AXIS] - current_position[Z_AXIS],
+          extruder_travel = target[E_AXIS] - current_position[E_AXIS],
+          r_X = -offset[X_AXIS],  // Radius vector from center to current location
+          r_Y = -offset[Y_AXIS],
+          rt_X = target[X_AXIS] - center_X,
+          rt_Y = target[Y_AXIS] - center_Y;
 
-  // Initialize the linear axis
+    // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
-  arc_target[Z_AXIS] = current_position[Z_AXIS];
+    float angular_travel = atan2(r_X * rt_Y - r_Y * rt_X, r_X * rt_X + r_Y * rt_Y);
+    if (angular_travel < 0) angular_travel += RADIANS(360);
+    if (clockwise) angular_travel -= RADIANS(360);
 
-  // Initialize the extruder axis
+    // Make a circle if the angular rotation is 0
-  arc_target[E_AXIS] = current_position[E_AXIS];
+    if (angular_travel == 0 && current_position[X_AXIS] == target[X_AXIS] && current_position[Y_AXIS] == target[Y_AXIS])
+      angular_travel += RADIANS(360);
 
-  float feed_rate = feedrate * feedrate_multiplier / 60 / 100.0;
+    float mm_of_travel = hypot(angular_travel * radius, fabs(linear_travel));
+    if (mm_of_travel < 0.001) return;
+    uint16_t segments = floor(mm_of_travel / (MM_PER_ARC_SEGMENT));
+    if (segments == 0) segments = 1;
 
-  for (i = 1; i < segments; i++) { // Iterate (segments-1) times
+    float theta_per_segment = angular_travel / segments;
+    float linear_per_segment = linear_travel / segments;
+    float extruder_per_segment = extruder_travel / segments;
 
-    if (++count < N_ARC_CORRECTION) {
+    /**
-      // Apply vector rotation matrix to previous r_X / 1
+     * Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
-      r_new_Y = r_X * sin_T + r_Y * cos_T;
+     * and phi is the angle of rotation. Based on the solution approach by Jens Geisler.
-      r_X = r_X * cos_T - r_Y * sin_T;
+     *     r_T = [cos(phi) -sin(phi);
-      r_Y = r_new_Y;
+     *            sin(phi)  cos(phi] * r ;
-    }
+     *
-    else {
+     * For arc generation, the center of the circle is the axis of rotation and the radius vector is
-      // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
+     * defined from the circle center to the initial position. Each line segment is formed by successive
-      // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
+     * vector rotations. This requires only two cos() and sin() computations to form the rotation
-      // To reduce stuttering, the sin and cos could be computed at different times.
+     * matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since
-      // For now, compute both at the same time.
+     * all double numbers are single precision on the Arduino. (True double precision will not have
-      cos_Ti = cos(i * theta_per_segment);
+     * round off issues for CNC applications.) Single precision error can accumulate to be greater than
-      sin_Ti = sin(i * theta_per_segment);
+     * tool precision in some cases. Therefore, arc path correction is implemented.
-      r_X = -offset[X_AXIS] * cos_Ti + offset[Y_AXIS] * sin_Ti;
+     *
-      r_Y = -offset[X_AXIS] * sin_Ti - offset[Y_AXIS] * cos_Ti;
+     * Small angle approximation may be used to reduce computation overhead further. This approximation
-      count = 0;
+     * holds for everything, but very small circles and large MM_PER_ARC_SEGMENT values. In other words,
-    }
+     * theta_per_segment would need to be greater than 0.1 rad and N_ARC_CORRECTION would need to be large
+     * to cause an appreciable drift error. N_ARC_CORRECTION~=25 is more than small enough to correct for
+     * numerical drift error. N_ARC_CORRECTION may be on the order a hundred(s) before error becomes an
+     * issue for CNC machines with the single precision Arduino calculations.
+     *
+     * This approximation also allows plan_arc to immediately insert a line segment into the planner
+     * without the initial overhead of computing cos() or sin(). By the time the arc needs to be applied
+     * a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead.
+     * This is important when there are successive arc motions.
+     */
+    // Vector rotation matrix values
+    float cos_T = 1 - 0.5 * theta_per_segment * theta_per_segment; // Small angle approximation
+    float sin_T = theta_per_segment;
 
-    // Update arc_target location
+    float arc_target[NUM_AXIS];
-    arc_target[X_AXIS] = center_X + r_X;
+    float sin_Ti, cos_Ti, r_new_Y;
-    arc_target[Y_AXIS] = center_Y + r_Y;
+    uint16_t i;
-    arc_target[Z_AXIS] += linear_per_segment;
+    int8_t count = 0;
-    arc_target[E_AXIS] += extruder_per_segment;
 
-    clamp_to_software_endstops(arc_target);
+    // Initialize the linear axis
+    arc_target[Z_AXIS] = current_position[Z_AXIS];
 
-    #if ENABLED(DELTA) || ENABLED(SCARA)
+    // Initialize the extruder axis
-      calculate_delta(arc_target);
+    arc_target[E_AXIS] = current_position[E_AXIS];
-      #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+
-        adjust_delta(arc_target);
+    float feed_rate = feedrate * feedrate_multiplier / 60 / 100.0;
+
+    millis_t previous_ms = millis();
+
+    for (i = 1; i < segments; i++) { // Iterate (segments-1) times
+
+      millis_t now = millis();
+      if (now - previous_ms > 200UL) {
+        previous_ms = now;
+        idle();
+      }
+
+      if (++count < N_ARC_CORRECTION) {
+        // Apply vector rotation matrix to previous r_X / 1
+        r_new_Y = r_X * sin_T + r_Y * cos_T;
+        r_X = r_X * cos_T - r_Y * sin_T;
+        r_Y = r_new_Y;
+      }
+      else {
+        // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
+        // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
+        // To reduce stuttering, the sin and cos could be computed at different times.
+        // For now, compute both at the same time.
+        cos_Ti = cos(i * theta_per_segment);
+        sin_Ti = sin(i * theta_per_segment);
+        r_X = -offset[X_AXIS] * cos_Ti + offset[Y_AXIS] * sin_Ti;
+        r_Y = -offset[X_AXIS] * sin_Ti - offset[Y_AXIS] * cos_Ti;
+        count = 0;
+      }
+
+      // Update arc_target location
+      arc_target[X_AXIS] = center_X + r_X;
+      arc_target[Y_AXIS] = center_Y + r_Y;
+      arc_target[Z_AXIS] += linear_per_segment;
+      arc_target[E_AXIS] += extruder_per_segment;
+
+      clamp_to_software_endstops(arc_target);
+
+      #if ENABLED(DELTA) || ENABLED(SCARA)
+        calculate_delta(arc_target);
+        #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+          adjust_delta(arc_target);
+        #endif
+        planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], arc_target[E_AXIS], feed_rate, active_extruder);
+      #else
+        planner.buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], feed_rate, active_extruder);
       #endif
-      planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], arc_target[E_AXIS], feed_rate, active_extruder);
+    }
+
+    // Ensure last segment arrives at target location.
+    #if ENABLED(DELTA) || ENABLED(SCARA)
+      calculate_delta(target);
+      #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+        adjust_delta(target);
+      #endif
+      planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], feed_rate, active_extruder);
     #else
-      planner.buffer_line(arc_target[X_AXIS], arc_target[Y_AXIS], arc_target[Z_AXIS], arc_target[E_AXIS], feed_rate, active_extruder);
+      planner.buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, active_extruder);
     #endif
+
+    // As far as the parser is concerned, the position is now == target. In reality the
+    // motion control system might still be processing the action and the real tool position
+    // in any intermediate location.
+    set_current_to_destination();
   }
-
+#endif
-  // Ensure last segment arrives at target location.
-  #if ENABLED(DELTA) || ENABLED(SCARA)
-    calculate_delta(target);
-    #if ENABLED(AUTO_BED_LEVELING_FEATURE)
-      adjust_delta(target);
-    #endif
-    planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], target[E_AXIS], feed_rate, active_extruder);
-  #else
-    planner.buffer_line(target[X_AXIS], target[Y_AXIS], target[Z_AXIS], target[E_AXIS], feed_rate, active_extruder);
-  #endif
-
-  // As far as the parser is concerned, the position is now == target. In reality the
-  // motion control system might still be processing the action and the real tool position
-  // in any intermediate location.
-  set_current_to_destination();
-}
 
 #if HAS_CONTROLLERFAN
 

Marlin/example_configurations/Felix/Configuration_adv.h

@@ -455,6 +455,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/Hephestos/Configuration_adv.h

@@ -455,6 +455,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/Hephestos_2/Configuration_adv.h

@@ -455,6 +455,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/K8200/Configuration_adv.h

@@ -461,6 +461,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/RigidBot/Configuration_adv.h

@@ -455,6 +455,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/SCARA/Configuration_adv.h

@@ -455,6 +455,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/TAZ4/Configuration_adv.h

@@ -463,6 +463,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/WITBOX/Configuration_adv.h

@@ -455,6 +455,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/delta/biv2.5/Configuration_adv.h

@@ -457,6 +457,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/delta/generic/Configuration_adv.h

@@ -457,6 +457,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h

@@ -456,6 +456,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h

@@ -461,6 +461,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h

@@ -457,6 +457,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/makibox/Configuration_adv.h

@@ -455,6 +455,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 

Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h

@@ -455,6 +455,7 @@
 // @section extras
 
 // Arc interpretation settings:
+#define ARC_SUPPORT  // Disabling this saves ~2660bytes
 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25