//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Arc Welder: Inverse Processor (firmware simulator). // Please see the copyright notices in the function definitions // // Converts G2/G3(arc) commands back to G0/G1 commands. Intended to test firmware changes to improve arc support. // This reduces file size and the number of gcodes per second. // // Based on arc interpolation implementations from: // Marlin 1.x (see https://github.com/MarlinFirmware/Marlin/blob/1.0.x/LICENSE for the current license) // Marlin 2.x (see https://github.com/MarlinFirmware/Marlin/blob/2.0.x/LICENSE for the current license) // Prusa-Firmware (see https://github.com/prusa3d/Prusa-Firmware/blob/MK3/LICENSE for the current license) // Smoothieware (see https://github.com/Smoothieware/Smoothieware for the current license) // Repetier (see https://github.com/repetier/Repetier-Firmware for the current license) // // Built using the 'Arc Welder: Anti Stutter' library // // Copyright(C) 2021 - Brad Hochgesang //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // This program is free software : you can redistribute it and/or modify // it under the terms of the GNU Affero 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 Affero General Public License for more details. // // // You can contact the author at the following email address: // FormerLurker@pm.me //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// #include "marlin_2.h" #include "utilities.h" marlin_2::marlin_2(firmware_arguments args) : firmware(args) { feedrate_mm_s = 0; current_position = new float[MARLIN_2_XYZE]; apply_arguments(); }; marlin_2::~marlin_2() { delete current_position; } void marlin_2::apply_arguments() { static const std::vector marlin_2_firmware_version_names{ "2.0.9.1", "2.0.9.2" }; set_versions(marlin_2_firmware_version_names, "2.0.9.1"); marlin_2_version_ = (marlin_2::marlin_2_firmware_versions)version_index_; std::vector used_arguments; switch (marlin_2_version_) { case marlin_2::marlin_2_firmware_versions::V2_0_9_2: used_arguments = { FIRMWARE_ARGUMENT_MIN_ARC_SEGMENT_MM, FIRMWARE_ARGUMENT_MAX_ARC_SEGMENT_MM, FIRMWARE_ARGUMENT_MIN_CIRCLE_SEGMENTS, FIRMWARE_ARGUMENT_ARC_SEGMENTS_PER_SEC, FIRMWARE_ARGUMENT_N_ARC_CORRECTION, FIRMWARE_ARGUMENT_G90_G91_INFLUENCES_EXTRUDER }; plan_arc_ = &marlin_2::plan_arc_2_0_9_2; break; default: used_arguments = { FIRMWARE_ARGUMENT_MM_PER_ARC_SEGMENT, FIRMWARE_ARGUMENT_ARC_SEGMENT_PER_R, FIRMWARE_ARGUMENT_MIN_ARC_SEGMENTS, FIRMWARE_ARGUMENT_ARC_SEGMENTS_PER_SEC, FIRMWARE_ARGUMENT_N_ARC_CORRECTION, FIRMWARE_ARGUMENT_G90_G91_INFLUENCES_EXTRUDER }; plan_arc_ = &marlin_2::plan_arc_2_0_9_1; break; } args_.set_used_arguments(used_arguments); } firmware_arguments marlin_2::get_default_arguments_for_current_version() const { // Start off with the current args so they are set up correctly for this firmware type and version firmware_arguments default_args = args_; // firmware defaults default_args.g90_g91_influences_extruder = true; switch (marlin_2_version_) { case marlin_2::marlin_2_firmware_versions::V2_0_9_2: // Active Settings default_args.min_arc_segment_mm = 0.1f; default_args.max_arc_segment_mm = 1.0f; default_args.min_circle_segments = 72; default_args.n_arc_correction = 25; // Inactive Settings default_args.arc_segments_per_r = 0; default_args.arc_segments_per_sec = 0; break; default: // Active Settings default_args.mm_per_arc_segment = 1.0f; default_args.min_arc_segments = 24; default_args.n_arc_correction = 25; // Inactive Settings default_args.arc_segments_per_r = 0; default_args.arc_segments_per_sec = 0; break; } return default_args; } std::string marlin_2::interpolate_arc(firmware_position& target, double i, double j, double r, bool is_clockwise) { // Clear the current list of gcodes gcodes_.clear(); // Setup the current position current_position[X_AXIS] = static_cast(position_.x); current_position[Y_AXIS] = static_cast(position_.y); current_position[Z_AXIS] = static_cast(position_.z); current_position[E_AXIS] = static_cast(position_.e); float marlin_target[MARLIN_2_XYZE]; marlin_target[X_AXIS] = static_cast(target.x); marlin_target[Y_AXIS] = static_cast(target.y); marlin_target[Z_AXIS] = static_cast(target.z); marlin_target[E_AXIS] = static_cast(target.e); float marlin_offset[2]; marlin_offset[0] = static_cast(i); marlin_offset[1] = static_cast(j); // TODO: handle R form!! // Set the feedrate feedrate_mm_s = static_cast(target.f); uint8_t marlin_isclockwise = is_clockwise ? 1 : 0; (this->*plan_arc_)(marlin_target, marlin_offset, marlin_isclockwise, 0); return gcodes_; } /// /// This function was adapted from the 2.0.9.1 release of Marlin firmware, which can be found at the following link: /// https://github.com/MarlinFirmware/Marlin/blob/b878127ea04cc72334eb35ce0dca39ccf7d73a68/Marlin/src/gcode/motion/G2_G3.cpp /// Copyright Notice found on that page: /// /// /// Marlin 3D Printer Firmware /// Copyright (C) 2016, 2017 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 . /// /// The target position /// The I and J offset /// Is the motion clockwise or counterclockwise void marlin_2::plan_arc_2_0_9_1( const float(&cart)[MARLIN_2_XYZE], // Destination position const float(&offset)[2], // Center of rotation relative to current_position const bool clockwise, // Clockwise? const uint8_t circles // Take the scenic route ) { uint8_t p_axis = X_AXIS, q_axis = Y_AXIS, l_axis = Z_AXIS; // Radius vector from center to current location float rvec[2]; rvec[0] = - offset[X_AXIS]; rvec[1] = - offset[Y_AXIS]; const float radius = utilities::hypotf(rvec[0], rvec[1]), center_P = current_position[p_axis] - rvec[0], center_Q = current_position[q_axis] - rvec[1], rt_X = cart[p_axis] - center_P, rt_Y = cart[q_axis] - center_Q, start_L = current_position[l_axis]; uint16_t min_segments = args_.min_arc_segments > 0 ? args_.min_arc_segments : 1; // 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 ? -utilities::radiansf(360.0f) : utilities::radiansf(360.0f); } else { // Calculate the angle angular_travel = utilities::atan2f(rvec[0] * rt_Y - rvec[1] * rt_X, rvec[0] * rt_X + rvec[1] * 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) | (int)clockwise) { case 1: angular_travel -= utilities::radiansf(360.0f); break; // Positive but CW? Reverse direction. case 2: angular_travel += utilities::radiansf(360.0f); break; // Negative but CCW? Reverse direction. } if (args_.min_arc_segments > 1) { min_segments = (uint16_t)utilities::ceilf(min_segments * utilities::absf(angular_travel) / utilities::radiansf(360.0f)); NOLESS(min_segments, 1U); } } float linear_travel = cart[Z_AXIS] - start_L; float extruder_travel = cart[E_AXIS] - current_position[E_AXIS]; const float flat_mm = radius * angular_travel, mm_of_travel = linear_travel ? utilities::hypotf(flat_mm, linear_travel) : utilities::absf(flat_mm); if (mm_of_travel < 0.001f) return; const float scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s); // Start with a nominal segment length float seg_length = (float)args_.mm_per_arc_segment; if (args_.arc_segments_per_r > 0) { seg_length = utilities::constrainf((float)args_.mm_per_arc_segment * radius, (float)args_.mm_per_arc_segment, (float)args_.arc_segments_per_r); } else if (args_.arc_segments_per_sec > 0) { seg_length = utilities::maxf(scaled_fr_mm_s * utilities::reciprocalf((float)args_.arc_segments_per_sec), (float)args_.mm_per_arc_segment); } // Divide total travel by nominal segment length uint16_t segments = (uint16_t)utilities::floorf(mm_of_travel / seg_length); //uint16_t segments = utilities::floorf(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 float raw[MARLIN_2_XYZE]; const float theta_per_segment = angular_travel / segments, sq_theta_per_segment = utilities::sqf(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 const float linear_per_segment = linear_travel / segments; const float extruder_per_segment = extruder_travel / segments; // Initialize the linear axis raw[l_axis] = current_position[l_axis]; // Initialize the extruder axis raw[E_AXIS] = current_position[E_AXIS]; int8_t arc_recalc_count = args_.n_arc_correction; for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times if (args_.n_arc_correction > 1 && --arc_recalc_count) { // Apply vector rotation matrix to previous rvec[0] / 1 const float r_new_Y = rvec[0] * sin_T + rvec[1] * cos_T; rvec[0] = rvec[0] * cos_T - rvec[1] * sin_T; rvec[1] = r_new_Y; } else { if (args_.n_arc_correction > 1) { arc_recalc_count = args_.n_arc_correction; } // 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 = (float)utilities::cos((i * (double)theta_per_segment)); const float sin_Ti = (float)utilities::sin((i * (double)theta_per_segment)); rvec[0] = -offset[0] * cos_Ti + offset[1] * sin_Ti; rvec[1] = -offset[0] * sin_Ti - offset[1] * cos_Ti; } // Update raw location raw[p_axis] = center_P + rvec[0]; raw[q_axis] = center_Q + rvec[1]; raw[l_axis] = start_L, raw[l_axis] + linear_per_segment; raw[E_AXIS] += extruder_per_segment; apply_motion_limits(raw); if (!buffer_line(raw, scaled_fr_mm_s, 0)) { break; } } // Ensure last segment arrives at target location. COPY(raw, cart); raw[l_axis] = start_L; apply_motion_limits(raw); buffer_line(raw, scaled_fr_mm_s, 0); raw[l_axis] = start_L; COPY(current_position, raw); } /// /// This function was adapted from the 2.0.9.2 release of Marlin firmware, which can be found at the following link: /// https://github.com/MarlinFirmware/Marlin/blob/b878127ea04cc72334eb35ce0dca39ccf7d73a68/Marlin/src/gcode/motion/G2_G3.cpp /// Copyright Notice found on that page: /// /// /// Marlin 3D Printer Firmware /// Copyright (C) 2016, 2017 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 . /// /// The target position /// The I and J offset /// Is the motion clockwise or counterclockwise void marlin_2::plan_arc_2_0_9_2( const float(&cart)[MARLIN_2_XYZE], // Destination position const float(&offset)[2], // Center of rotation relative to current_position const bool clockwise, // Clockwise? const uint8_t circles // Take the scenic route ) { int min_circle_segments = args_.min_circle_segments > 0 ? args_.min_circle_segments : 1; uint8_t p_axis = X_AXIS, q_axis = Y_AXIS, l_axis = Z_AXIS; // Radius vector from center to current location float rvec[2]; rvec[0] = -offset[X_AXIS]; rvec[1] = -offset[Y_AXIS]; const float radius = utilities::hypotf(rvec[0], rvec[1]), center_P = current_position[p_axis] - rvec[0], center_Q = current_position[q_axis] - rvec[1], rt_X = cart[p_axis] - center_P, rt_Y = cart[q_axis] - center_Q, start_L = current_position[l_axis]; uint16_t min_segments = args_.min_arc_segments > 0 ? args_.min_arc_segments : 1; // Angle of rotation between position and target from the circle center. float angular_travel, abs_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 ? -utilities::radiansf(360.0f) : utilities::radiansf(360.0f); abs_angular_travel = utilities::radiansf(360.0f); min_segments = min_circle_segments; } else { // Calculate the angle angular_travel = utilities::atan2f(rvec[0] * rt_Y - rvec[1] * rt_X, rvec[0] * rt_X + rvec[1] * 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) | (int)clockwise) { case 1: angular_travel -= utilities::radiansf(360.0f); break; // Positive but CW? Reverse direction. case 2: angular_travel += utilities::radiansf(360.0f); break; // Negative but CCW? Reverse direction. } abs_angular_travel = utilities::absf(angular_travel); // Apply minimum segments to the arc const float portion_of_circle = abs_angular_travel / utilities::radiansf(360.0f); // Portion of a complete circle (0 < N < 1) min_segments = (uint16_t)utilities::ceilf((min_circle_segments)*portion_of_circle); // Minimum segments for the arc } float travel_L = cart[Z_AXIS] - start_L; float travel_E = cart[E_AXIS] - current_position[E_AXIS]; // Millimeters in the arc, assuming it's flat const float flat_mm = radius * abs_angular_travel; if (flat_mm < 0.001f && travel_L < 0.001f ) return; // Feedrate for the move, scaled by the feedrate multiplier const float scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s); // Get the nominal segment length based on settings float nominal_segment_mm; if (args_.arc_segments_per_sec > 0) { nominal_segment_mm = utilities::constrainf(scaled_fr_mm_s * utilities::reciprocalf((float)args_.arc_segments_per_sec), (float)args_.min_arc_segment_mm, (float)args_.max_arc_segment_mm); } else { nominal_segment_mm = (float)args_.max_arc_segment_mm; } // Number of whole segments based on the nominal segment length const float nominal_segments = utilities::maxf(utilities::floorf(flat_mm / nominal_segment_mm), min_segments); // A new segment length based on the required minimum const float segment_mm = utilities::constrainf(flat_mm / nominal_segments, (float)args_.min_arc_segment_mm, (float)args_.max_arc_segment_mm); // The number of whole segments in the arc, ignoring the remainder uint16_t segments = (uint16_t)utilities::floorf(flat_mm / segment_mm); // Are the segments now too few to reach the destination? const float segmented_length = segment_mm * segments; const bool tooshort = segmented_length < flat_mm - 0.0001f; const float proportion = tooshort ? segmented_length / flat_mm : 1.0f; /** * 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 float raw[MARLIN_2_XYZE]; const float theta_per_segment = proportion * angular_travel / segments, sq_theta_per_segment = utilities::sqf(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 const float per_segment_L = proportion * travel_L / segments; 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 raw[l_axis] = current_position[l_axis]; // Initialize the extruder axis raw[E_AXIS] = current_position[E_AXIS]; int8_t arc_recalc_count = args_.n_arc_correction; for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times if (args_.n_arc_correction > 1 && --arc_recalc_count) { // Apply vector rotation matrix to previous rvec[0] / 1 const float r_new_Y = rvec[0] * sin_T + rvec[1] * cos_T; rvec[0] = rvec[0] * cos_T - rvec[1] * sin_T; rvec[1] = r_new_Y; } else { if (args_.n_arc_correction > 1) { arc_recalc_count = args_.n_arc_correction; } // 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 = (float)utilities::cos(i * (double)theta_per_segment); const float sin_Ti = (float)utilities::sin(i * (double)theta_per_segment); rvec[0] = -offset[0] * cos_Ti + offset[1] * sin_Ti; rvec[1] = -offset[0] * sin_Ti - offset[1] * cos_Ti; } // Update raw location raw[p_axis] = center_P + rvec[0]; raw[q_axis] = center_Q + rvec[1]; raw[l_axis] = start_L, raw[l_axis] + per_segment_L; raw[E_AXIS] += extruder_per_segment; apply_motion_limits(raw); if (!buffer_line(raw, scaled_fr_mm_s, 0)) { break; } } // Ensure last segment arrives at target location. COPY(raw, cart); raw[l_axis] = start_L; apply_motion_limits(raw); buffer_line(raw, scaled_fr_mm_s, 0); raw[l_axis] = start_L; COPY(current_position, raw); } // Marlin Function Defs void marlin_2::NOLESS(uint16_t& x, uint16_t y) { if (x < y) x = y; } float marlin_2::MMS_SCALED(float x) { // No scaling return x; } bool marlin_2::NEAR_ZERO(float x) { return utilities::withinf(x, -0.000001f, 0.000001f); } bool marlin_2::NEAR(float x, float y) { return NEAR_ZERO((x)-(y)); } void marlin_2::COPY(float target[MARLIN_2_XYZE], const float(&source)[MARLIN_2_XYZE]) { // This is a slow copy, but speed isn't much of an issue here. for (int i = 0; i < MARLIN_2_XYZE; i++) { target[i] = source[i]; } } void marlin_2::apply_motion_limits(float (&pos)[MARLIN_2_XYZE]) { // do nothing return; } //void marlin::buffer_line_kinematic(float x, float y, float z, const float& e, float feed_rate, uint8_t extruder, const float* gcode_target) bool marlin_2::buffer_line(const float(&cart)[MARLIN_2_XYZE], double fr_mm_s, int active_extruder) { // create the target position firmware_position target; target.x = cart[AxisEnum::X_AXIS]; target.y = cart[AxisEnum::Y_AXIS]; target.z = cart[AxisEnum::Z_AXIS]; target.e = cart[AxisEnum::E_AXIS]; target.f = fr_mm_s; if (gcodes_.size() > 0) { gcodes_ += "\n"; } // Generate the gcode gcodes_ += g1_command(target); return true; }