Mirrors_Framework/g2
1
0
Fork 0
mirror of https://github.com/synthetos/g2.git synced 2026-02-06 02:51:54 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Moved arc structure into canonical machine; Testing refactored canonical machine settings

Browse Source
This commit is contained in:
Alden Hart
2016-12-18 08:51:35 -05:00
parent 85044a216c
commit 642236dbd9
5 changed files with 148 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

196
g2core/plan_arc.cpp
View File

@@ -26,7 +26,7 @@
// Allocate arc planner singleton structure
arc_t arc;
//arc_t arc;
// Local functions
@@ -49,8 +49,8 @@ static stat_t _test_arc_soft_limits(void);
*/
void cm_arc_init()
{
arc.magic_start = MAGICNUM;
arc.magic_end = MAGICNUM;
cm->arc.magic_start = MAGICNUM;
cm->arc.magic_end = MAGICNUM;
}
/*
@@ -61,7 +61,7 @@ void cm_arc_init()
void cm_abort_arc()
{
arc.run_state = BLOCK_INACTIVE;
cm->arc.run_state = BLOCK_INACTIVE;
}
/*
@@ -75,24 +75,24 @@ void cm_abort_arc()
stat_t cm_arc_callback()
{
if (arc.run_state == BLOCK_INACTIVE) {
if (cm->arc.run_state == BLOCK_INACTIVE) {
return (STAT_NOOP);
}
if (mp_planner_is_full(ACTIVE_Q)) {
return (STAT_EAGAIN);
}
arc.theta += 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.segment_linear_travel;
cm->arc.theta += cm->arc.segment_theta;
cm->arc.gm.target[cm->arc.plane_axis_0] = cm->arc.center_0 + sin(cm->arc.theta) * cm->arc.radius;
cm->arc.gm.target[cm->arc.plane_axis_1] = cm->arc.center_1 + cos(cm->arc.theta) * cm->arc.radius;
cm->arc.gm.target[cm->arc.linear_axis] += cm->arc.segment_linear_travel;
mp_aline(&arc.gm); // run the line
copy_vector(arc.position, arc.gm.target); // update arc current position
mp_aline(&(cm->arc.gm)); // run the line
copy_vector(cm->arc.position, cm->arc.gm.target); // update arc current position
if (--arc.segment_count > 0) {
if (--(cm->arc.segment_count) > 0) {
return (STAT_EAGAIN);
}
arc.run_state = BLOCK_INACTIVE;
cm->arc.run_state = BLOCK_INACTIVE;
return (STAT_OK);
}
@@ -142,41 +142,41 @@ stat_t cm_arc_feed(const float target[], const bool target_f[], // target en
// 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;
cm->arc.plane_axis_0 = AXIS_X;
cm->arc.plane_axis_1 = AXIS_Y;
cm->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;
cm->arc.plane_axis_0 = AXIS_X;
cm->arc.plane_axis_1 = AXIS_Z;
cm->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;
cm->arc.plane_axis_0 = AXIS_Y;
cm->arc.plane_axis_1 = AXIS_Z;
cm->arc.linear_axis = AXIS_X;
} else {
return(cm_panic(STAT_ACTIVE_PLANE_IS_MISSING, "cm_arc_feed() impossible value")); // plane axis has impossible value
}
// test if no endpoints are specified in the selected plane
arc.full_circle = false; // initial condition
if (!(target_f[arc.plane_axis_0] || target_f[arc.plane_axis_1])) {
cm->arc.full_circle = false; // initial condition
if (!(target_f[cm->arc.plane_axis_0] || target_f[cm->arc.plane_axis_1])) {
if (radius_f) { // in radius mode arcs missing both endpoints is an error
return (STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE);
} else {
arc.full_circle = true; // in center format arc this specifies a full circle
cm->arc.full_circle = true; // in center format arc this specifies a full circle
}
}
// test radius arcs for radius tolerance
if (radius_f) {
arc.radius = _to_millimeters(radius); // set radius to internal format (mm)
if (fabs(arc.radius) < MIN_ARC_RADIUS) { // radius value must be > minimum radius
cm->arc.radius = _to_millimeters(radius); // set radius to internal format (mm)
if (fabs(cm->arc.radius) < MIN_ARC_RADIUS) { // radius value must be > minimum radius
return (STAT_ARC_RADIUS_OUT_OF_TOLERANCE);
}
}
else { // test that center format absolute distance mode arcs have both offsets specified
if (cm->gm.arc_distance_mode == ABSOLUTE_MODE) {
if (!(offset_f[arc.plane_axis_0] && offset_f[arc.plane_axis_1])) { // if one or both offsets are missing
if (!(offset_f[cm->arc.plane_axis_0] && offset_f[cm->arc.plane_axis_1])) { // if one or both offsets are missing
return (STAT_ARC_OFFSETS_MISSING_FOR_SELECTED_PLANE);
}
}
@@ -190,12 +190,12 @@ stat_t cm_arc_feed(const float target[], const bool target_f[], // target en
if (floor(P_word) - (P_word) > 0) {
return (STAT_P_WORD_IS_NOT_AN_INTEGER);
}
arc.rotations = P_word;
cm->arc.rotations = P_word;
} else {
if (arc.full_circle) { // arc rotations default to 1 for full circles
arc.rotations = 1;
if (cm->arc.full_circle) { // arc rotations default to 1 for full circles
cm->arc.rotations = 1;
} else {
arc.rotations = 0; // no rotations
cm->arc.rotations = 0; // no rotations
}
}
@@ -213,24 +213,24 @@ stat_t cm_arc_feed(const float target[], const bool target_f[], // target en
// *** 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
cm_set_work_offsets(&cm->gm); // capture the fully resolved offsets to gm
memcpy(&(cm->arc.gm), &cm->gm, sizeof(GCodeState_t)); // copy GCode context to arc singleton - some will be overwritten to run segments
copy_vector(cm->arc.position, cm->gmx.position); // set initial arc position from gcode model
// setup offsets
arc.offset[OFS_I] = _to_millimeters(offset[OFS_I]); // copy offsets with conversion to canonical form (mm)
arc.offset[OFS_J] = _to_millimeters(offset[OFS_J]);
arc.offset[OFS_K] = _to_millimeters(offset[OFS_K]);
cm->arc.offset[OFS_I] = _to_millimeters(offset[OFS_I]); // copy offsets with conversion to canonical form (mm)
cm->arc.offset[OFS_J] = _to_millimeters(offset[OFS_J]);
cm->arc.offset[OFS_K] = _to_millimeters(offset[OFS_K]);
if (arc.gm.arc_distance_mode == ABSOLUTE_MODE) { // adjust offsets if in absolute mode
arc.offset[OFS_I] -= arc.position[AXIS_X];
arc.offset[OFS_J] -= arc.position[AXIS_Y];
arc.offset[OFS_K] -= arc.position[AXIS_Z];
if (cm->arc.gm.arc_distance_mode == ABSOLUTE_MODE) { // adjust offsets if in absolute mode
cm->arc.offset[OFS_I] -= cm->arc.position[AXIS_X];
cm->arc.offset[OFS_J] -= cm->arc.position[AXIS_Y];
cm->arc.offset[OFS_K] -= cm->arc.position[AXIS_Z];
}
if ((fp_ZERO(arc.offset[OFS_I])) && // it's an error if no offsets are provided
(fp_ZERO(arc.offset[OFS_J])) &&
(fp_ZERO(arc.offset[OFS_K]))) {
if ((fp_ZERO(cm->arc.offset[OFS_I])) && // it's an error if no offsets are provided
(fp_ZERO(cm->arc.offset[OFS_J])) &&
(fp_ZERO(cm->arc.offset[OFS_K]))) {
return (cm_alarm(STAT_ARC_OFFSETS_MISSING_FOR_SELECTED_PLANE, "arc offsets missing or zero"));
}
@@ -241,12 +241,12 @@ stat_t cm_arc_feed(const float target[], const bool target_f[], // target en
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, arc.position); // reset model position
return (cm_alarm(status, "arc soft_limits")); // throw an alarm
copy_vector(cm->gm.target, cm->arc.position); // reset model position
return (cm_alarm(status, "arc soft_limits")); // throw an alarm
}
cm_cycle_start(); // if not already started
arc.run_state = BLOCK_ACTIVE; // enable arc to be run from the callback
cm_cycle_start(); // if not already started
cm->arc.run_state = BLOCK_ACTIVE; // enable arc to be run from the callback
cm_finalize_move();
return (STAT_OK);
}
@@ -275,7 +275,7 @@ static stat_t _compute_arc(const bool radius_f)
if (radius_f) { // 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]);
cm->arc.radius = hypotf(-cm->arc.offset[cm->arc.plane_axis_0], -cm->arc.offset[cm->arc.plane_axis_1]);
}
// Test arc specification for correctness according to:
@@ -285,68 +285,74 @@ static stat_t _compute_arc(const bool radius_f)
// 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
float end_0 = cm->arc.gm.target[cm->arc.plane_axis_0] -
cm->arc.position[cm->arc.plane_axis_0] -
cm->arc.offset[cm->arc.plane_axis_0];
float end_1 = cm->arc.gm.target[cm->arc.plane_axis_1] -
cm->arc.position[cm->arc.plane_axis_1] -
cm->arc.offset[cm->arc.plane_axis_1];
float err = fabs(hypotf(end_0, end_1) - cm->arc.radius); // end radius - start radius
if ((err > ARC_RADIUS_ERROR_MAX) ||
((err > ARC_RADIUS_ERROR_MIN) && (err > arc.radius * ARC_RADIUS_TOLERANCE))) {
((err > ARC_RADIUS_ERROR_MIN) && (err > cm->arc.radius * ARC_RADIUS_TOLERANCE))) {
return (STAT_ARC_HAS_IMPOSSIBLE_CENTER_POINT);
}
// Compute the angular travel
// Calculate the theta angle of the current position (theta is also needed for calculating center point)
// Note: gcc atan2 reverses args, i.e.: atan2(Y,X)
arc.theta = atan2(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]);
cm->arc.theta = atan2(-cm->arc.offset[cm->arc.plane_axis_0], -cm->arc.offset[cm->arc.plane_axis_1]);
// Compute angular travel if not a full circle arc
if (!arc.full_circle) {
arc.angular_travel = atan2(end_0, end_1) - arc.theta; // travel = theta_end - theta_start
if (!cm->arc.full_circle) {
cm->arc.angular_travel = atan2(end_0, end_1) - cm->arc.theta; // travel = theta_end - theta_start
// correct for atan2 output quadrants
if (arc.gm.motion_mode == MOTION_MODE_CW_ARC) {
if (arc.angular_travel <= 0) { arc.angular_travel += 2*M_PI; }
if (cm->arc.gm.motion_mode == MOTION_MODE_CW_ARC) {
if (cm->arc.angular_travel <= 0) { cm->arc.angular_travel += 2*M_PI; }
} else {
if (arc.angular_travel > 0) { arc.angular_travel -= 2*M_PI; }
if (cm->arc.angular_travel > 0) { cm->arc.angular_travel -= 2*M_PI; }
}
// add in travel for rotations
if (arc.angular_travel >= 0) { arc.angular_travel += 2*M_PI * arc.rotations; }
else { arc.angular_travel -= 2*M_PI * arc.rotations; }
if (cm->arc.angular_travel >= 0) { cm->arc.angular_travel += 2*M_PI * cm->arc.rotations; }
else { cm->arc.angular_travel -= 2*M_PI * cm->arc.rotations; }
}
// Compute full-circle arcs
else {
if (arc.gm.motion_mode == MOTION_MODE_CCW_ARC) { arc.rotations *= -1; }
arc.angular_travel = 2 * M_PI * arc.rotations;
if (cm->arc.gm.motion_mode == MOTION_MODE_CCW_ARC) { cm->arc.rotations *= -1; }
cm->arc.angular_travel = 2 * M_PI * cm->arc.rotations;
}
// Trap zero movement arcs
if (fp_ZERO(arc.angular_travel)) {
if (fp_ZERO(cm->arc.angular_travel)) {
return (STAT_ARC_ENDPOINT_IS_STARTING_POINT);
}
// Calculate travel in the plane and the depth axis of the helix
// Length is the total mm of travel of the helix (or just the 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, fabs(arc.linear_travel));
cm->arc.linear_travel = cm->arc.gm.target[cm->arc.linear_axis] - cm->arc.position[cm->arc.linear_axis];
cm->arc.planar_travel = cm->arc.angular_travel * cm->arc.radius;
cm->arc.length = hypotf(cm->arc.planar_travel, fabs(cm->arc.linear_travel));
// Find the minimum number of segments that meet accuracy and time constraints...
// Note: removed segment_length test as segment_time accounts for this (build 083.37)
float arc_time;
float segments_for_minimum_time = _estimate_arc_time(arc_time) * (MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC);
float segments_for_chordal_accuracy = arc.length / sqrt(4*cm->chordal_tolerance * (2 * arc.radius - cm->chordal_tolerance));
arc.segments = floor(min(segments_for_chordal_accuracy, segments_for_minimum_time));
arc.segments = max(arc.segments, (float)1.0); //...but is at least 1 segment
float segments_for_chordal_accuracy = cm->arc.length / sqrt(4*cm->chordal_tolerance * (2 * cm->arc.radius - cm->chordal_tolerance));
cm->arc.segments = floor(min(segments_for_chordal_accuracy, segments_for_minimum_time));
cm->arc.segments = max(cm->arc.segments, (float)1.0); //...but is at least 1 segment
if (arc.gm.feed_rate_mode == INVERSE_TIME_MODE) {
arc.gm.feed_rate /= arc.segments;
if (cm->arc.gm.feed_rate_mode == INVERSE_TIME_MODE) {
cm->arc.gm.feed_rate /= cm->arc.segments;
}
// setup the rest of the arc parameters
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
cm->arc.segment_count = (int32_t)cm->arc.segments;
cm->arc.segment_theta = cm->arc.angular_travel / cm->arc.segments;
cm->arc.segment_linear_travel = cm->arc.linear_travel / cm->arc.segments;
cm->arc.center_0 = cm->arc.position[cm->arc.plane_axis_0] - sin(cm->arc.theta) * cm->arc.radius;
cm->arc.center_1 = cm->arc.position[cm->arc.plane_axis_1] - cos(cm->arc.theta) * cm->arc.radius;
cm->arc.gm.target[cm->arc.linear_axis] = cm->arc.position[cm->arc.linear_axis]; // initialize the linear target
return (STAT_OK);
}
@@ -428,8 +434,8 @@ static stat_t _compute_arc(const bool radius_f)
static void _compute_arc_offsets_from_radius()
{
// Calculate the change in position along each selected axis
float x = arc.gm.target[arc.plane_axis_0] - arc.position[arc.plane_axis_0];
float y = arc.gm.target[arc.plane_axis_1] - arc.position[arc.plane_axis_1];
float x = cm->arc.gm.target[cm->arc.plane_axis_0] - cm->arc.position[cm->arc.plane_axis_0];
float y = cm->arc.gm.target[cm->arc.plane_axis_1] - cm->arc.position[cm->arc.plane_axis_1];
// *** From Forrest Green - Other Machine Co, 3/27/14
// If the distance between endpoints is greater than the arc diameter, disc will be
@@ -440,13 +446,13 @@ static void _compute_arc_offsets_from_radius()
// 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));
float disc = 4 * square(cm->arc.radius) - (square(x) + square(y));
// h_x2_div_d == -(h * 2 / d)
float h_x2_div_d = (disc > 0) ? -sqrt(disc) / hypotf(x,y) : 0;
// Invert the sign of h_x2_div_d if circle is counter clockwise (see header notes)
if (arc.gm.motion_mode == MOTION_MODE_CCW_ARC) {
if (cm->arc.gm.motion_mode == MOTION_MODE_CCW_ARC) {
h_x2_div_d = -h_x2_div_d;
}
@@ -455,15 +461,15 @@ static void _compute_arc_offsets_from_radius()
// single Gcode block. 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 inadvisably
// long arcs as prescribed.
if (arc.radius < 0) {
if (cm->arc.radius < 0) {
h_x2_div_d = -h_x2_div_d;
arc.radius *= -1; // and flip the radius sign while you are at it
cm->arc.radius *= -1; // and flip the radius sign while you are at it
}
// 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;
cm->arc.offset[cm->arc.plane_axis_0] = (x-(y*h_x2_div_d))/2;
cm->arc.offset[cm->arc.plane_axis_1] = (y+(x*h_x2_div_d))/2;
cm->arc.offset[cm->arc.linear_axis] = 0;
}
/*
@@ -479,17 +485,17 @@ static void _compute_arc_offsets_from_radius()
static float _estimate_arc_time (float arc_time)
{
// Determine move time at requested feed rate
if (arc.gm.feed_rate_mode == INVERSE_TIME_MODE) {
arc_time = arc.gm.feed_rate; // inverse feed rate has been normalized to minutes
if (cm->arc.gm.feed_rate_mode == INVERSE_TIME_MODE) {
arc_time = cm->arc.gm.feed_rate; // inverse feed rate has been normalized to minutes
} else {
arc_time = arc.length / cm->gm.feed_rate;
arc_time = cm->arc.length / cm->gm.feed_rate;
}
// Downgrade the time if there is a rate-limiting axis
arc_time = max(arc_time, (float)fabs(arc.planar_travel/cm->a[arc.plane_axis_0].feedrate_max));
arc_time = max(arc_time, (float)fabs(arc.planar_travel/cm->a[arc.plane_axis_1].feedrate_max));
if (fabs(arc.linear_travel) > 0) {
arc_time = max(arc_time, (float)fabs(arc.linear_travel/cm->a[arc.linear_axis].feedrate_max));
arc_time = max(arc_time, (float)fabs(cm->arc.planar_travel/cm->a[cm->arc.plane_axis_0].feedrate_max));
arc_time = max(arc_time, (float)fabs(cm->arc.planar_travel/cm->a[cm->arc.plane_axis_1].feedrate_max));
if (fabs(cm->arc.linear_travel) > 0) {
arc_time = max(arc_time, (float)fabs(cm->arc.linear_travel/cm->a[cm->arc.linear_axis].feedrate_max));
}
return (arc_time);
}