uint8_t cm_straight_probe(float target[], float flags[])
{
// trap zero feed rate condition
if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) {
return (STAT_GCODE_FEEDRATE_NOT_SPECIFIED);
}
// error if no axes specified
if (fp_NOT_ZERO(flags[AXIS_X]) && fp_NOT_ZERO(flags[AXIS_Y]) && fp_NOT_ZERO(flags[AXIS_Z])) {
return (STAT_GCODE_AXIS_IS_MISSING);
}
// set probe move endpoint
copy_vector(pb.target, target); // set probe move endpoint
copy_vector(pb.flags, flags); // set axes involved on the move
clear_vector(cm.probe_results); // clear the old probe position.
// NOTE: relying on probe_result will not detect a probe to 0,0,0.
cm.probe_state = PROBE_WAITING; // wait until planner queue empties before completing initialization
pb.func = _probing_init; // bind probing initialization function
return (STAT_OK);
}
/*
* _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);
}
void mp_calculate_trapezoid(mpBuf_t *bf)
{
//********************************************
//********************************************
//** RULE #1 of mp_calculate_trapezoid() **
//** DON'T CHANGE bf->length **
//********************************************
//********************************************
// F case: Block is too short - run time < minimum segment time
// Force block into a single segment body with limited velocities
// Accept the entry velocity, limit the cruise, and go for the best exit velocity
// you can get given the delta_vmax (maximum velocity slew) supportable.
bf->naiive_move_time = 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average
if (bf->naiive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) {
bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN;
bf->exit_velocity = max(0.0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax)));
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
// We are violating the jerk value but since it's a single segment move we don't use it.
return;
}
// B" case: Block is short, but fits into a single body segment
if (bf->naiive_move_time <= NOM_SEGMENT_TIME) {
bf->entry_velocity = bf->pv->exit_velocity;
if (fp_NOT_ZERO(bf->entry_velocity)) {
bf->cruise_velocity = bf->entry_velocity;
bf->exit_velocity = bf->entry_velocity;
} else {
bf->cruise_velocity = bf->delta_vmax / 2;
bf->exit_velocity = bf->delta_vmax;
}
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
// We are violating the jerk value but since it's a single segment move we don't use it.
return;
}
// B case: Velocities all match (or close enough)
// This occurs frequently in normal gcode files with lots of short lines
// This case is not really necessary, but saves lots of processing time
if (((bf->cruise_velocity - bf->entry_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) &&
((bf->cruise_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE)) {
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
return;
}
// Head-only and tail-only short-line cases
// H" and T" degraded-fit cases
// H' and T' requested-fit cases where the body residual is less than MIN_BODY_LENGTH
bf->body_length = 0;
float minimum_length = mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf);
if (bf->length <= (minimum_length + MIN_BODY_LENGTH)) { // head-only & tail-only cases
if (bf->entry_velocity > bf->exit_velocity) { // tail-only cases (short decelerations)
if (bf->length < minimum_length) { // T" (degraded case)
bf->entry_velocity = mp_get_target_velocity(bf->exit_velocity, bf->length, bf);
}
bf->cruise_velocity = bf->entry_velocity;
bf->tail_length = bf->length;
bf->head_length = 0;
return;
}
if (bf->entry_velocity < bf->exit_velocity) { // head-only cases (short accelerations)
if (bf->length < minimum_length) { // H" (degraded case)
bf->exit_velocity = mp_get_target_velocity(bf->entry_velocity, bf->length, bf);
}
bf->cruise_velocity = bf->exit_velocity;
bf->head_length = bf->length;
bf->tail_length = 0;
return;
}
}
// Set head and tail lengths for evaluating the next cases
bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf);
bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf);
if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;}
if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;}
// Rate-limited HT and HT' cases
if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited
// Symmetric rate-limited case (HT)
if (fabs(bf->entry_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) {
bf->head_length = bf->length/2;
bf->tail_length = bf->head_length;
bf->cruise_velocity = min(bf->cruise_vmax, mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf));
if (bf->head_length < MIN_HEAD_LENGTH) {
// Convert this to a body-only move
bf->body_length = bf->length;
bf->head_length = 0;
bf->tail_length = 0;
// Average the entry speed and computed best cruise-speed
bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity)/2;
bf->entry_velocity = bf->cruise_velocity;
bf->exit_velocity = bf->cruise_velocity;
}
return;
}
// Asymmetric HT' rate-limited case. This is relatively expensive but it's not called very often
// iteration trap: uint8_t i=0;
// iteration trap: if (++i > TRAPEZOID_ITERATION_MAX) { fprintf_P(stderr,PSTR("_calculate_trapezoid() failed to converge"));}
float computed_velocity = bf->cruise_vmax;
do {
bf->cruise_velocity = computed_velocity; // initialize from previous iteration
bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf);
bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf);
if (bf->head_length > bf->tail_length) {
bf->head_length = (bf->head_length / (bf->head_length + bf->tail_length)) * bf->length;
computed_velocity = mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf);
} else {
bf->tail_length = (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length;
computed_velocity = mp_get_target_velocity(bf->exit_velocity, bf->tail_length, bf);
}
// insert iteration trap here if needed
} while ((fabs(bf->cruise_velocity - computed_velocity) / computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT);
// set velocity and clean up any parts that are too short
bf->cruise_velocity = computed_velocity;
bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf);
bf->tail_length = bf->length - bf->head_length;
if (bf->head_length < MIN_HEAD_LENGTH) {
bf->tail_length = bf->length; // adjust the move to be all tail...
bf->head_length = 0;
}
if (bf->tail_length < MIN_TAIL_LENGTH) {
bf->head_length = bf->length; //...or all head
bf->tail_length = 0;
}
return;
}
// Requested-fit cases: remaining of: HBT, HB, BT, BT, H, T, B, cases
bf->body_length = bf->length - bf->head_length - bf->tail_length;
// If a non-zero body is < minimum length distribute it to the head and/or tail
// This will generate small (acceptable) velocity errors in runtime execution
// but preserve correct distance, which is more important.
if ((bf->body_length < MIN_BODY_LENGTH) && (fp_NOT_ZERO(bf->body_length))) {
if (fp_NOT_ZERO(bf->head_length)) {
if (fp_NOT_ZERO(bf->tail_length)) { // HBT reduces to HT
bf->head_length += bf->body_length/2;
bf->tail_length += bf->body_length/2;
} else { // HB reduces to H
bf->head_length += bf->body_length;
}
} else { // BT reduces to T
bf->tail_length += bf->body_length;
}
bf->body_length = 0;
// If the body is a standalone make the cruise velocity match the entry velocity
// This removes a potential velocity discontinuity at the expense of top speed
} else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length))) {
bf->cruise_velocity = bf->entry_velocity;
}
}
/*
* cm_arc_feed() - 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
if ((radius_f) && (cm.gn.arc_radius < MIN_ARC_RADIUS)) { // radius value must be + and > minimum radius
return (STAT_ARC_RADIUS_OUT_OF_TOLERANCE);
}
// setup some flags
bool target_x = fp_NOT_ZERO(flags[AXIS_X]); // set true if X axis has been 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]); // set true if offset I has been 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.
if (cm.gm.select_plane == CANON_PLANE_XY) { // G17 - the vast majority of arcs are in the G17 (XY) plane
arc.plane_axis_0 = AXIS_X;
arc.plane_axis_1 = AXIS_Y;
arc.linear_axis = AXIS_Z;
if (radius_f) {
if (!(target_x || target_y)) { // must have at least one endpoint specified
return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE);
}
} else { // center format arc tests
if (offset_k) { // 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) {
if ((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); // capture the fully resolved offsets to gm
memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // copy GCode context to arc singleton - some will be overwritten to run segments
copy_vector(arc.position, cm.gmx.position); // set initial arc position from gcode model
arc.radius = _to_millimeters(radius); // set arc radius or zero
arc.offset[0] = _to_millimeters(i); // copy offsets with conversion to 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 a positive integer - force it if not
// 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());
if (fp_ZERO(arc.length)) {
return (STAT_MINIMUM_LENGTH_MOVE); // trap zero length arcs that _compute_arc can throw
}
/* // test arc soft limits
stat_t status = _test_arc_soft_limits();
if (status != STAT_OK) {
cm.gm.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE;
copy_vector(cm.gm.target, cm.gmx.position); // reset model position
return (cm_soft_alarm(status));
}
*/
cm_cycle_start(); // if not already started
arc.run_state = MOVE_RUN; // enable arc to be run from the callback
cm_finalize_move();
return (STAT_OK);
}
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);
}
// 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]);
// 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;
if (arc.full_circle) { // if full circle you can skip the stuff in the else clause
arc.angular_travel = 0; // angular travel always starts as zero for full circles
if (fp_ZERO(arc.rotations)) { // handle the valid case of a full circle arc w/P=0
arc.rotations = 1.0;
}
} else { // ... it's not a full circle
arc.theta_end = atan2(end_0, end_1);
// Compute the angular travel
if (fp_EQ(arc.theta_end, arc.theta)) {
arc.angular_travel = 0; // very large radii arcs can have zero angular travel (thanks PartKam)
} else {
if (arc.theta_end < arc.theta) { // make the difference positive so we have clockwise travel
arc.theta_end += (2*M_PI * g18_correction);
}
arc.angular_travel = arc.theta_end - arc.theta; // compute positive angular travel
if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) { // reverse travel direction if it's CCW arc
arc.angular_travel -= (2*M_PI * g18_correction);
}
}
}
// Add in travel for rotations
if (cm.gm.motion_mode == MOTION_MODE_CW_ARC) {
arc.angular_travel += (2*M_PI * arc.rotations * g18_correction);
} else {
arc.angular_travel -= (2*M_PI * arc.rotations * g18_correction);
}
// Calculate travel in the depth axis of the helix and compute the time it should take to perform the move
// arc.length is the total mm of travel of the helix (or just a planar arc)
arc.linear_travel = arc.gm.target[arc.linear_axis] - arc.position[arc.linear_axis];
arc.planar_travel = arc.angular_travel * arc.radius;
arc.length = hypotf(arc.planar_travel, arc.linear_travel); // NB: hypot is insensitive to +/- signs
_estimate_arc_time(); // get an estimate of execution time to inform arc_segment calculation
// Find the minimum number of arc_segments that meets these constraints...
float arc_segments_for_chordal_accuracy = arc.length / sqrt(4*cm.chordal_tolerance * (2 * arc.radius - cm.chordal_tolerance));
float arc_segments_for_minimum_distance = arc.length / cm.arc_segment_len;
float arc_segments_for_minimum_time = arc.arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC;
arc.arc_segments = floor(min3(arc_segments_for_chordal_accuracy,
arc_segments_for_minimum_distance,
arc_segments_for_minimum_time));
arc.arc_segments = max(arc.arc_segments, 1); //...but is at least 1 arc_segment
arc.gm.move_time = arc.arc_time / arc.arc_segments; // gcode state struct gets arc_segment_time, not arc time
arc.arc_segment_count = (int32_t)arc.arc_segments;
arc.arc_segment_theta = arc.angular_travel / arc.arc_segments;
arc.arc_segment_linear_travel = arc.linear_travel / arc.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);
}
