diff options
Diffstat (limited to 'ArcWelderInverseProcessor/repetier.cpp')
| -rw-r--r-- | ArcWelderInverseProcessor/repetier.cpp | 476 |
1 files changed, 476 insertions, 0 deletions
diff --git a/ArcWelderInverseProcessor/repetier.cpp b/ArcWelderInverseProcessor/repetier.cpp new file mode 100644 index 0000000..7d5798c --- /dev/null +++ b/ArcWelderInverseProcessor/repetier.cpp @@ -0,0 +1,476 @@ +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Arc Welder: Repetier arc interpolation simulator. Please see the copyright notices in the function definitions +// starting with plan_arc_ for the original license. +// +// 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. +// +// 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 "repetier.h" +#include "utilities.h" +repetier::repetier(firmware_arguments args) : firmware(args) { + + feedrate = 0; + apply_arguments(); +}; + +void repetier::apply_arguments() +{ + static const std::vector<std::string> repetier_firmware_version_names{ "1.0.4", "1.0.5" }; + set_versions(repetier_firmware_version_names, "1.0.4"); + repetier_version_ = (repetier::repetier_firmware_versions)version_index_; + std::vector<std::string> used_arguments; + switch (repetier_version_) + { + case repetier::repetier_firmware_versions::V1_0_5: + used_arguments = { "mm_per_arc_segment", "n_arc_correction", "g90_g91_influences_extruder" }; + arc_ = &repetier::arc_1_0_5; + break; + default: + used_arguments = { "mm_per_arc_segment", "n_arc_correction", "g90_g91_influences_extruder" }; + arc_ = &repetier::arc_1_0_4; + break; + } + + args_.set_used_arguments(used_arguments); + +} +firmware_arguments repetier::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 = false; + // Leave the switch in here in case we want to add more versions. + switch (repetier_version_) + { + case repetier::repetier_firmware_versions::V1_0_5: + // Active Settings + default_args.mm_per_arc_segment = 1.0f; + default_args.n_arc_correction = 25; + default_args.min_arc_segments = 24; + // Inactive Settings + default_args.min_arc_segments = 0; + default_args.arc_segments_per_r = 0; + default_args.min_mm_per_arc_segment = 0; + default_args.arc_segments_per_sec = 0; + // Settings that do not apply + default_args.mm_max_arc_error = 0; + break; + default: + // Active Settings + default_args.mm_per_arc_segment = 1.0f; + default_args.n_arc_correction = 25; + // Inactive Settings + default_args.min_arc_segments = 0; + default_args.arc_segments_per_r = 0; + default_args.min_mm_per_arc_segment = 0; + default_args.arc_segments_per_sec = 0; + // Settings that do not apply + default_args.mm_max_arc_error = 0; + break; + } + return default_args; +} + +repetier::~repetier() +{ +} + +std::string repetier::interpolate_arc(firmware_position& target, double i, double j, double r, bool is_clockwise) +{ + // Clear the current list of gcodes + gcodes_.clear(); + // Set up the necessary values to call mc_arc + float repetier_position[4]; + repetier_position[X_AXIS] = static_cast<float>(position_.x); + repetier_position[Y_AXIS] = static_cast<float>(position_.y); + repetier_position[Z_AXIS] = static_cast<float>(position_.z); + repetier_position[E_AXIS] = static_cast<float>(position_.e); + float repetier_target[4]; + repetier_target[X_AXIS] = static_cast<float>(target.x); + repetier_target[Y_AXIS] = static_cast<float>(target.y); + repetier_target[Z_AXIS] = static_cast<float>(target.z); + repetier_target[E_AXIS] = static_cast<float>(target.e); + float repetier_offset[2]; + repetier_offset[0] = static_cast<float>(i); + repetier_offset[1] = static_cast<float>(j); + float repetier_radius = static_cast<float>(r); + if (repetier_radius != 0) + { + repetier_radius = (float)utilities::hypot(repetier_offset[X_AXIS], repetier_offset[Y_AXIS]); // Compute arc radius for mc_arc + } + float repetier_f = static_cast<float>(target.f); + + uint8_t repetier_isclockwise = is_clockwise ? 1 : 0; + + feedrate = repetier_f; + (this->*arc_)(repetier_position, repetier_target, repetier_offset, repetier_radius, repetier_isclockwise); + + return gcodes_; +} + +/// <summary> +/// This is intended as a possible replacement for the arc function in 1.0.4 (1.0.5?), which can be found at the following link: +/// https://github.com/repetier/Repetier-Firmware/blob/2bbda51eb6407faf29a09987fd635c86818d32db/src/ArduinoAVR/Repetier/motion.cpp +/// This copyright notice was taken from the link above: +/// +/// This file is part of Repetier-Firmware. +/// Repetier-Firmware 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. +/// Repetier-Firmware 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 Repetier-Firmware. If not, see <http://www.gnu.org/licenses/>. +/// This firmware is a nearly complete rewrite of the sprinter firmware +/// by kliment (https://github.com/kliment/Sprinter) +/// which based on Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware. +/// Functions in this file are used to communicate using ascii or repetier protocol. +/// </summary> +/// <param name="cart">The target position</param> +/// <param name="offset">The I and J offset</param> +/// <param name="clockwise">Is the motion clockwise or counterclockwise</param> +void repetier::arc_1_0_5(float* position, float* target, float* offset, float radius, uint8_t isclockwise) +{ + // int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); + // plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc + float center_axis0 = position[X_AXIS] + offset[X_AXIS]; + float center_axis1 = position[Y_AXIS] + offset[Y_AXIS]; + float linear_travel = target[Z_AXIS] - position[Z_AXIS]; + float extruder_travel = (target[E_AXIS] - position[E_AXIS]); // * Printer::invAxisStepsPerMM[E_AXIS]; -- Not sure what this does... + float r_axis0 = -offset[0]; // Radius vector from center to current location + float r_axis1 = -offset[1]; + float rt_axis0 = target[0] - center_axis0; + float rt_axis1 = target[1] - center_axis1; + + float angular_travel; + // First determine if we have a full circle by checking to see if the start and ending + // XY position is the same before and after the arc is drawn + //if (position[X_AXIS] == target[X_AXIS] && position[Y_AXIS] == target[Y_AXIS]) + if (repetier_is_close(position[X_AXIS], target[X_AXIS]) && repetier_is_close(position[Y_AXIS], target[Y_AXIS])) + { + // Preserve direction for circles + angular_travel = isclockwise ? -2.0f * (float)M_PI : 2.0f * (float)M_PI; + } + else + { + // CCW angle between position and target from circle center. Only one atan2() trig computation required. + angular_travel = (float)utilities::atan2((double)(r_axis0 * rt_axis1) - (double)(r_axis1 * rt_axis0), (double)(r_axis0 * rt_axis0) + (double)(r_axis1 * rt_axis1)); + + // No need to draw an arc if there is no angular travel + if (!angular_travel) return; + + // Make sure angular travel over 180 degrees goes the other way around. + if (angular_travel > 0) + { + if (isclockwise) + { + angular_travel -= 2.0f * (float)M_PI; + } + } + else if (!isclockwise) + { + angular_travel += 2.0f * (float)M_PI; + } + } + + // Determine the number of mm of total travel + float millimeters_of_travel = abs(angular_travel) * radius; + if (linear_travel) + { + // If we have any Z motion, add this to the total mm of travel. + millimeters_of_travel = (float)utilities::hypot(millimeters_of_travel, linear_travel); + } + + if (millimeters_of_travel < 0.001f) { + return; // treat as succes because there is nothing to do; + } + + // The speed restrictions will be based on some new parameters. Removing the hard coded values + // Increase segment size if printing faster then computation speed allows + //uint16_t segments = (feedrate > 60.0f ? floor(millimeters_of_travel / min(static_cast<float>(args_.mm_per_arc_segment), feedrate * 0.01666f * static_cast<float>(args_.mm_per_arc_segment))) : floor(millimeters_of_travel / static_cast<float>(args_.mm_per_arc_segment))); + + // Use ceil here since the final segment could be nearly 2x as long as the rest. + // Example, assume millimeters_of_travel = 1.999. In this case segments will be 1, + // the interpolation loop will be skipped, and the segment drawn will be of length + // 1.999, which is not great. + uint16_t segments = (uint16_t)utilities::ceil(millimeters_of_travel / (float)args_.mm_per_arc_segment); + + if (segments == 0) + segments = 1; + /* + // Multiply inverse feed_rate to compensate for the fact that this movement is approximated + // by a number of discrete segments. The inverse feed_rate should be correct for the sum of + // all segments. + if (invert_feed_rate) { feed_rate *= segments; } + */ + 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, + 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 mc_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 mc_arc overhead. + This is important when there are successive arc motions. + */ + // Vector rotation matrix values + // Use two terms for better accuracy + float sq_theta_per_segment = theta_per_segment * theta_per_segment; + float cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation + float sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6; + + float arc_target[4]; + float sin_Ti; + float cos_Ti; + float r_axisi; + uint16_t i; + int8_t count = 0; + + // Initialize the linear axis + //arc_target[axis_linear] = position[axis_linear]; + + // Initialize the extruder axis + arc_target[E_AXIS] = position[E_AXIS]; // * Printer::invAxisStepsPerMM[E_AXIS]; -- Not sure what this does + + for (i = 1; i < segments; i++) { + // Increment (segments-1) + + if (count < args_.n_arc_correction) { //25 pieces + // Apply vector rotation matrix + r_axisi = r_axis0 * sin_T + r_axis1 * cos_T; + r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T; + r_axis1 = r_axisi; + count++; + } + 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). + cos_Ti = (float)utilities::cos(i * theta_per_segment); + sin_Ti = (float)utilities::sin(i * theta_per_segment); + r_axis0 = -offset[0] * cos_Ti + offset[1] * sin_Ti; + r_axis1 = -offset[0] * sin_Ti - offset[1] * cos_Ti; + count = 0; + } + + // Update arc_target location + arc_target[X_AXIS] = center_axis0 + r_axis0; + arc_target[Y_AXIS] = center_axis1 + r_axis1; + arc_target[Z_AXIS] += linear_per_segment; + arc_target[E_AXIS] += extruder_per_segment; + moveToReal(arc_target[X_AXIS], arc_target[Y_AXIS], position[Z_AXIS], arc_target[E_AXIS]); + } + // Ensure last segment arrives at target location. + moveToReal(target[X_AXIS], target[Y_AXIS], position[Z_AXIS], target[E_AXIS]); +} + +/// <summary> +/// This function was adapted from the 1.0.4 release of Repetier firmware, which can be found at the following link: +/// https://github.com/repetier/Repetier-Firmware/blob/2bbda51eb6407faf29a09987fd635c86818d32db/src/ArduinoAVR/Repetier/motion.cpp +/// This copyright notice was taken from the link above: +/// +/// This file is part of Repetier-Firmware. +/// Repetier-Firmware 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. +/// Repetier-Firmware 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 Repetier-Firmware. If not, see <http://www.gnu.org/licenses/>. +/// This firmware is a nearly complete rewrite of the sprinter firmware +/// by kliment (https://github.com/kliment/Sprinter) +/// which based on Tonokip RepRap firmware rewrite based off of Hydra-mmm firmware. +/// Functions in this file are used to communicate using ascii or repetier protocol. +/// </summary> +/// <param name="cart">The target position</param> +/// <param name="offset">The I and J offset</param> +/// <param name="clockwise">Is the motion clockwise or counterclockwise</param> +void repetier::arc_1_0_4(float* position, float* target, float* offset, float radius, uint8_t isclockwise) +{ + // int acceleration_manager_was_enabled = plan_is_acceleration_manager_enabled(); + // plan_set_acceleration_manager_enabled(false); // disable acceleration management for the duration of the arc + float center_axis0 = position[X_AXIS] + offset[X_AXIS]; + float center_axis1 = position[Y_AXIS] + offset[Y_AXIS]; + //float linear_travel = 0; //target[axis_linear] - position[axis_linear]; + float extruder_travel = (target[E_AXIS] - position[E_AXIS]); // * Printer::invAxisStepsPerMM[E_AXIS]; -- Not sure what this does... + float r_axis0 = -offset[0]; // Radius vector from center to current location + float r_axis1 = -offset[1]; + float rt_axis0 = target[0] - center_axis0; + float rt_axis1 = target[1] - center_axis1; + /*long xtarget = Printer::destinationSteps[X_AXIS]; + long ytarget = Printer::destinationSteps[Y_AXIS]; + long ztarget = Printer::destinationSteps[Z_AXIS]; + long etarget = Printer::destinationSteps[E_AXIS]; + */ + // CCW angle between position and target from circle center. Only one atan2() trig computation required. + float angular_travel = (float)utilities::atan2((double)(r_axis0 * rt_axis1) - (double)(r_axis1 * rt_axis0), (double)(r_axis0 * rt_axis0) + (double)(r_axis1 * rt_axis1)); + if ((!isclockwise && angular_travel <= 0.00001) || (isclockwise && angular_travel < -0.000001)) { + angular_travel += 2.0f * (float)M_PI; + } + if (isclockwise) { + angular_travel -= 2.0f * (float)M_PI; + } + + float millimeters_of_travel = (float)utilities::fabs(angular_travel) * radius; //hypot(angular_travel*radius, fabs(linear_travel)); + if (millimeters_of_travel < 0.001f) { + return; // treat as succes because there is nothing to do; + } + //uint16_t segments = (radius>=BIG_ARC_RADIUS ? floor(millimeters_of_travel/MM_PER_ARC_SEGMENT_BIG) : floor(millimeters_of_travel/MM_PER_ARC_SEGMENT)); + // Increase segment size if printing faster then computation speed allows + uint16_t segments = (uint16_t)(feedrate > 60.0f ? utilities::floor(millimeters_of_travel / utilities::min(static_cast<float>(args_.mm_per_arc_segment), feedrate * 0.01666f * static_cast<float>(args_.mm_per_arc_segment))) : utilities::floor(millimeters_of_travel / static_cast<float>(args_.mm_per_arc_segment))); + if (segments == 0) + segments = 1; + /* + // Multiply inverse feed_rate to compensate for the fact that this movement is approximated + // by a number of discrete segments. The inverse feed_rate should be correct for the sum of + // all segments. + if (invert_feed_rate) { feed_rate *= segments; } + */ + 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, + 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 mc_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 mc_arc overhead. + This is important when there are successive arc motions. + */ + // Vector rotation matrix values + float cos_T = 1 - 0.5f * theta_per_segment * theta_per_segment; // Small angle approximation + float sin_T = theta_per_segment; + + float arc_target[4]; + float sin_Ti; + float cos_Ti; + float r_axisi; + uint16_t i; + int8_t count = 0; + + // Initialize the linear axis + //arc_target[axis_linear] = position[axis_linear]; + + // Initialize the extruder axis + arc_target[E_AXIS] = position[E_AXIS]; // * Printer::invAxisStepsPerMM[E_AXIS]; -- Not sure what this does + + for (i = 1; i < segments; i++) { + // Increment (segments-1) + + if (count < args_.n_arc_correction) { //25 pieces + // Apply vector rotation matrix + r_axisi = r_axis0 * sin_T + r_axis1 * cos_T; + r_axis0 = r_axis0 * cos_T - r_axis1 * sin_T; + r_axis1 = r_axisi; + count++; + } + 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). + cos_Ti = (float)utilities::cos(i * theta_per_segment); + sin_Ti = (float)utilities::sin(i * theta_per_segment); + r_axis0 = -offset[0] * cos_Ti + offset[1] * sin_Ti; + r_axis1 = -offset[0] * sin_Ti - offset[1] * cos_Ti; + count = 0; + } + + // Update arc_target location + arc_target[X_AXIS] = center_axis0 + r_axis0; + arc_target[Y_AXIS] = center_axis1 + r_axis1; + //arc_target[axis_linear] += linear_per_segment; + arc_target[E_AXIS] += extruder_per_segment; + moveToReal(arc_target[X_AXIS], arc_target[Y_AXIS], position[Z_AXIS], arc_target[E_AXIS]); + } + // Ensure last segment arrives at target location. + moveToReal(target[X_AXIS], target[Y_AXIS], position[Z_AXIS], target[E_AXIS]); +} + +float repetier::min(float x, float y) +{ + if (x < y) + { + return x; + } + return y; +} + +//void repetier::buffer_line_kinematic(float x, float y, float z, const float& e, float feed_rate, uint8_t extruder, const float* gcode_target) +void repetier::moveToReal(float x, float y, float z, float e) +{ + + // create the target position + firmware_position target; + target.x = x; + target.y = y; + target.z = z; + target.e = e; + target.f = feedrate; + if (gcodes_.size() > 0) + { + gcodes_ += "\n"; + } + // Generate the gcode + gcodes_ += g1_command(target); + + // update the current position + set_current_position(target); +} |