uint8_t _compute_center_arc()
{
// calculate the theta (angle) of the current point (see header notes)
double theta_start = _get_theta(-gm.arc_offset[gm.plane_axis_0], -gm.arc_offset[gm.plane_axis_1]);
if(isnan(theta_start) == true) { return(TG_ARC_SPECIFICATION_ERROR);}
// calculate the theta (angle) of the target point
double theta_end = _get_theta(
gm.target[gm.plane_axis_0] - gm.arc_offset[gm.plane_axis_0] - gm.position[gm.plane_axis_0],
gm.target[gm.plane_axis_1] - gm.arc_offset[gm.plane_axis_1] - gm.position[gm.plane_axis_1]);
if(isnan(theta_end) == true) { return (TG_ARC_SPECIFICATION_ERROR); }
// ensure that the difference is positive so we have clockwise travel
if (theta_end < theta_start) { theta_end += 2*M_PI; }
// compute angular travel and invert if gcode wants a counterclockwise arc
// if angular travel is zero interpret it as a full circle
double angular_travel = theta_end - theta_start;
if (angular_travel == 0) {
if (gm.motion_mode == MOTION_MODE_CCW_ARC) {
angular_travel -= 2*M_PI;
} else {
angular_travel = 2*M_PI;
}
} else {
if (gm.motion_mode == MOTION_MODE_CCW_ARC) {
angular_travel -= 2*M_PI;
}
}
// Find the radius, calculate travel in the depth axis of the helix,
// and compute the time it should take to perform the move
double radius_tmp = hypot(gm.arc_offset[gm.plane_axis_0], gm.arc_offset[gm.plane_axis_1]);
double linear_travel = gm.target[gm.plane_axis_2] - gm.position[gm.plane_axis_2];
double move_time = _get_arc_time(linear_travel, angular_travel, radius_tmp);
// Trace the arc
set_vector(gm.target[gm.plane_axis_0], gm.target[gm.plane_axis_1], gm.target[gm.plane_axis_2],
gm.target[A], gm.target[B], gm.target[C]);
return(ar_arc(vector,
gm.arc_offset[gm.plane_axis_0],
gm.arc_offset[gm.plane_axis_1],
gm.arc_offset[gm.plane_axis_2],
theta_start, radius_tmp, angular_travel, linear_travel,
gm.plane_axis_0, gm.plane_axis_1, gm.plane_axis_2, move_time));
}
/*
* _compute_arc() - compute arc from I and J (arc center point)
*
* The theta calculation sets up an clockwise or counterclockwise arc from the current
* position to the target position around the center designated by the offset vector.
* All theta-values measured in radians of deviance from the positive y-axis.
*
* | <- theta == 0
* * * *
* * *
* * *
* * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
* * /
* C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
*
* Parts of this routine were originally sourced from the grbl project.
*/
static stat_t _compute_arc()
{
// A non-zero radius value indicates a radius arc
// Compute IJK offset coordinates. These override any current IJK offsets
if (fp_NOT_ZERO(arc.radius)) ritorno(_compute_arc_offsets_from_radius()); // returns if error
// Calculate the theta (angle) of the current point (see header notes)
// Arc.theta is starting point for theta (theta_start)
arc.theta = _get_theta(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]);
if(isnan(arc.theta) == true) return(STAT_ARC_SPECIFICATION_ERROR);
// calculate the theta (angle) of the target point
float theta_end = _get_theta(
arc.gm.target[arc.plane_axis_0] - arc.offset[arc.plane_axis_0] - arc.position[arc.plane_axis_0],
arc.gm.target[arc.plane_axis_1] - arc.offset[arc.plane_axis_1] - arc.position[arc.plane_axis_1]);
if(isnan(theta_end) == true) return (STAT_ARC_SPECIFICATION_ERROR);
// ensure that the difference is positive so we have clockwise travel
if (theta_end < arc.theta) { theta_end += 2*M_PI; }
// compute angular travel and invert if gcode wants a counterclockwise arc
// if angular travel is zero interpret it as a full circle
arc.angular_travel = theta_end - arc.theta;
if (fp_ZERO(arc.angular_travel)) {
if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) {
arc.angular_travel -= 2*M_PI;
} else {
arc.angular_travel = 2*M_PI;
}
} else {
if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) {
arc.angular_travel -= 2*M_PI;
}
}
// Find the radius, calculate travel in the depth axis of the helix,
// and compute the time it should take to perform the move
arc.radius = hypot(arc.offset[arc.plane_axis_0], arc.offset[arc.plane_axis_1]);
arc.linear_travel = arc.gm.target[arc.linear_axis] - arc.position[arc.linear_axis];
// length is the total mm of travel of the helix (or just a planar arc)
arc.length = hypot(arc.angular_travel * arc.radius, fabs(arc.linear_travel));
if (arc.length < cm.arc_segment_len) return (STAT_MINIMUM_LENGTH_MOVE); // arc is too short to draw
arc.time = _get_arc_time(arc.linear_travel, arc.angular_travel, arc.radius);
// Find the minimum number of segments that meets these constraints...
float segments_required_for_chordal_accuracy = arc.length / sqrt(4*cm.chordal_tolerance * (2 * arc.radius - cm.chordal_tolerance));
float segments_required_for_minimum_distance = arc.length / cm.arc_segment_len;
float segments_required_for_minimum_time = arc.time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC;
arc.segments = floor(min3(segments_required_for_chordal_accuracy,
segments_required_for_minimum_distance,
segments_required_for_minimum_time));
arc.segments = max(arc.segments, 1); //...but is at least 1 segment
arc.gm.move_time = arc.time / arc.segments; // gcode state struct gets segment_time, not arc time
arc.segment_count = (int32_t)arc.segments;
arc.segment_theta = arc.angular_travel / arc.segments;
arc.segment_linear_travel = arc.linear_travel / arc.segments;
arc.center_0 = arc.position[arc.plane_axis_0] - sin(arc.theta) * arc.radius;
arc.center_1 = arc.position[arc.plane_axis_1] - cos(arc.theta) * arc.radius;
arc.gm.target[arc.linear_axis] = arc.position[arc.linear_axis]; // initialize the linear target
return (STAT_OK);
}
