/* 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 <math.h>
#include <stdbool.h>
#include <string.h>
-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;
}
}
-/* 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;
}
projects / bbctrl-firmware / commitdiff