From 1db46149e09bf94a1d9d88098354e2267a818359 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 23 Mar 2016 14:11:26 -0700 Subject: [PATCH] arc.{c,h} cleanup --- src/canonical_machine.c | 1 - src/config.h | 6 + src/plan/arc.c | 552 +++++++++++++++++++++------------------- src/plan/arc.h | 47 +--- 4 files changed, 296 insertions(+), 310 deletions(-) diff --git a/src/canonical_machine.c b/src/canonical_machine.c index 308efaa..df55fd0 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -651,7 +651,6 @@ void canonical_machine_init() { // Sub-system inits cm_spindle_init(); - cm_arc_init(); } diff --git a/src/config.h b/src/config.h index 252fac0..5135383 100644 --- a/src/config.h +++ b/src/config.h @@ -363,3 +363,9 @@ typedef enum { // Input #define INPUT_BUFFER_LEN 255 // text buffer size (255 max) + + +// Arc +#define ARC_RADIUS_ERROR_MAX 1.0 // max mm diff between start and end radius +#define ARC_RADIUS_ERROR_MIN 0.005 // min mm where 1% rule applies +#define ARC_RADIUS_TOLERANCE 0.001 // 0.1% radius variance test diff --git a/src/plan/arc.c b/src/plan/arc.c index e6a4d43..1d4008f 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -28,174 +28,204 @@ /* This module actually contains some parts that belong ion the * canonical machine, and other parts that belong at the motion planner - * level, but the whole thing is * treated as if it were part of the + * level, but the whole thing is treated as if it were part of the * motion planner. */ #include "arc.h" #include "planner.h" +#include "config.h" #include "util.h" #include #include #include -arc_t arc; -static stat_t _compute_arc(); -static stat_t _compute_arc_offsets_from_radius(); -static void _estimate_arc_time(); +// See planner.h for MM_PER_ARC_SEGMENT and other arc setting #defines +typedef struct arArcSingleton { // persistent planner and runtime variables + uint8_t run_state; // runtime state machine sequence -/// Initialize arc structures -void cm_arc_init() {} + float position[AXES]; // accumulating runtime position + float offset[3]; // IJK offsets + float length; // length of line or helix in mm + float theta; // total angle specified by arc + float theta_end; + float radius; // Raw R value, or computed via offsets + float angular_travel; // travel along the arc + float linear_travel; // travel along linear axis of arc + float planar_travel; + uint8_t full_circle; // set true if full circle arcs specified + uint32_t rotations; // Full rotations for full circles (P value) -/* Canonical machine entry point for arc - * - * Generates an arc by queuing line segments to the move buffer. The arc is - * approximated by generating a large number of tiny, linear arc_segments. - */ -stat_t cm_arc_feed(float target[], float flags[], // arc endpoints - float i, float j, float k, // raw arc offsets - float radius, // non-zero radius implies radius mode - uint8_t motion_mode) { // defined motion mode - // Set axis plane and trap arc specification errors - - // trap missing feed rate - if (cm.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(cm.gm.feed_rate)) - return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - - // set radius mode flag and do simple test(s) - bool radius_f = fp_NOT_ZERO(cm.gf.arc_radius); // set true if radius arc - // radius value must be + and > minimum radius - if (radius_f && cm.gn.arc_radius < MIN_ARC_RADIUS) - return STAT_ARC_RADIUS_OUT_OF_TOLERANCE; - - // setup some flags - bool target_x = fp_NOT_ZERO(flags[AXIS_X]); // is X axis specified - bool target_y = fp_NOT_ZERO(flags[AXIS_Y]); - bool target_z = fp_NOT_ZERO(flags[AXIS_Z]); - - bool offset_i = fp_NOT_ZERO(cm.gf.arc_offset[0]); // is offset I specified - bool offset_j = fp_NOT_ZERO(cm.gf.arc_offset[1]); // J - bool offset_k = fp_NOT_ZERO(cm.gf.arc_offset[2]); // K - - // Set the arc plane for the current G17/G18/G19 setting and test arc - // specification Plane axis 0 and 1 are the arc plane, the linear axis is - // normal to the arc plane. - // G17 - the vast majority of arcs are in the G17 (XY) plane - if (cm.gm.select_plane == CANON_PLANE_XY) { - arc.plane_axis_0 = AXIS_X; - arc.plane_axis_1 = AXIS_Y; - arc.linear_axis = AXIS_Z; - - if (radius_f) { - // must have at least one endpoint specified - if (!(target_x || target_y)) - return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; - - } else if (offset_k) - // center format arc tests, it's OK to be missing either or both i and j, - // but error if k is present - return STAT_ARC_SPECIFICATION_ERROR; - - } else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18 - arc.plane_axis_0 = AXIS_X; - arc.plane_axis_1 = AXIS_Z; - arc.linear_axis = AXIS_Y; - - if (radius_f) { - if (!(target_x || target_z)) - return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; - } else if (offset_j) return STAT_ARC_SPECIFICATION_ERROR; - - } else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19 - arc.plane_axis_0 = AXIS_Y; - arc.plane_axis_1 = AXIS_Z; - arc.linear_axis = AXIS_X; - - if (radius_f) { - if (!target_y && !target_z) - return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; - } else if (offset_i) return STAT_ARC_SPECIFICATION_ERROR; - } - - // set values in the Gcode model state & copy it (linenum was already - // captured) - cm_set_model_target(target, flags); - - // in radius mode it's an error for start == end - if (radius_f && fp_EQ(cm.gmx.position[AXIS_X], cm.gm.target[AXIS_X]) && - fp_EQ(cm.gmx.position[AXIS_Y], cm.gm.target[AXIS_Y]) && - fp_EQ(cm.gmx.position[AXIS_Z], cm.gm.target[AXIS_Z])) - return STAT_ARC_ENDPOINT_IS_STARTING_POINT; - - // now get down to the rest of the work setting up the arc for execution - cm.gm.motion_mode = motion_mode; - cm_set_work_offsets(&cm.gm); // resolved offsets to gm - memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // context to arc singleton + uint8_t plane_axis_0; // arc plane axis 0 - e.g. X for G17 + uint8_t plane_axis_1; // arc plane axis 1 - e.g. Y for G17 + uint8_t linear_axis; // linear axis (normal to plane) - copy_vector(arc.position, cm.gmx.position); // arc pos from gcode model - - arc.radius = _to_millimeters(radius); // set arc radius or zero + float arc_time; // total running time for arc (derived) + float arc_segments; // number of segments in arc or blend + int32_t arc_segment_count; // count of running segments + float arc_segment_theta; // angular motion per segment + float arc_segment_linear_travel; // linear motion per segment + float center_0; // center of circle at plane axis 0 (e.g. X for G17) + float center_1; // center of circle at plane axis 1 (e.g. Y for G17) - arc.offset[0] = _to_millimeters(i); // offsets canonical form (mm) - arc.offset[1] = _to_millimeters(j); - arc.offset[2] = _to_millimeters(k); + GCodeState_t gm; // state struct is passed for each arc segment. +} arc_t; - arc.rotations = floor(fabs(cm.gn.parameter)); // P must be positive integer +arc_t arc = {}; - // determine if this is a full circle arc. Evaluates true if no target is set - arc.full_circle = - fp_ZERO(flags[arc.plane_axis_0]) & fp_ZERO(flags[arc.plane_axis_1]); - // compute arc runtime values - ritorno(_compute_arc()); +/* Returns a naive estimate of arc execution time to inform segment + * calculation. The arc time is computed not to exceed the time taken + * in the slowest dimension in the arc plane or in linear + * travel. Maximum feed rates are compared in each dimension, but the + * comparison assumes that the arc will have at least one segment + * where the unit vector is 1 in that dimension. This is not true for + * any arbitrary arc, with the result that the time returned may be + * less than optimal. + */ +static void _estimate_arc_time() { + // Determine move time at requested feed rate + if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) { + // inverse feed rate has been normalized to minutes + arc.arc_time = cm.gm.feed_rate; + // reset feed rate so next block requires an explicit feed rate setting + cm.gm.feed_rate = 0; + cm.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE; - // trap zero length arcs that _compute_arc can throw - if (fp_ZERO(arc.length)) return STAT_MINIMUM_LENGTH_MOVE; + } else arc.arc_time = arc.length / cm.gm.feed_rate; - cm_cycle_start(); // if not already started - arc.run_state = MOVE_RUN; // enable arc run from the callback - cm_finalize_move(); + // Downgrade the time if there is a rate-limiting axis + arc.arc_time = + max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_0].feedrate_max); + arc.arc_time = + max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_1].feedrate_max); - return STAT_OK; + if (0 < fabs(arc.linear_travel)) + arc.arc_time = + max(arc.arc_time, + fabs(arc.linear_travel/cm.a[arc.linear_axis].feedrate_max)); } -/* Generate an arc +/* Compute arc center (offset) from radius. * - * Called from the controller main loop. Each time it's called it queues - * as many arc segments (lines) as it can before it blocks, then returns. + * Needs to calculate the center of the circle that has the designated radius + * and passes through both the current position and the target position * - * Parts of this routine were originally sourced from the grbl project. + * This method calculates the following set of equations where: + * + * [x,y] is the vector from current to target position, + * d == magnitude of that vector, + * h == hypotenuse of the triangle formed by the radius of the circle, + * the distance to the center of the travel vector. + * + * A vector perpendicular to the travel vector [-y,x] is scaled to the length + * of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] + * to form the new point [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the + * center of the arc. + * + * d^2 == x^2 + y^2 + * h^2 == r^2 - (d/2)^2 + * i == x/2 - y/d*h + * j == y/2 + x/d*h + * O <- [i,j] + * - | + * r - | + * - | + * - | h + * - | + * [0,0] -> C -----------------+--------------- T <- [x,y] + * | <------ d/2 ---->| + * + * C - Current position + * T - Target position + * O - center of circle that pass through both C and T + * d - distance from C to T + * r - designated radius + * h - distance from center of CT to O + * + * Expanding the equations: + * + * d -> sqrt(x^2 + y^2) + * h -> sqrt(4 * r^2 - x^2 - y^2)/2 + * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 + * + * Which can be written: + * + * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 + * + * Which we for size and speed reasons optimize to: + * + * h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) + * i = (x - (y * h_x2_div_d))/2 + * j = (y + (x * h_x2_div_d))/2 + * + * Computing clockwise vs counter-clockwise motion + * + * The counter clockwise circle lies to the left of the target direction. + * When offset is positive the left hand circle will be generated - + * when it is negative the right hand circle is generated. + * + * T <-- Target position + * ^ + * Clockwise circles with | Clockwise circles with + * this center will have | this center will have + * > 180 deg of angular travel | < 180 deg of angular travel, + * \ | which is a good thing! + * \ | / + * center of arc when -> x <----- | -----> x <- center of arc when + * h_x2_div_d is positive | h_x2_div_d is negative + * | + * C <-- Current position + * + * + * Assumes arc singleton has been pre-loaded with target and position. + * Parts of this routine were originally sourced from the grbl project. */ -stat_t cm_arc_callback() { - if (arc.run_state == MOVE_OFF) return STAT_NOOP; - if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) - return STAT_EAGAIN; +static stat_t _compute_arc_offsets_from_radius() { + // Calculate the change in position along each selected axis + float x = cm.gm.target[arc.plane_axis_0] - cm.gmx.position[arc.plane_axis_0]; + float y = cm.gm.target[arc.plane_axis_1] - cm.gmx.position[arc.plane_axis_1]; - arc.theta += arc.arc_segment_theta; - arc.gm.target[arc.plane_axis_0] = arc.center_0 + sin(arc.theta) * arc.radius; - arc.gm.target[arc.plane_axis_1] = arc.center_1 + cos(arc.theta) * arc.radius; - arc.gm.target[arc.linear_axis] += arc.arc_segment_linear_travel; - mp_aline(&arc.gm); // run the line - copy_vector(arc.position, arc.gm.target); // update arc current pos + // *** From Forrest Green - Other Machine Co, 3/27/14 + // If the distance between endpoints is greater than the arc diameter, disc + // will be negative indicating that the arc is offset into the complex plane + // beyond the reach of any real CNC. However, numerical errors can flip the + // sign of disc as it approaches zero (which happens as the arc angle + // approaches 180 degrees). To avoid mishandling these arcs we use the + // closest real solution (which will be 0 when disc <= 0). This risks + // obscuring g-code errors where the radius is actually too small (they will + // be treated as half circles), but ensures that all valid arcs end up + // reasonably close to their intended paths regardless of any numerical + // issues. + float disc = 4 * square(arc.radius) - (square(x) + square(y)); - if (--arc.arc_segment_count > 0) return STAT_EAGAIN; + float h_x2_div_d = (disc > 0) ? -sqrt(disc) / hypotf(x, y) : 0; - arc.run_state = MOVE_OFF; + // Invert the sign of h_x2_div_d if circle is counter clockwise (see header + // notes) + if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) h_x2_div_d = -h_x2_div_d; - return STAT_OK; -} + // Negative R is g-code-alese for "I want a circle with more than 180 degrees + // of travel" (go figure!), even though it is advised against ever generating + // such circles in a single line of g-code. By inverting the sign of + // h_x2_div_d the center of the circles is placed on the opposite side of + // the line of travel and thus we get the unadvisably long arcs as prescribed. + if (arc.radius < 0) h_x2_div_d = -h_x2_div_d; + // Complete the operation by calculating the actual center of the arc + arc.offset[arc.plane_axis_0] = (x - y * h_x2_div_d) / 2; + arc.offset[arc.plane_axis_1] = (y + x * h_x2_div_d) / 2; + arc.offset[arc.linear_axis] = 0; -/// Stop arc movement without maintaining position -/// OK to call if no arc is running -void cm_abort_arc() { - arc.run_state = MOVE_OFF; + return STAT_OK; } @@ -325,152 +355,148 @@ static stat_t _compute_arc() { } -/* Compute arc center (offset) from radius. - * - * Needs to calculate the center of the circle that has the designated radius - * and passes through both the current position and the target position - * - * This method calculates the following set of equations where: - * - * [x,y] is the vector from current to target position, - * d == magnitude of that vector, - * h == hypotenuse of the triangle formed by the radius of the circle, - * the distance to the center of the travel vector. - * - * A vector perpendicular to the travel vector [-y,x] is scaled to the length - * of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] - * to form the new point [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the - * center of the arc. - * - * d^2 == x^2 + y^2 - * h^2 == r^2 - (d/2)^2 - * i == x/2 - y/d*h - * j == y/2 + x/d*h - * O <- [i,j] - * - | - * r - | - * - | - * - | h - * - | - * [0,0] -> C -----------------+--------------- T <- [x,y] - * | <------ d/2 ---->| - * - * C - Current position - * T - Target position - * O - center of circle that pass through both C and T - * d - distance from C to T - * r - designated radius - * h - distance from center of CT to O - * - * Expanding the equations: - * - * d -> sqrt(x^2 + y^2) - * h -> sqrt(4 * r^2 - x^2 - y^2)/2 - * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - * - * Which can be written: - * - * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - * - * Which we for size and speed reasons optimize to: - * - * h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) - * i = (x - (y * h_x2_div_d))/2 - * j = (y + (x * h_x2_div_d))/2 - * - * Computing clockwise vs counter-clockwise motion - * - * The counter clockwise circle lies to the left of the target direction. - * When offset is positive the left hand circle will be generated - - * when it is negative the right hand circle is generated. - * - * T <-- Target position - * - * ^ - * Clockwise circles with | Clockwise circles with - * this center will have | this center will have - * > 180 deg of angular travel | < 180 deg of angular travel, - * \ | which is a good thing! - * \ | / - * center of arc when -> x <----- | -----> x <- center of arc when - * h_x2_div_d is positive | h_x2_div_d is negative - * | - * C <-- Current position - * +/* Canonical machine entry point for arc * - * Assumes arc singleton has been pre-loaded with target and position. - * Parts of this routine were originally sourced from the grbl project. + * Generates an arc by queuing line segments to the move buffer. The arc is + * approximated by generating a large number of tiny, linear arc_segments. */ -static stat_t _compute_arc_offsets_from_radius() { - // Calculate the change in position along each selected axis - float x = cm.gm.target[arc.plane_axis_0] - cm.gmx.position[arc.plane_axis_0]; - float y = cm.gm.target[arc.plane_axis_1] - cm.gmx.position[arc.plane_axis_1]; +stat_t cm_arc_feed(float target[], float flags[], // arc endpoints + float i, float j, float k, // raw arc offsets + float radius, // non-zero radius implies radius mode + uint8_t motion_mode) { // defined motion mode + // Set axis plane and trap arc specification errors - // *** From Forrest Green - Other Machine Co, 3/27/14 - // If the distance between endpoints is greater than the arc diameter, disc - // will be negative indicating that the arc is offset into the complex plane - // beyond the reach of any real CNC. However, numerical errors can flip the - // sign of disc as it approaches zero (which happens as the arc angle - // approaches 180 degrees). To avoid mishandling these arcs we use the - // closest real solution (which will be 0 when disc <= 0). This risks - // obscuring g-code errors where the radius is actually too small (they will - // be treated as half circles), but ensures that all valid arcs end up - // reasonably close to their intended paths regardless of any numerical - // issues. - float disc = 4 * square(arc.radius) - (square(x) + square(y)); + // trap missing feed rate + if (cm.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(cm.gm.feed_rate)) + return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - float h_x2_div_d = (disc > 0) ? -sqrt(disc) / hypotf(x, y) : 0; + // set radius mode flag and do simple test(s) + bool radius_f = fp_NOT_ZERO(cm.gf.arc_radius); // set true if radius arc + // radius value must be + and > minimum radius + if (radius_f && cm.gn.arc_radius < MIN_ARC_RADIUS) + return STAT_ARC_RADIUS_OUT_OF_TOLERANCE; - // Invert the sign of h_x2_div_d if circle is counter clockwise (see header - // notes) - if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) h_x2_div_d = -h_x2_div_d; + // setup some flags + bool target_x = fp_NOT_ZERO(flags[AXIS_X]); // is X axis specified + bool target_y = fp_NOT_ZERO(flags[AXIS_Y]); + bool target_z = fp_NOT_ZERO(flags[AXIS_Z]); - // Negative R is g-code-alese for "I want a circle with more than 180 degrees - // of travel" (go figure!), even though it is advised against ever generating - // such circles in a single line of g-code. By inverting the sign of - // h_x2_div_d the center of the circles is placed on the opposite side of - // the line of travel and thus we get the unadvisably long arcs as prescribed. - if (arc.radius < 0) h_x2_div_d = -h_x2_div_d; + bool offset_i = fp_NOT_ZERO(cm.gf.arc_offset[0]); // is offset I specified + bool offset_j = fp_NOT_ZERO(cm.gf.arc_offset[1]); // J + bool offset_k = fp_NOT_ZERO(cm.gf.arc_offset[2]); // K - // Complete the operation by calculating the actual center of the arc - arc.offset[arc.plane_axis_0] = (x - y * h_x2_div_d) / 2; - arc.offset[arc.plane_axis_1] = (y + x * h_x2_div_d) / 2; - arc.offset[arc.linear_axis] = 0; + // Set the arc plane for the current G17/G18/G19 setting and test arc + // specification Plane axis 0 and 1 are the arc plane, the linear axis is + // normal to the arc plane. + // G17 - the vast majority of arcs are in the G17 (XY) plane + if (cm.gm.select_plane == CANON_PLANE_XY) { + arc.plane_axis_0 = AXIS_X; + arc.plane_axis_1 = AXIS_Y; + arc.linear_axis = AXIS_Z; + + if (radius_f) { + // must have at least one endpoint specified + if (!(target_x || target_y)) + return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; + + } else if (offset_k) + // center format arc tests, it's OK to be missing either or both i and j, + // but error if k is present + return STAT_ARC_SPECIFICATION_ERROR; + + } else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18 + arc.plane_axis_0 = AXIS_X; + arc.plane_axis_1 = AXIS_Z; + arc.linear_axis = AXIS_Y; + + if (radius_f) { + if (!(target_x || target_z)) + return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; + } else if (offset_j) return STAT_ARC_SPECIFICATION_ERROR; + + } else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19 + arc.plane_axis_0 = AXIS_Y; + arc.plane_axis_1 = AXIS_Z; + arc.linear_axis = AXIS_X; + + if (radius_f) { + if (!target_y && !target_z) + return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; + } else if (offset_i) return STAT_ARC_SPECIFICATION_ERROR; + } + + // set values in the Gcode model state & copy it (linenum was already + // captured) + cm_set_model_target(target, flags); + + // in radius mode it's an error for start == end + if (radius_f && fp_EQ(cm.gmx.position[AXIS_X], cm.gm.target[AXIS_X]) && + fp_EQ(cm.gmx.position[AXIS_Y], cm.gm.target[AXIS_Y]) && + fp_EQ(cm.gmx.position[AXIS_Z], cm.gm.target[AXIS_Z])) + return STAT_ARC_ENDPOINT_IS_STARTING_POINT; + + // now get down to the rest of the work setting up the arc for execution + cm.gm.motion_mode = motion_mode; + cm_set_work_offsets(&cm.gm); // resolved offsets to gm + memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // context to arc singleton + + copy_vector(arc.position, cm.gmx.position); // arc pos from gcode model + + arc.radius = _to_millimeters(radius); // set arc radius or zero + + arc.offset[0] = _to_millimeters(i); // offsets canonical form (mm) + arc.offset[1] = _to_millimeters(j); + arc.offset[2] = _to_millimeters(k); + + arc.rotations = floor(fabs(cm.gn.parameter)); // P must be positive integer + + // determine if this is a full circle arc. Evaluates true if no target is set + arc.full_circle = + fp_ZERO(flags[arc.plane_axis_0]) & fp_ZERO(flags[arc.plane_axis_1]); + + // compute arc runtime values + ritorno(_compute_arc()); + + // trap zero length arcs that _compute_arc can throw + if (fp_ZERO(arc.length)) return STAT_MINIMUM_LENGTH_MOVE; + + cm_cycle_start(); // if not already started + arc.run_state = MOVE_RUN; // enable arc run from the callback + cm_finalize_move(); return STAT_OK; } -/* Returns a naiive estimate of arc execution time to inform segment - * calculation. The arc time is computed not to exceed the time taken - * in the slowest dimension in the arc plane or in linear - * travel. Maximum feed rates are compared in each dimension, but the - * comparison assumes that the arc will have at least one segment - * where the unit vector is 1 in that dimension. This is not true for - * any arbitrary arc, with the result that the time returned may be - * less than optimal. +/* Generate an arc + * + * Called from the controller main loop. Each time it's called it queues + * as many arc segments (lines) as it can before it blocks, then returns. + * + * Parts of this routine were originally sourced from the grbl project. */ -static void _estimate_arc_time() { - // Determine move time at requested feed rate - if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) { - // inverse feed rate has been normalized to minutes - arc.arc_time = cm.gm.feed_rate; - // reset feed rate so next block requires an explicit feed rate setting - cm.gm.feed_rate = 0; - cm.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE; +stat_t cm_arc_callback() { + if (arc.run_state == MOVE_OFF) return STAT_NOOP; + if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) + return STAT_EAGAIN; - } else arc.arc_time = arc.length / cm.gm.feed_rate; + arc.theta += arc.arc_segment_theta; + arc.gm.target[arc.plane_axis_0] = arc.center_0 + sin(arc.theta) * arc.radius; + arc.gm.target[arc.plane_axis_1] = arc.center_1 + cos(arc.theta) * arc.radius; + arc.gm.target[arc.linear_axis] += arc.arc_segment_linear_travel; + mp_aline(&arc.gm); // run the line + copy_vector(arc.position, arc.gm.target); // update arc current pos - // Downgrade the time if there is a rate-limiting axis - arc.arc_time = - max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_0].feedrate_max); - arc.arc_time = - max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_1].feedrate_max); + if (--arc.arc_segment_count > 0) return STAT_EAGAIN; - if (0 < fabs(arc.linear_travel)) - arc.arc_time = - max(arc.arc_time, - fabs(arc.linear_travel/cm.a[arc.linear_axis].feedrate_max)); + arc.run_state = MOVE_OFF; + + return STAT_OK; +} + + +/// Stop arc movement without maintaining position +/// OK to call if no arc is running +void cm_abort_arc() { + arc.run_state = MOVE_OFF; } diff --git a/src/plan/arc.h b/src/plan/arc.h index 8d29426..ba8bb53 100644 --- a/src/plan/arc.h +++ b/src/plan/arc.h @@ -28,52 +28,7 @@ #pragma once +#include "status.h" -#include "canonical_machine.h" - -// Arc radius tests. -// See http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc - -/// max allowable mm between start and end radius -#define ARC_RADIUS_ERROR_MAX ((float)1.0) -#define ARC_RADIUS_ERROR_MIN ((float)0.005) // min mm where 1% rule applies -#define ARC_RADIUS_TOLERANCE ((float)0.001) // 0.1% radius variance test - -// See planner.h for MM_PER_ARC_SEGMENT and other arc setting #defines - -typedef struct arArcSingleton { // persistent planner and runtime variables - uint8_t run_state; // runtime state machine sequence - - float position[AXES]; // accumulating runtime position - float offset[3]; // IJK offsets - - float length; // length of line or helix in mm - float theta; // total angle specified by arc - float theta_end; - float radius; // Raw R value, or computed via offsets - float angular_travel; // travel along the arc - float linear_travel; // travel along linear axis of arc - float planar_travel; - uint8_t full_circle; // set true if full circle arcs specified - uint32_t rotations; // Full rotations for full circles (P value) - - uint8_t plane_axis_0; // arc plane axis 0 - e.g. X for G17 - uint8_t plane_axis_1; // arc plane axis 1 - e.g. Y for G17 - uint8_t linear_axis; // linear axis (normal to plane) - - float arc_time; // total running time for arc (derived) - float arc_segments; // number of segments in arc or blend - int32_t arc_segment_count; // count of running segments - float arc_segment_theta; // angular motion per segment - float arc_segment_linear_travel; // linear motion per segment - float center_0; // center of circle at plane axis 0 (e.g. X for G17) - float center_1; // center of circle at plane axis 1 (e.g. Y for G17) - - GCodeState_t gm; // state struct is passed for each arc segment. -} arc_t; -extern arc_t arc; - - -void cm_arc_init(); stat_t cm_arc_callback(); void cm_abort_arc(); -- 2.27.0