diff options
Diffstat (limited to 'Marlin/src/gcode/motion/G2_G3.cpp')
| -rw-r--r-- | Marlin/src/gcode/motion/G2_G3.cpp | 370 |
1 files changed, 370 insertions, 0 deletions
diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp new file mode 100644 index 0000000..61e5024 --- /dev/null +++ b/Marlin/src/gcode/motion/G2_G3.cpp @@ -0,0 +1,370 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(ARC_SUPPORT) + +#include "../gcode.h" +#include "../../module/motion.h" +#include "../../module/planner.h" +#include "../../module/temperature.h" + +#if ENABLED(DELTA) + #include "../../module/delta.h" +#elif ENABLED(SCARA) + #include "../../module/scara.h" +#endif + +#if N_ARC_CORRECTION < 1 + #undef N_ARC_CORRECTION + #define N_ARC_CORRECTION 1 +#endif + +/** + * Plan an arc in 2 dimensions, with optional linear motion in a 3rd dimension + * + * The arc is traced by generating many small linear segments, as configured by + * MM_PER_ARC_SEGMENT (Default 1mm). In the future we hope more slicers will include + * an option to generate G2/G3 arcs for curved surfaces, as this will allow faster + * boards to produce much smoother curved surfaces. + */ +void plan_arc( + const xyze_pos_t &cart, // Destination position + const ab_float_t &offset, // Center of rotation relative to current_position + const bool clockwise, // Clockwise? + const uint8_t circles // Take the scenic route +) { + #if ENABLED(CNC_WORKSPACE_PLANES) + AxisEnum p_axis, q_axis, l_axis; + 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; + } + #else + constexpr AxisEnum p_axis = X_AXIS, q_axis = Y_AXIS, l_axis = 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, + start_L = current_position[l_axis]; + + #ifdef MIN_ARC_SEGMENTS + uint16_t min_segments = MIN_ARC_SEGMENTS; + #else + constexpr uint16_t min_segments = 1; + #endif + + // Angle of rotation between position and target from the circle center. + float angular_travel; + + // 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])) { + // Preserve direction for circles + angular_travel = clockwise ? -RADIANS(360) : RADIANS(360); + } + else { + // Calculate the angle + angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y); + + // Angular travel too small to detect? Just return. + if (!angular_travel) return; + + // Make sure angular travel over 180 degrees goes the other way around. + switch (((angular_travel < 0) << 1) | clockwise) { + case 1: angular_travel -= RADIANS(360); break; // Positive but CW? Reverse direction. + case 2: angular_travel += RADIANS(360); break; // Negative but CCW? Reverse direction. + } + + #ifdef MIN_ARC_SEGMENTS + min_segments = CEIL(min_segments * ABS(angular_travel) / RADIANS(360)); + NOLESS(min_segments, 1U); + #endif + } + + float linear_travel = cart[l_axis] - start_L, + extruder_travel = cart.e - current_position.e; + + // If circling around... + if (ENABLED(ARC_P_CIRCLES) && circles) { + const float total_angular = 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 + l_per_circle = linear_travel * part_per_circle, // L movement per circle + e_per_circle = extruder_travel * part_per_circle; // E movement per circle + xyze_pos_t temp_position = current_position; // for plan_arc to compare to current_position + for (uint16_t n = circles; n--;) { + temp_position.e += e_per_circle; // Destination E axis + temp_position[l_axis] += l_per_circle; // Destination L axis + plan_arc(temp_position, offset, clockwise, 0); // Plan a single whole circle + } + linear_travel = cart[l_axis] - current_position[l_axis]; + extruder_travel = cart.e - current_position.e; + } + + const float flat_mm = radius * angular_travel, + mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm); + if (mm_of_travel < 0.001f) return; + + const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s); + + // Start with a nominal segment length + float seg_length = ( + #ifdef ARC_SEGMENTS_PER_R + constrain(MM_PER_ARC_SEGMENT * radius, MM_PER_ARC_SEGMENT, ARC_SEGMENTS_PER_R) + #elif ARC_SEGMENTS_PER_SEC + _MAX(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MM_PER_ARC_SEGMENT) + #else + MM_PER_ARC_SEGMENT + #endif + ); + // Divide total travel by nominal segment length + uint16_t segments = FLOOR(mm_of_travel / seg_length); + NOLESS(segments, min_segments); // At least some segments + seg_length = mm_of_travel / segments; + + /** + * Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, + * 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 + * defined from the circle center to the initial position. Each line segment is formed by successive + * vector rotations. This requires only two cos() and sin() computations to form the rotation + * matrix for the duration of the entire arc. Error may accumulate from numerical round-off, since + * all double numbers are single precision on the Arduino. (True double precision will not have + * 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 + xyze_pos_t raw; + const float theta_per_segment = angular_travel / segments, + linear_per_segment = linear_travel / segments, + extruder_per_segment = extruder_travel / segments, + sq_theta_per_segment = sq(theta_per_segment), + 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 + + // Initialize the linear axis + raw[l_axis] = current_position[l_axis]; + + // Initialize the extruder axis + raw.e = current_position.e; + + #if ENABLED(SCARA_FEEDRATE_SCALING) + const float inv_duration = scaled_fr_mm_s / seg_length; + #endif + + millis_t next_idle_ms = millis() + 200UL; + + #if N_ARC_CORRECTION > 1 + int8_t arc_recalc_count = N_ARC_CORRECTION; + #endif + + for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times + + thermalManager.manage_heater(); + if (ELAPSED(millis(), next_idle_ms)) { + next_idle_ms = millis() + 200UL; + idle(); + } + + #if N_ARC_CORRECTION > 1 + if (--arc_recalc_count) { + // Apply vector rotation matrix to previous rvec.a / 1 + const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T; + rvec.a = rvec.a * cos_T - rvec.b * sin_T; + rvec.b = r_new_Y; + } + else + #endif + { + #if N_ARC_CORRECTION > 1 + arc_recalc_count = N_ARC_CORRECTION; + #endif + + // 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. + const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment); + rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti; + rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti; + } + + // Update raw location + raw[p_axis] = center_P + rvec.a; + raw[q_axis] = center_Q + rvec.b; + #if ENABLED(AUTO_BED_LEVELING_UBL) + raw[l_axis] = start_L; + UNUSED(linear_per_segment); + #else + raw[l_axis] += linear_per_segment; + #endif + 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 + #if ENABLED(SCARA_FEEDRATE_SCALING) + , inv_duration + #endif + )) break; + } + + // Ensure last segment arrives at target location. + raw = cart; + TERN_(AUTO_BED_LEVELING_UBL, raw[l_axis] = start_L); + + 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 + #if ENABLED(SCARA_FEEDRATE_SCALING) + , inv_duration + #endif + ); + + TERN_(AUTO_BED_LEVELING_UBL, raw[l_axis] = start_L); + current_position = raw; + +} // plan_arc + +/** + * G2: Clockwise Arc + * G3: Counterclockwise Arc + * + * This command has two forms: IJ-form (JK, KI) and R-form. + * + * - Depending on the current Workspace Plane orientation, + * use parameters IJ/JK/KI to specify the XY/YZ/ZX offsets. + * At least one of the IJ/JK/KI parameters is required. + * XY/YZ/ZX can be omitted to do a complete circle. + * The given XY/YZ/ZX is not error-checked. The arc ends + * based on the angle of the destination. + * Mixing IJ/JK/KI with R will throw an error. + * + * - R specifies the radius. X or Y (Y or Z / Z or X) is required. + * Omitting both XY/YZ/ZX will throw an error. + * XY/YZ/ZX must differ from the current XY/YZ/ZX. + * Mixing R with IJ/JK/KI will throw an error. + * + * - P specifies the number of full circles to do + * before the specified arc move. + * + * Examples: + * + * G2 I10 ; CW circle centered at X+10 + * G3 X20 Y12 R14 ; CCW circle with r=14 ending at X20 Y12 + */ +void GcodeSuite::G2_G3(const bool clockwise) { + if (MOTION_CONDITIONS) { + + #if ENABLED(SF_ARC_FIX) + const bool relative_mode_backup = relative_mode; + relative_mode = true; + #endif + + get_destination_from_command(); // Get X Y Z E F (and set cutter power) + + TERN_(SF_ARC_FIX, relative_mode = relative_mode_backup); + + ab_float_t arc_offset = { 0, 0 }; + if (parser.seenval('R')) { + const float r = parser.value_linear_units(); + if (r) { + const xy_pos_t p1 = current_position, p2 = destination; + if (p1 != p2) { + const xy_pos_t d2 = (p2 - p1) * 0.5f; // XY vector to midpoint of move from current + const float e = clockwise ^ (r < 0) ? -1 : 1, // clockwise -1/1, counterclockwise 1/-1 + len = d2.magnitude(), // Distance to mid-point of move from current + h2 = (r - len) * (r + len), // factored to reduce rounding error + h = (h2 >= 0) ? SQRT(h2) : 0.0f; // Distance to the arc pivot-point from midpoint + const xy_pos_t s = { -d2.y, d2.x }; // Perpendicular bisector. (Divide by len for unit vector.) + arc_offset = d2 + s / len * e * h; // The calculated offset (mid-point if |r| <= len) + } + } + } + else { + #if ENABLED(CNC_WORKSPACE_PLANES) + char achar, bchar; + switch (gcode.workspace_plane) { + default: + case GcodeSuite::PLANE_XY: achar = 'I'; bchar = 'J'; break; + case GcodeSuite::PLANE_YZ: achar = 'J'; bchar = 'K'; break; + case GcodeSuite::PLANE_ZX: achar = 'K'; bchar = 'I'; break; + } + #else + constexpr char achar = 'I', bchar = 'J'; + #endif + if (parser.seenval(achar)) arc_offset.a = parser.value_linear_units(); + if (parser.seenval(bchar)) arc_offset.b = parser.value_linear_units(); + } + + if (arc_offset) { + + #if ENABLED(ARC_P_CIRCLES) + // P indicates number of circles to do + const int8_t circles_to_do = parser.byteval('P'); + if (!WITHIN(circles_to_do, 0, 100)) + SERIAL_ERROR_MSG(STR_ERR_ARC_ARGS); + #else + constexpr uint8_t circles_to_do = 0; + #endif + + // Send the arc to the planner + plan_arc(destination, arc_offset, clockwise, circles_to_do); + reset_stepper_timeout(); + } + else + SERIAL_ERROR_MSG(STR_ERR_ARC_ARGS); + } +} + +#endif // ARC_SUPPORT |