/*
* cm_arc_feed() - canonical machine entry point for arc
*
* Generates an arc by queueing line segments to the move buffer. The arc is
* approximated by generating a large number of tiny, linear 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
{
// 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);
}
// Trap conditions where no arc movement will occur, but the system is still in
// arc motion mode - this is not an error. This can happen when a F word or M
// word is by itself.(The tests below are organized for execution efficiency)
if ( fp_ZERO(i) && fp_ZERO(j) && fp_ZERO(k) && fp_ZERO(radius) ) {
if ( fp_ZERO((flags[AXIS_X] + flags[AXIS_Y] + flags[AXIS_Z] +
flags[AXIS_A] + flags[AXIS_B] + flags[AXIS_C]))) {
return (STAT_OK);
}
}
// set values in the Gcode model state & copy it (linenum was already captured)
cm_set_model_target(target, flags);
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
// populate the arc control singleton
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);
// Set the arc plane for the current G17/G18/G19 setting
// Plane axis 0 and 1 are the arc plane, 2 is the linear axis 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;
} 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;
} 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;
}
// compute arc runtime values and prep for execution by the callback
ritorno(_compute_arc());
// 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);
}
/*
* 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);
}
