From dfe73b5216a3bddc1e696dc337f5fbb5b06a4dc0 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 21 Aug 2021 20:21:23 -0500 Subject: [PATCH] Support 6-axis in G2/G3 --- Marlin/src/core/macros.h | 2 + Marlin/src/gcode/gcode.cpp | 2 +- Marlin/src/gcode/motion/G0_G1.cpp | 2 +- Marlin/src/gcode/motion/G2_G3.cpp | 156 +++++++++++++++++++----------- 4 files changed, 105 insertions(+), 57 deletions(-) diff --git a/Marlin/src/core/macros.h b/Marlin/src/core/macros.h index 86368bf5e7181..0174e21add27a 100644 --- a/Marlin/src/core/macros.h +++ b/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) diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp index 7933c3141a110..94496f2b25766 100644 --- a/Marlin/src/gcode/gcode.cpp +++ b/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 diff --git a/Marlin/src/gcode/motion/G0_G1.cpp b/Marlin/src/gcode/motion/G0_G1.cpp index eb79180c69887..cc6979b74c12a 100644 --- a/Marlin/src/gcode/motion/G0_G1.cpp +++ b/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) { diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp index e0f1208d5c0c8..351f71b56dc73 100644 --- a/Marlin/src/gcode/motion/G2_G3.cpp +++ b/Marlin/src/gcode/motion/G2_G3.cpp @@ -48,6 +48,16 @@ #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 + /** * 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 +69,30 @@ 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 + OPTARG(HAS_Z_AXIS, start_L = current_position[axis_l]) + OPTARG(HAS_I_AXIS, start_I = current_position.i) + OPTARG(HAS_J_AXIS, start_J = current_position.j) + OPTARG(HAS_K_AXIS, start_K = current_position.k) + ; // Angle of rotation between position and target from the circle center. float angular_travel, abs_angular_travel; @@ -87,7 +101,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 +127,63 @@ 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 + CODE_N(SUB2(LINEAR_AXES), + 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 + ); #if HAS_EXTRUDERS - float extruder_travel = cart.e - current_position.e; + float travel_E = cart.e - current_position.e; #endif // 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 + OPTARG(HAS_Z_AXIS, per_circle_L = travel_L * part_per_circle) // L movement per circle + OPTARG(HAS_I_AXIS, per_circle_I = travel_I * part_per_circle) + OPTARG(HAS_J_AXIS, per_circle_J = travel_J * part_per_circle) + OPTARG(HAS_K_AXIS, per_circle_K = travel_K * part_per_circle) + OPTARG(HAS_EXTRUDERS, 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 + CODE_N(SUB2(LINEAR_AXES), // 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 + ); + TERN_(HAS_EXTRUDERS, 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); + CODE_N(SUB2(LINEAR_AXES), + 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 + ); + TERN_(HAS_EXTRUDERS, 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 + && TERN0(HAS_Z_AXIS, travel_L < 0.0001f) + && TERN0(HAS_I_AXIS, travel_I < 0.0001f) + && TERN0(HAS_J_AXIS, travel_J < 0.0001f) + && TERN0(HAS_K_AXIS, 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,18 +238,29 @@ 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; + #if DISABLED(AUTO_BED_LEVELING_UBL) + CODE_N(SUB2(LINEAR_AXES), + 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 + #if HAS_EXTRUDERS - const float extruder_per_segment = proportion * extruder_travel / segments; + const float extruder_per_segment = proportion * travel_E / segments; #endif // 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 all linear axes + CODE_N(SUB2(LINEAR_AXES), + raw[axis_l] = current_position[axis_l], + raw.i = current_position.i, + raw.j = current_position.j, + raw.k = current_position.k + ); // Initialize the extruder axis TERN_(HAS_EXTRUDERS, raw.e = current_position.e); @@ -266,12 +308,14 @@ 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); - #endif - + raw[axis_p] = center_P + rvec.a; + raw[axis_q] = center_Q + rvec.b; + CODE_N(SUB2(LINEAR_AXES), + raw[axis_l] = TERN(AUTO_BED_LEVELING_UBL, start_L, raw[axis_l] + per_segment_L), + raw.i = TERN(AUTO_BED_LEVELING_UBL, start_I, raw.i + per_segment_I), + raw.j = TERN(AUTO_BED_LEVELING_UBL, start_J, raw.j + per_segment_J), + raw.k = TERN(AUTO_BED_LEVELING_UBL, start_K, raw.k + per_segment_K) + ); TERN_(HAS_EXTRUDERS, raw.e += extruder_per_segment); apply_motion_limits(raw); @@ -280,14 +324,16 @@ void plan_arc( 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) + CODE_N(SUB2(LINEAR_AXES), raw[axis_l] = start_L, raw.i = start_I, raw.j = start_J, raw.k = start_K); + #endif apply_motion_limits(raw); @@ -295,11 +341,11 @@ void plan_arc( 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) + CODE_N(SUB2(LINEAR_AXES), 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 +387,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);