Support 6-axis in G2/G3

Marlin/src/core/macros.h

@@ -260,6 +260,7 @@
 #define CODE_3( A,B,C,...) A; B; C
 #define CODE_2( A,B,...) A; B
 #define CODE_1( A,...) A
+#define CODE_0(...)
 #define _CODE_N(N,V...) CODE_##N(V)
 #define CODE_N(N,V...) _CODE_N(N,V)
 
@@ -279,6 +280,7 @@
 #define GANG_3( A,B,C,...) A B C
 #define GANG_2( A,B,...) A B
 #define GANG_1( A,...) A
+#define GANG_0(...)
 #define _GANG_N(N,V...) GANG_##N(V)
 #define GANG_N(N,V...) _GANG_N(N,V)
 #define GANG_N_1(N,K) _GANG_N(N,K,K,K,K,K,K,K,K,K,K,K,K,K,K,K,K)

Marlin/src/gcode/gcode.cpp

@@ -136,7 +136,7 @@ int8_t GcodeSuite::get_target_e_stepper_from_command() {
 }
 
 /**
- * Set XYZE destination and feedrate from the current GCode command
+ * Set XYZIJKE destination and feedrate from the current GCode command
  *
  *  - Set destination from included axis codes
  *  - Set to current for missing axis codes

Marlin/src/gcode/motion/G0_G1.cpp

@@ -71,7 +71,7 @@ void GcodeSuite::G0_G1(TERN_(HAS_FAST_MOVES, const bool fast_move/*=false*/)) {
       #endif
     #endif
 
-    get_destination_from_command();                 // Get X Y Z E F (and set cutter power)
+    get_destination_from_command();                 // Get X Y [Z[I[J[K]]]] [E] F (and set cutter power)
 
     #ifdef G0_FEEDRATE
       if (fast_move) {

Marlin/src/gcode/motion/G2_G3.cpp

@@ -48,6 +48,19 @@
   #define MIN_ARC_SEGMENT_MM MAX_ARC_SEGMENT_MM
 #endif
 
+#if LINEAR_AXES >= 4
+  #define HAS_I_AXIS 1
+#endif
+#if LINEAR_AXES >= 5
+  #define HAS_J_AXIS 1
+#endif
+#if LINEAR_AXES >= 6
+  #define HAS_K_AXIS 1
+#endif
+
+#define ARC_LIJK_CODE(L,I,J,K)    CODE_N(SUB2(LINEAR_AXES),L,I,J,K)
+#define ARC_LIJKE_CODE(L,I,J,K,E) ARC_LIJK_CODE(L,I,J,K); CODE_ITEM_E(E)
+
 /**
  * Plan an arc in 2 dimensions, with linear motion in the other axes.
  * The arc is traced with many small linear segments according to the configuration.
@@ -59,26 +72,32 @@ void plan_arc(
   const uint8_t circles     // Take the scenic route
 ) {
   #if ENABLED(CNC_WORKSPACE_PLANES)
-    AxisEnum p_axis, q_axis, l_axis;
+    AxisEnum axis_p, axis_q, axis_l;
     switch (gcode.workspace_plane) {
       default:
-      case GcodeSuite::PLANE_XY: p_axis = X_AXIS; q_axis = Y_AXIS; l_axis = Z_AXIS; break;
-      case GcodeSuite::PLANE_YZ: p_axis = Y_AXIS; q_axis = Z_AXIS; l_axis = X_AXIS; break;
-      case GcodeSuite::PLANE_ZX: p_axis = Z_AXIS; q_axis = X_AXIS; l_axis = Y_AXIS; break;
+      case GcodeSuite::PLANE_XY: axis_p = X_AXIS; axis_q = Y_AXIS; axis_l = Z_AXIS; break;
+      case GcodeSuite::PLANE_YZ: axis_p = Y_AXIS; axis_q = Z_AXIS; axis_l = X_AXIS; break;
+      case GcodeSuite::PLANE_ZX: axis_p = Z_AXIS; axis_q = X_AXIS; axis_l = Y_AXIS; break;
     }
   #else
-    constexpr AxisEnum p_axis = X_AXIS, q_axis = Y_AXIS OPTARG(HAS_Z_AXIS, l_axis = Z_AXIS);
+    constexpr AxisEnum axis_p = X_AXIS, axis_q = Y_AXIS OPTARG(HAS_Z_AXIS, axis_l = Z_AXIS);
   #endif
 
   // Radius vector from center to current location
   ab_float_t rvec = -offset;
 
   const float radius = HYPOT(rvec.a, rvec.b),
-              center_P = current_position[p_axis] - rvec.a,
-              center_Q = current_position[q_axis] - rvec.b,
-              rt_X = cart[p_axis] - center_P,
-              rt_Y = cart[q_axis] - center_Q
-              OPTARG(HAS_Z_AXIS, start_L = current_position[l_axis]);
+              center_P = current_position[axis_p] - rvec.a,
+              center_Q = current_position[axis_q] - rvec.b,
+              rt_X = cart[axis_p] - center_P,
+              rt_Y = cart[axis_q] - center_Q;
+
+  ARC_LIJK_CODE(
+    const float start_L = current_position[axis_l],
+    const float start_I = current_position.i,
+    const float start_J = current_position.j,
+    const float start_K = current_position.k
+  );
 
   // Angle of rotation between position and target from the circle center.
   float angular_travel, abs_angular_travel;
@@ -87,7 +106,7 @@ void plan_arc(
   uint16_t min_segments = 1;
 
   // Do a full circle if starting and ending positions are "identical"
-  if (NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {
+  if (NEAR(current_position[axis_p], cart[axis_p]) && NEAR(current_position[axis_q], cart[axis_q])) {
     // Preserve direction for circles
     angular_travel = clockwise ? -RADIANS(360) : RADIANS(360);
     abs_angular_travel = RADIANS(360);
@@ -113,46 +132,65 @@ void plan_arc(
     min_segments = CEIL((MIN_CIRCLE_SEGMENTS) * portion_of_circle);     // MinumSegments for the arc
   }
 
-  #if HAS_Z_AXIS
-    float linear_travel = cart[l_axis] - start_L;
-  #endif
-  #if HAS_EXTRUDERS
-    float extruder_travel = cart.e - current_position.e;
-  #endif
+  ARC_LIJKE_CODE(
+    float travel_L = cart[axis_l] - start_L,
+    float travel_I = cart.i - start_I,
+    float travel_J = cart.j - start_J,
+    float travel_K = cart.k - start_K,
+    float travel_E = cart.e - current_position.e
+  );
 
   // If "P" specified circles, call plan_arc recursively then continue with the rest of the arc
   if (TERN0(ARC_P_CIRCLES, circles)) {
-    const float total_angular = abs_angular_travel + circles * RADIANS(360),  // Total rotation with all circles and remainder
-              part_per_circle = RADIANS(360) / total_angular;             // Each circle's part of the total
-
-    #if HAS_Z_AXIS
-      const float l_per_circle = linear_travel * part_per_circle;         // L movement per circle
-    #endif
-    #if HAS_EXTRUDERS
-      const float e_per_circle = extruder_travel * part_per_circle;       // E movement per circle
-    #endif
-
-    xyze_pos_t temp_position = current_position;                          // for plan_arc to compare to current_position
+    const float total_angular = abs_angular_travel + circles * RADIANS(360),    // Total rotation with all circles and remainder
+              part_per_circle = RADIANS(360) / total_angular;                   // Each circle's part of the total
+
+    ARC_LIJKE_CODE(
+      const float per_circle_L = travel_L * part_per_circle,    // L movement per circle
+      const float per_circle_I = travel_I * part_per_circle,
+      const float per_circle_J = travel_J * part_per_circle,
+      const float per_circle_K = travel_K * part_per_circle,
+      const float per_circle_E = travel_E * part_per_circle     // E movement per circle
+    );
+
+    xyze_pos_t temp_position = current_position;
     for (uint16_t n = circles; n--;) {
-      TERN_(HAS_EXTRUDERS, temp_position.e += e_per_circle);              // Destination E axis
-      TERN_(HAS_Z_AXIS, temp_position[l_axis] += l_per_circle);           // Destination L axis
-      plan_arc(temp_position, offset, clockwise, 0);                      // Plan a single whole circle
+      ARC_LIJKE_CODE(                                           // Destination Linear Axes
+        temp_position[axis_l] += per_circle_L,
+        temp_position.i       += per_circle_I,
+        temp_position.j       += per_circle_J,
+        temp_position.k       += per_circle_K,
+        temp_position.e       += per_circle_E                   // Destination E axis
+      );
+      plan_arc(temp_position, offset, clockwise, 0);            // Plan a single whole circle
     }
-    TERN_(HAS_Z_AXIS, linear_travel = cart[l_axis] - current_position[l_axis]);
-    TERN_(HAS_EXTRUDERS, extruder_travel = cart.e - current_position.e);
+    ARC_LIJKE_CODE(
+      travel_L = cart[axis_l] - current_position[axis_l],
+      travel_I = cart.i       - current_position.i,
+      travel_J = cart.j       - current_position.j,
+      travel_K = cart.k       - current_position.k,
+      travel_E = cart.e       - current_position.e
+    );
   }
 
-  const float flat_mm = radius * abs_angular_travel,                      // Millimeters in the arc
-              mm_of_travel = TERN_(HAS_Z_AXIS, linear_travel ? HYPOT(flat_mm, linear_travel) :) flat_mm;  // Real distance according to Pythagoras
+  // Millimeters in the arc, assuming it's flat
+  const float flat_mm = radius * abs_angular_travel;
 
   // Return if the move is near zero
-  if (mm_of_travel < 0.001f) return;
+  if (flat_mm < 0.0001f
+    GANG_N(SUB2(LINEAR_AXES),
+      && travel_L < 0.0001f,
+      && travel_I < 0.0001f,
+      && travel_J < 0.0001f,
+      && travel_K < 0.0001f
+    )
+  ) return;
 
   // Feedrate for the move, scaled by the feedrate multiplier
   const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
 
   // Get the nominal segment length based on settings
-  float nominal_segment_mm = (
+  const float nominal_segment_mm = (
     #if ARC_SEGMENTS_PER_SEC  // Length based on segments per second and feedrate
       constrain(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MIN_ARC_SEGMENT_MM, MAX_ARC_SEGMENT_MM)
     #else
@@ -207,21 +245,28 @@ void plan_arc(
               sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
               cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
 
-  #if HAS_Z_AXIS && DISABLED(AUTO_BED_LEVELING_UBL)
-    const float linear_per_segment = proportion * linear_travel / segments;
-  #endif
-  #if HAS_EXTRUDERS
-    const float extruder_per_segment = proportion * extruder_travel / segments;
+  #if DISABLED(AUTO_BED_LEVELING_UBL)
+    ARC_LIJK_CODE(
+      const float per_segment_L = proportion * travel_L / segments,
+      const float per_segment_I = proportion * travel_I / segments,
+      const float per_segment_J = proportion * travel_J / segments,
+      const float per_segment_K = proportion * travel_K / segments
+    );
   #endif
 
+  CODE_ITEM_E(const float extruder_per_segment = proportion * travel_E / segments);
+
   // For shortened segments, run all but the remainder in the loop
   if (tooshort) segments++;
 
-  // Initialize the linear axis
-  TERN_(HAS_Z_AXIS, raw[l_axis] = current_position[l_axis]);
-
-  // Initialize the extruder axis
-  TERN_(HAS_EXTRUDERS, raw.e = current_position.e);
+  // Initialize all linear axes and E
+  ARC_LIJKE_CODE(
+    raw[axis_l] = current_position[axis_l],
+    raw.i       = current_position.i,
+    raw.j       = current_position.j,
+    raw.k       = current_position.k
+    raw.e       = current_position.e
+  );
 
   #if ENABLED(SCARA_FEEDRATE_SCALING)
     const float inv_duration = scaled_fr_mm_s / segment_mm;
@@ -266,40 +311,43 @@ void plan_arc(
     }
 
     // Update raw location
-    raw[p_axis] = center_P + rvec.a;
-    raw[q_axis] = center_Q + rvec.b;
-    #if HAS_Z_AXIS
-      raw[l_axis] = TERN(AUTO_BED_LEVELING_UBL, start_L, raw[l_axis] + linear_per_segment);
+    raw[axis_p] = center_P + rvec.a;
+    raw[axis_q] = center_Q + rvec.b;
+    #if ENABLED(AUTO_BED_LEVELING_UBL)
+      ARC_LIJK_CODE(raw[axis_l] = start_L, raw.i = start_I, raw.j = start_J, raw.k = start_K);
+    #else
+      ARC_LIJK_CODE(raw[axis_l] += per_segment_L, raw.i += r_segment_I, raw.j += r_segment_J, raw.k += r_segment_K);
     #endif
 
-    TERN_(HAS_EXTRUDERS, raw.e += extruder_per_segment);
+    CODE_ITEM_E(raw.e += extruder_per_segment);
 
     apply_motion_limits(raw);
 
     #if HAS_LEVELING && !PLANNER_LEVELING
       planner.apply_leveling(raw);
     #endif
 
-    if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0
-      OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
-    )) break;
+    if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
+      break;
   }
 
   // Ensure last segment arrives at target location.
   raw = cart;
-  TERN_(AUTO_BED_LEVELING_UBL, TERN_(HAS_Z_AXIS, raw[l_axis] = start_L));
+  #if ENABLED(AUTO_BED_LEVELING_UBL)
+    ARC_LIJK_CODE(raw[axis_l] = start_L, raw.i = start_I, raw.j = start_J, raw.k = start_K);
+  #endif
 
   apply_motion_limits(raw);
 
   #if HAS_LEVELING && !PLANNER_LEVELING
     planner.apply_leveling(raw);
   #endif
 
-  planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0
-    OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
-  );
+  planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration));
 
-  TERN_(AUTO_BED_LEVELING_UBL, TERN_(HAS_Z_AXIS, raw[l_axis] = start_L));
+  #if ENABLED(AUTO_BED_LEVELING_UBL)
+    ARC_LIJK_CODE(raw[axis_l] = start_L, raw.i = start_I, raw.j = start_J, raw.k = start_K);
+  #endif
   current_position = raw;
 
 } // plan_arc
@@ -341,7 +389,7 @@ void GcodeSuite::G2_G3(const bool clockwise) {
       relative_mode = true;
     #endif
 
-    get_destination_from_command();   // Get X Y Z E F (and set cutter power)
+    get_destination_from_command();   // Get X Y [Z[I[J[K]]]] [E] F (and set cutter power)
 
     TERN_(SF_ARC_FIX, relative_mode = relative_mode_backup);