diff options
Diffstat (limited to 'ArcWelderInverseProcessor/marlin_2.cpp')
| -rw-r--r-- | ArcWelderInverseProcessor/marlin_2.cpp | 677 |
1 files changed, 677 insertions, 0 deletions
diff --git a/ArcWelderInverseProcessor/marlin_2.cpp b/ArcWelderInverseProcessor/marlin_2.cpp new file mode 100644 index 0000000..4893dd1 --- /dev/null +++ b/ArcWelderInverseProcessor/marlin_2.cpp @@ -0,0 +1,677 @@ +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Arc Welder: Marlin 2 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 "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<std::string> 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<std::string> used_arguments; + switch (marlin_2_version_) + { + case marlin_2::marlin_2_firmware_versions::V2_0_9_2: + used_arguments = {"min_arc_segment_mm", "max_arc_segment_mm", "min_circle_segments", "arc_segments_per_sec", "n_arc_correction", "g90_g91_influences_extruder" }; + plan_arc_ = &marlin_2::plan_arc_2_0_9_2; + break; + default: + used_arguments = { "mm_per_arc_segment", "arc_segments_per_r", "min_arc_segments", "arc_segments_per_sec", "n_arc_correction", "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.set_min_arc_segment_mm(0.1f); + default_args.set_max_arc_segment_mm(1.0f); + default_args.set_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; + // 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.min_arc_segments = 24; + default_args.n_arc_correction = 25; + // Inactive Settings + 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; +} + +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<float>(position_.x); + current_position[Y_AXIS] = static_cast<float>(position_.y); + current_position[Z_AXIS] = static_cast<float>(position_.z); + current_position[E_AXIS] = static_cast<float>(position_.e); + float marlin_target[MARLIN_2_XYZE]; + marlin_target[X_AXIS] = static_cast<float>(target.x); + marlin_target[Y_AXIS] = static_cast<float>(target.y); + marlin_target[Z_AXIS] = static_cast<float>(target.z); + marlin_target[E_AXIS] = static_cast<float>(target.e); + float marlin_offset[2]; + marlin_offset[0] = static_cast<float>(i); + marlin_offset[1] = static_cast<float>(j); + // TODO: handle R form!! + + // Set the feedrate + feedrate_mm_s = static_cast<float>(target.f); + uint8_t marlin_isclockwise = is_clockwise ? 1 : 0; + + (this->*plan_arc_)(marlin_target, marlin_offset, marlin_isclockwise, 0); + + return gcodes_; +} + +/// <summary> +/// 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 <http://www.gnu.org/licenses/>. +/// </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 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 = HYPOT(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 ? -RADIANS(360) : RADIANS(360); + } + else { + // Calculate the angle + angular_travel = ATAN2(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 -= RADIANS(360); break; // Positive but CW? Reverse direction. + case 2: angular_travel += RADIANS(360); break; // Negative but CCW? Reverse direction. + } + + if (args_.min_arc_segments > 1) + { + min_segments = (uint16_t)CEIL(min_segments * ABS(angular_travel) / RADIANS(360)); + min_segments = (uint16_t)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 ? HYPOT(flat_mm, linear_travel) : ABS(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 = constrain((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 = _MAX(scaled_fr_mm_s * RECIPROCAL((float)args_.arc_segments_per_sec), (float)args_.mm_per_arc_segment); + } + + // Divide total travel by nominal segment length + uint16_t segments = (uint16_t)FLOOR(mm_of_travel / seg_length); + //uint16_t segments = FLOOR(mm_of_travel / seg_length); + segments = (uint16_t)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 = 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 + + + 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 = COS(i * theta_per_segment); + const float sin_Ti = SIN(i * 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); +} + + + +/// <summary> +/// 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 <http://www.gnu.org/licenses/>. +/// </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 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_.get_min_circle_segments() > 0 ? args_.get_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 = HYPOT(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 ? -RADIANS(360) : RADIANS(360); + abs_angular_travel = RADIANS(360); + min_segments = min_circle_segments; + } + else { + // Calculate the angle + angular_travel = ATAN2(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 -= RADIANS(360); break; // Positive but CW? Reverse direction. + case 2: angular_travel += RADIANS(360); break; // Negative but CCW? Reverse direction. + } + + abs_angular_travel = ABS(angular_travel); + + // Apply minimum segments to the arc + const float portion_of_circle = abs_angular_travel / RADIANS(360); // Portion of a complete circle (0 < N < 1) + min_segments = (uint16_t)CEIL((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 = constrain(scaled_fr_mm_s * RECIPROCAL((float)args_.arc_segments_per_sec), (float)args_.get_min_arc_segment_mm(), (float)args_.get_max_arc_segment_mm()); + } + else { + nominal_segment_mm = (float)args_.get_max_arc_segment_mm(); + } + // Number of whole segments based on the nominal segment length + const float nominal_segments = _MAX(FLOOR(flat_mm / nominal_segment_mm), min_segments); + + // A new segment length based on the required minimum + const float segment_mm = constrain(flat_mm / nominal_segments, (float)args_.get_min_arc_segment_mm(), (float)args_.get_max_arc_segment_mm()); + + // The number of whole segments in the arc, ignoring the remainder + uint16_t segments = (uint16_t)FLOOR(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 = 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 + + + 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 = COS(i * theta_per_segment); + const float sin_Ti = SIN(i * 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 +float marlin_2::HYPOT(float x, float y) +{ + return (float)utilities::hypot(x, y); +} + +float marlin_2::ATAN2(float x, float y) +{ + return (float)utilities::atan2(x, y); +} + +float marlin_2::RADIANS(float x) +{ + return (x * (float)M_PI) / 180; +} + +float marlin_2::ABS(float x) +{ + return (float)utilities::abs(x); +} + +float marlin_2::FLOOR(float x) +{ + return (float)utilities::floor(x); +} + +float marlin_2::COS(float x) +{ + return (float)utilities::cos(x); +} + +float marlin_2::SIN(float x) +{ + return (float)utilities::sin(x); +} + +float marlin_2::NOLESS(uint16_t x, uint16_t y) +{ + if (x < y) + return y; + return x; +} + +float marlin_2::sq(float x) +{ + return x * x; +} + +float marlin_2::MMS_SCALED(float x) +{ + // No scaling + return x; +} + +bool marlin_2::WITHIN(float N, float L, float H) +{ + return ((N) >= (L) && (N) <= (H)); +} +bool marlin_2::NEAR_ZERO(float x) +{ + return WITHIN(x, -0.000001f, 0.000001f); +} +bool marlin_2::NEAR(float x, float y) +{ + return NEAR_ZERO((x)-(y)); +} + +float marlin_2::CEIL(float x) +{ + return (float)utilities::ceil(x); +} + +float marlin_2::constrain(float value, float arg_min, float arg_max) +{ + return ((value) < (arg_min) ? (arg_min) : ((value) > (arg_max) ? (arg_max) : (value))); +} + +float marlin_2::_MAX(float x, float y) +{ + if (x>y) return x; + return y; +} +float marlin_2::_MIN(float x, float y) +{ + if (x < y) return x; + return y; +} + +float marlin_2::RECIPROCAL(float x) +{ + return (float)1.0/x; +} +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; +} |