From 0bd667bd66536164ed07162d159873af1e6f261a Mon Sep 17 00:00:00 2001 From: Stephen Green Date: Tue, 5 Jan 2021 21:39:04 -0500 Subject: [PATCH] Fix for negative arc radius support --- firmware/tinyg/plan_arc.c | 56 +++++++++++++++++++++------------------ 1 file changed, 30 insertions(+), 26 deletions(-) diff --git a/firmware/tinyg/plan_arc.c b/firmware/tinyg/plan_arc.c index 2713641e6..ea6e40d90 100755 --- a/firmware/tinyg/plan_arc.c +++ b/firmware/tinyg/plan_arc.c @@ -77,7 +77,7 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints // set radius mode flag and do simple test(s) bool radius_f = fp_NOT_ZERO(cm.gf.arc_radius); // set true if radius arc - if ((radius_f) && (cm.gn.arc_radius < MIN_ARC_RADIUS)) { // radius value must be + and > minimum radius + if ((radius_f) && (fabs(cm.gn.arc_radius) < MIN_ARC_RADIUS)) { // radius value must > minimum radius return (STAT_ARC_RADIUS_OUT_OF_TOLERANCE); } @@ -243,32 +243,32 @@ void cm_abort_arc() static stat_t _compute_arc() { // Compute radius. A non-zero radius value indicates a radius arc - if (fp_NOT_ZERO(arc.radius)) { // indicates a radius arc - _compute_arc_offsets_from_radius(); - } else { // compute start radius - arc.radius = hypotf(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]); - } - - // Test arc specification for correctness according to: - // http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc - // "It is an error if: when the arc is projected on the selected plane, the distance from - // the current point to the center differs from the distance from the end point to the - // center by more than (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)." - - // Compute end radius from the center of circle (offsets) to target endpoint - float end_0 = arc.gm.target[arc.plane_axis_0] - arc.position[arc.plane_axis_0] - arc.offset[arc.plane_axis_0]; - float end_1 = arc.gm.target[arc.plane_axis_1] - arc.position[arc.plane_axis_1] - arc.offset[arc.plane_axis_1]; - float err = fabs(hypotf(end_0, end_1) - arc.radius); // end radius - start radius - if ( (err > ARC_RADIUS_ERROR_MAX) || - ((err < ARC_RADIUS_ERROR_MIN) && - (err > arc.radius * ARC_RADIUS_TOLERANCE)) ) { -// return (STAT_ARC_HAS_IMPOSSIBLE_CENTER_POINT); - return (STAT_ARC_SPECIFICATION_ERROR); - } + if (fp_NOT_ZERO(arc.radius)) { // indicates a radius arc + _compute_arc_offsets_from_radius(); + } else { // compute start radius + arc.radius = hypotf(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]); + } + + // Test arc specification for correctness according to: + // http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc + // "It is an error if: when the arc is projected on the selected plane, the distance from + // the current point to the center differs from the distance from the end point to the + // center by more than (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)." + + // Compute end radius from the center of circle (offsets) to target endpoint + float end_0 = arc.gm.target[arc.plane_axis_0] - arc.position[arc.plane_axis_0] - arc.offset[arc.plane_axis_0]; + float end_1 = arc.gm.target[arc.plane_axis_1] - arc.position[arc.plane_axis_1] - arc.offset[arc.plane_axis_1]; + float err = fabs(hypotf(end_0, end_1) - arc.radius); // end radius - start radius + if ( (err > ARC_RADIUS_ERROR_MAX) || + ((err < ARC_RADIUS_ERROR_MIN) && + (err > arc.radius * ARC_RADIUS_TOLERANCE)) ) { +// return (STAT_ARC_HAS_IMPOSSIBLE_CENTER_POINT); + return (STAT_ARC_SPECIFICATION_ERROR); + } // Calculate the theta (angle) of the current point (position) // arc.theta is angular starting point for the arc (also needed later for calculating center point) - arc.theta = atan2(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]); + arc.theta = atan2(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]); // g18_correction is used to invert G18 XZ plane arcs for proper CW orientation float g18_correction = (cm.gm.select_plane == CANON_PLANE_XZ) ? -1 : 1; @@ -433,12 +433,16 @@ static stat_t _compute_arc_offsets_from_radius() // 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; } + if (arc.radius < 0) { + h_x2_div_d = -h_x2_div_d; + arc.radius *= -1; + } // 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; + return (STAT_OK); } @@ -557,4 +561,4 @@ static stat_t _test_arc_soft_limits() return(STAT_OK); } -*/ \ No newline at end of file +*/