uint8_t cm_set_feed_rate(double feed_rate)
{
if (gm.inverse_feed_rate_mode == true) {
gm.inverse_feed_rate = feed_rate; // minutes per motion for this block only
} else {
gm.feed_rate = _to_millimeters(feed_rate);
}
return (TG_OK);
}
uint8_t cm_set_origin_offsets(double offset[], double flag[]) // G92
{
gm.origin_offset_mode = true;
for (uint8_t i=0; i<AXES; i++) {
if (flag[i] > EPSILON) { // behaves according to NIST 3.5.18
gm.origin_offset[i] = gm.position[i] - cfg.offset[gm.coord_system][i]
- _to_millimeters(offset[i]);
}
}
return (TG_OK);
}
void cm_set_target(double target[], double flag[])
{
uint8_t i;
double length;
double tmp = 0;
// process XYZABC for lower modes
for (i=X; i<=Z; i++) {
if ((flag[i] < EPSILON) || (cfg.a[i].axis_mode == AXIS_DISABLED)) {
continue;
} else if ((cfg.a[i].axis_mode == AXIS_STANDARD) || (cfg.a[i].axis_mode == AXIS_INHIBITED)) {
if (gm.distance_mode == ABSOLUTE_MODE) {
gm.target[i] = cm_get_coord_offset(i) + _to_millimeters(target[i]);
} else {
gm.target[i] += _to_millimeters(target[i]);
}
// } else if (i<A) {
// printf_P(stderr,PSTR("%c axis using unsupported axis mode"), cfg_get_configuration_group_char(i));
}
}
// FYI: The ABC loop below relies on the XYZ loop having been run first
for (i=A; i<=C; i++) {
// skip axis if not flagged for update or its disabled
if ((flag[i] < EPSILON) || (cfg.a[i].axis_mode == AXIS_DISABLED)) {
continue;
} else if ((cfg.a[i].axis_mode == AXIS_STANDARD) || (cfg.a[i].axis_mode == AXIS_INHIBITED)) {
tmp = target[i]; // no mm conversion - it's in degrees
} else if ((cfg.a[i].axis_mode == AXIS_RADIUS) && (flag[i] > EPSILON)) {
tmp = _to_millimeters(target[i]) * 360 / (2 * M_PI * cfg.a[i].radius);
} else if ((cfg.a[i].axis_mode == AXIS_SLAVE_X) && (flag[X] > EPSILON)) {
tmp = (target[X] - gm.position[X]) * 360 / (2 * M_PI * cfg.a[i].radius);
} else if ((cfg.a[i].axis_mode == AXIS_SLAVE_Y) && (flag[Y] > EPSILON)) {
tmp = (target[Y] - gm.position[Y]) * 360 / (2 * M_PI * cfg.a[i].radius);
} else if ((cfg.a[i].axis_mode == AXIS_SLAVE_Z) && (flag[Z] > EPSILON)) {
tmp = (target[Z] - gm.position[Z]) * 360 / (2 * M_PI * cfg.a[i].radius);
} else if ((cfg.a[i].axis_mode == AXIS_SLAVE_XY) &&
((flag[X] > EPSILON) || (flag[Y] > EPSILON))) {
length = sqrt(square(target[X] - gm.position[X]) +
square(target[Y] - gm.position[Y]));
tmp = length * 360 / (2 * M_PI * cfg.a[i].radius);
} else if ((cfg.a[i].axis_mode == AXIS_SLAVE_XZ) &&
((flag[X] > EPSILON) || (flag[Z] > EPSILON))) {
length = sqrt(square(target[X] - gm.position[X]) +
square(target[Z] - gm.position[Z]));
tmp = length * 360 / (2 * M_PI * cfg.a[i].radius);
} else if ((cfg.a[i].axis_mode == AXIS_SLAVE_YZ) &&
((flag[Y] > EPSILON) || (flag[Z] > EPSILON))) {
length = sqrt(square(target[Y] - gm.position[Y]) +
square(target[Z] - gm.position[Z]));
tmp = length * 360 / (2 * M_PI * cfg.a[i].radius);
} else if ((cfg.a[i].axis_mode == AXIS_SLAVE_XYZ) &&
((flag[X] > EPSILON) || (flag[Y] > EPSILON) || (flag[Z] > EPSILON))) {
length = sqrt(square(target[X] - gm.position[X]) +
square(target[Y] - gm.position[Y]) +
square(target[Z] - gm.position[Z]));
tmp = length * 360 / (2 * M_PI * cfg.a[i].radius);
}
if (gm.distance_mode == ABSOLUTE_MODE) {
gm.target[i] = tmp;
} else {
gm.target[i] += tmp;
}
}
}
void cm_set_arc_radius(double r)
{
gm.arc_radius = _to_millimeters(r);
}
void cm_set_arc_offset(double i, double j, double k)
{
gm.arc_offset[0] = _to_millimeters(i);
gm.arc_offset[1] = _to_millimeters(j);
gm.arc_offset[2] = _to_millimeters(k);
}
/*
* 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);
}
