diff options
Diffstat (limited to 'ArcWelderInverseProcessor/smoothieware.cpp')
| -rw-r--r-- | ArcWelderInverseProcessor/smoothieware.cpp | 274 |
1 files changed, 274 insertions, 0 deletions
diff --git a/ArcWelderInverseProcessor/smoothieware.cpp b/ArcWelderInverseProcessor/smoothieware.cpp new file mode 100644 index 0000000..bf17934 --- /dev/null +++ b/ArcWelderInverseProcessor/smoothieware.cpp @@ -0,0 +1,274 @@ +#include "smoothieware.h" +#include "utilities.h" +smoothieware::smoothieware(firmware_arguments args) : firmware(args) { + feed_rate = 0; + for (int i = 0; i < REPETIER_XYZE; i++) + { + machine_position[i] = 0; + } + THEKERNEL = new SmoothiewareKernel(); + apply_arguments(); +}; + +void smoothieware::apply_arguments() +{ + static const std::vector<std::string> smoothieware_firmware_version_names{ "2021-06-19" }; + set_versions(smoothieware_firmware_version_names, "2021-06-19"); + smoothieware_version_ = (smoothieware::smoothieware_firmware_versions)version_index_; + std::vector<std::string> used_arguments; + // Add switch back in if we ever add more versions + //switch (smoothieware_version_) + //{ + //default: + append_arc_ = &smoothieware::append_arc_2021_06_19; + used_arguments = { "mm_per_arc_segment", "mm_max_arc_error", "n_arc_correction", "g90_g91_influences_extruder" }; + //break; + //} + + args_.set_used_arguments(used_arguments); + +} + +smoothieware::~smoothieware() +{ + delete THEKERNEL; +} + +firmware_arguments smoothieware::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; + // Add the switch back if we ever need to add more versions + //switch (smoothieware_version_) + //{ + //default: + // Active Settings + default_args.mm_per_arc_segment = 0.0f; + default_args.mm_max_arc_error = 0.01; + default_args.n_arc_correction = 5; + //break; + //} + return default_args; +} + +std::string smoothieware::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 + machine_position[X_AXIS] = static_cast<float>(position_.x); + machine_position[Y_AXIS] = static_cast<float>(position_.y); + machine_position[Z_AXIS] = static_cast<float>(position_.z); + machine_position[E_AXIS] = static_cast<float>(position_.e); + float smoothieware_target[k_max_actuators]; + smoothieware_target[X_AXIS] = static_cast<float>(target.x); + smoothieware_target[Y_AXIS] = static_cast<float>(target.y); + smoothieware_target[Z_AXIS] = static_cast<float>(target.z); + smoothieware_target[E_AXIS] = static_cast<float>(target.e); + float smoothieware_offset[2]; + smoothieware_offset[0] = static_cast<float>(i); + smoothieware_offset[1] = static_cast<float>(j); + float radius = static_cast<float>(r); + + + // Set the feedrate + feed_rate = static_cast<float>(target.f); + uint8_t smoothieware_isclockwise = is_clockwise ? 1 : 0; + + (this->*append_arc_)(&gcode_, smoothieware_target, smoothieware_offset, radius, smoothieware_isclockwise); + + return gcodes_; +} +// Append an arc to the queue ( cutting it into segments as needed ) +bool smoothieware::append_arc_2021_06_19(SmoothiewareGcode* gcode, const float target[], const float offset[], float radius, bool is_clockwise) +{ + float rate_mm_s = this->feed_rate / seconds_per_minute; + // catch negative or zero feed rates and return the same error as GRBL does + if (rate_mm_s <= 0.0F) { + gcode->is_error = true; + gcode->txt_after_ok = (rate_mm_s == 0 ? "Undefined feed rate" : "feed rate < 0"); + return false; + } + + // Scary math. + float center_axis0 = this->machine_position[this->plane_axis_0] + offset[this->plane_axis_0]; + float center_axis1 = this->machine_position[this->plane_axis_1] + offset[this->plane_axis_1]; + float linear_travel = target[this->plane_axis_2] - this->machine_position[this->plane_axis_2]; + float r_axis0 = -offset[this->plane_axis_0]; // Radius vector from center to start position + float r_axis1 = -offset[this->plane_axis_1]; + float rt_axis0 = target[this->plane_axis_0] - this->machine_position[this->plane_axis_0] - offset[this->plane_axis_0]; // Radius vector from center to target position + float rt_axis1 = target[this->plane_axis_1] - this->machine_position[this->plane_axis_1] - offset[this->plane_axis_1]; + float angular_travel = 0; + //check for condition where atan2 formula will fail due to everything canceling out exactly + if ((this->machine_position[this->plane_axis_0] == target[this->plane_axis_0]) && (this->machine_position[this->plane_axis_1] == target[this->plane_axis_1])) { + if (is_clockwise) { // set angular_travel to -2pi for a clockwise full circle + angular_travel = (-2 * (float)PI); + } + else { // set angular_travel to 2pi for a counterclockwise full circle + angular_travel = (2 * (float)PI); + } + } + else { + // Patch from GRBL Firmware - Christoph Baumann 04072015 + // CCW angle between position and target from circle center. Only one atan2() trig computation required. + // Only run if not a full circle or angular travel will incorrectly result in 0.0f + angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1); + if (plane_axis_2 == Y_AXIS) { is_clockwise = !is_clockwise; } //Math for XZ plane is reverse of other 2 planes + if (is_clockwise) { // adjust angular_travel to be in the range of -2pi to 0 for clockwise arcs + if (angular_travel > 0) { angular_travel -= (2 * (float)PI); } + } + else { // adjust angular_travel to be in the range of 0 to 2pi for counterclockwise arcs + if (angular_travel < 0) { angular_travel += (2 * (float)PI); } + } + } + + // initialize linear travel for ABC +#if SMOOTHIEWARE_MAX_ROBOT_ACTUATORS > 3 + float abc_travel[n_motors - 3]; + for (int i = A_AXIS; i < n_motors; i++) { + abc_travel[i - 3] = target[i] - this->machine_position[i]; + } +#endif + + + // Find the distance for this gcode + float millimeters_of_travel = (float)utilities::hypot(angular_travel * radius, utilities::fabsf(linear_travel)); + + // We don't care about non-XYZ moves ( for example the extruder produces some of those ) + if (millimeters_of_travel < 0.000001F) { + return false; + } + + // limit segments by maximum arc error + float arc_segment = (float)args_.mm_per_arc_segment; + if ((args_.mm_max_arc_error > 0) && (2 * radius > args_.mm_max_arc_error)) { + float min_err_segment = 2 * (float)utilities::sqrtf((args_.mm_max_arc_error * (2 * radius - args_.mm_max_arc_error))); + if (args_.mm_per_arc_segment < min_err_segment) { + arc_segment = min_err_segment; + } + } + + // catch fall through on above + if (arc_segment < 0.0001F) { + arc_segment = 0.5F; /// the old default, so we avoid the divide by zero + } + + // Figure out how many segments for this gcode + // TODO for deltas we need to make sure we are at least as many segments as requested, also if mm_per_line_segment is set we need to use the + uint16_t segments = (uint16_t)floorf(millimeters_of_travel / arc_segment); + bool moved = false; + + if (segments > 1) { + float theta_per_segment = angular_travel / segments; + float linear_per_segment = linear_travel / segments; +#if SMOOTHIEWARE_MAX_ROBOT_ACTUATORS > 3 + float abc_per_segment[n_motors - 3]; + for (int i = 0; i < n_motors - 3; i++) { + abc_per_segment[i] = abc_travel[i] / segments; + } +#endif + + /* 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 float numbers are single precision on the Arduino. (True float 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[n_motors]; + float sin_Ti; + float cos_Ti; + float r_axisi; + uint16_t i; + int8_t count = 0; + + // init array for all axis + utilities::memcpy(arc_target, machine_position, n_motors * sizeof(float)); + + // Initialize the linear axis + arc_target[this->plane_axis_2] = this->machine_position[this->plane_axis_2]; + + for (i = 1; i < segments; i++) { // Increment (segments-1) + if (THEKERNEL->is_halted()) return false; // don't queue any more segments + + if (count < args_.n_arc_correction) { + // 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::cosf(i * theta_per_segment); + sin_Ti = (float)utilities::sinf(i * theta_per_segment); + r_axis0 = -offset[this->plane_axis_0] * cos_Ti + offset[this->plane_axis_1] * sin_Ti; + r_axis1 = -offset[this->plane_axis_0] * sin_Ti - offset[this->plane_axis_1] * cos_Ti; + count = 0; + } + + // Update arc_target location + arc_target[this->plane_axis_0] = center_axis0 + r_axis0; + arc_target[this->plane_axis_1] = center_axis1 + r_axis1; + arc_target[this->plane_axis_2] += linear_per_segment; +#if SMOOTHIEWARE_MAX_ROBOT_ACTUATORS > 3 + for (int a = A_AXIS; a < n_motors; a++) { + arc_target[a] += abc_per_segment[a - 3]; + } +#endif + + // Append this segment to the queue + bool b = this->append_milestone(arc_target, rate_mm_s); + moved = moved || b; + } + } + + // Ensure last segment arrives at target location. + if (this->append_milestone(target, rate_mm_s)) moved = true; + + return moved; +} + +bool smoothieware::append_milestone(const float target[], double rate_mm_s) +{ + double rate_mm_min = rate_mm_s * 60; + // create the target position + firmware_position gcode_target; + gcode_target.x = target[AxisEnum::X_AXIS]; + gcode_target.y = target[AxisEnum::Y_AXIS]; + gcode_target.z = target[AxisEnum::Z_AXIS]; + gcode_target.e = target[AxisEnum::E_AXIS]; + gcode_target.f = rate_mm_min; + if (gcodes_.size() > 0) + { + gcodes_ += "\n"; + } + // Generate the gcode + gcodes_ += g1_command(gcode_target); + + return true; + return true; +} |