From 3437a4c4b1e6b42f0b89e7ba6aa71c819d241287 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 7 Feb 2016 15:32:55 -0800 Subject: [PATCH] More cleanup --- src/plan/arc.c | 219 +++++++++++---------------- src/plan/exec.c | 36 ++--- src/plan/line.c | 51 +++---- src/plan/planner.c | 127 ++++++++-------- src/plan/planner.h | 29 ++-- src/plan/zoid.c | 369 +++++++++++++++++++++++---------------------- src/stepper.c | 2 +- 7 files changed, 397 insertions(+), 436 deletions(-) diff --git a/src/plan/arc.c b/src/plan/arc.c index 07d82ba..9ce7925 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -31,29 +31,19 @@ #include #include -// Allocate arc planner singleton structure - arc_t arc; -// Local functions static stat_t _compute_arc(); static stat_t _compute_arc_offsets_from_radius(); static void _estimate_arc_time(); -/***************************************************************************** - * Canonical Machining arc functions (arc prep for planning and runtime) - * - * cm_arc_init() - initialize arcs - * cm_arc_feed() - canonical machine entry point for arc - * cm_arc_callback() - mail-loop callback for arc generation - * cm_abort_arc() - stop an arc in process - */ /// Initialize arc structures void cm_arc_init() {} + /* - * cm_arc_feed() - canonical machine entry point for arc + * 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. @@ -61,28 +51,24 @@ void cm_arc_init() {} 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 -{ - //////////////////////////////////////////////////// + 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))) { + 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 + 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_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_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 @@ -92,62 +78,52 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints 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; - } - } + // must have at least one endpoint specified + if (!(target_x || target_y)) return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; + + } else if (offset_k) // center format arc tests, 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; - } + 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; - } + 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; - } - } + if (radius_f && 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 + 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.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[0] = _to_millimeters(i); // copy offsets with conversion to canonical form (mm) arc.offset[1] = _to_millimeters(j); arc.offset[2] = _to_millimeters(k); @@ -159,59 +135,54 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints // 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 - } + // trap zero length arcs that _compute_arc can throw + if (fp_ZERO(arc.length)) return STAT_MINIMUM_LENGTH_MOVE; 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_callback() - generate an arc + * Generate an arc * * cm_arc_callback() is called from the controller main loop. Each time it's called it * queues as many arc segments (lines) as it can before it blocks, then returns. * * Parts of this routine were originally sourced from the grbl project. */ +stat_t cm_arc_callback() { + if (arc.run_state == MOVE_OFF) return STAT_NOOP; -stat_t cm_arc_callback() -{ - if (arc.run_state == MOVE_OFF) - return STAT_NOOP; - - if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) - return STAT_EAGAIN; + if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) return STAT_EAGAIN; arc.theta += arc.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.arc_segment_linear_travel; - mp_aline(&arc.gm); // run the line + mp_aline(&arc.gm); // run the line copy_vector(arc.position, arc.gm.target); // update arc current position - if (--arc.arc_segment_count > 0) - return STAT_EAGAIN; + if (--arc.arc_segment_count > 0) return STAT_EAGAIN; + arc.run_state = MOVE_OFF; + return STAT_OK; } -/* - * cm_abort_arc() - stop arc movement without maintaining position - * - * OK to call if no arc is running - */ -void cm_abort_arc() -{ +/// Stop arc movement without maintaining position +/// OK to call if no arc is running +void cm_abort_arc() { arc.run_state = MOVE_OFF; } + /* - * _compute_arc() - compute arc from I and J (arc center point) + * 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. @@ -227,15 +198,11 @@ void cm_abort_arc() * * Parts of this routine were originally sourced from the grbl project. */ - -static stat_t _compute_arc() -{ +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 + if (fp_NOT_ZERO(arc.radius)) _compute_arc_offsets_from_radius(); // indicates a radius arc + 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 @@ -247,12 +214,10 @@ static stat_t _compute_arc() 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; + + if ((err > ARC_RADIUS_ERROR_MAX) || + ((err < ARC_RADIUS_ERROR_MIN) && (err > arc.radius * ARC_RADIUS_TOLERANCE))) 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) @@ -261,34 +226,32 @@ static stat_t _compute_arc() // 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 + 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 + if (fp_ZERO(arc.rotations)) arc.rotations = 1.0; // handle the valid case of a full circle arc w/P=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)) { + 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); - } + + 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); - } + + 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) { + 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); - } + 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) @@ -298,13 +261,16 @@ static stat_t _compute_arc() _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_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; + 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 = + 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 @@ -314,11 +280,13 @@ static stat_t _compute_arc() 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; } + /* - * _compute_arc_offsets_from_radius() - compute arc center (offset) from radius. + * Compute arc center (offset) from radius. * * Needs to calculate the center of the circle that has the designated radius and * passes through both the current position and the target position @@ -392,8 +360,7 @@ static stat_t _compute_arc() * Assumes arc singleton has been pre-loaded with target and position. * Parts of this routine were originally sourced from the grbl project. */ -static stat_t _compute_arc_offsets_from_radius() -{ +static stat_t _compute_arc_offsets_from_radius() { // Calculate the change in position along each selected axis float x = cm.gm.target[arc.plane_axis_0] - cm.gmx.position[arc.plane_axis_0]; float y = cm.gm.target[arc.plane_axis_1] - cm.gmx.position[arc.plane_axis_1]; @@ -411,28 +378,28 @@ static stat_t _compute_arc_offsets_from_radius() float disc = 4 * square(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; + 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 (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) { h_x2_div_d = -h_x2_div_d;} + if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) h_x2_div_d = -h_x2_div_d; // Negative R is g-code-alese for "I want a circle with more than 180 degrees // of travel" (go figure!), even though it is advised against ever generating // such circles in a single line of g-code. 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 unadvisably long arcs as prescribed. - if (arc.radius < 0) { h_x2_div_d = -h_x2_div_d; } + if (arc.radius < 0) h_x2_div_d = -h_x2_div_d; // 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.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; + return STAT_OK; } + /* - * _estimate_arc_time () - * * Returns a naiive estimate of arc execution time to inform segment calculation. * The arc time is computed not to exceed the time taken in the slowest dimension * in the arc plane or in linear travel. Maximum feed rates are compared in each @@ -440,21 +407,19 @@ static stat_t _compute_arc_offsets_from_radius() * where the unit vector is 1 in that dimension. This is not true for any arbitrary arc, * with the result that the time returned may be less than optimal. */ -static void _estimate_arc_time () -{ +static void _estimate_arc_time() { // Determine move time at requested feed rate if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) { - arc.arc_time = cm.gm.feed_rate; // inverse feed rate has been normalized to minutes + arc.arc_time = cm.gm.feed_rate; // inverse feed rate has been normalized to minutes cm.gm.feed_rate = 0; // reset feed rate so next block requires an explicit feed rate setting cm.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE; - } else { - arc.arc_time = arc.length / cm.gm.feed_rate; - } + + } else arc.arc_time = arc.length / cm.gm.feed_rate; // Downgrade the time if there is a rate-limiting axis arc.arc_time = max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_0].feedrate_max); arc.arc_time = max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_1].feedrate_max); - if (fabs(arc.linear_travel) > 0) { + + if (fabs(arc.linear_travel) > 0) arc.arc_time = max(arc.arc_time, fabs(arc.linear_travel/cm.a[arc.linear_axis].feedrate_max)); - } } diff --git a/src/plan/exec.c b/src/plan/exec.c index 7f6905f..888eec5 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -48,12 +48,8 @@ static void _init_forward_diffs(float Vi, float Vt); #endif -/************************************************************************* - * Execute runtime functions to prep move for steppers - * - * Dequeues the buffer queue and executes the move continuations. - * Manages run buffers and other details - */ +/// Dequeues the buffer queue and executes the move continuations. +/// Manages run buffers and other details stat_t mp_exec_move() { mpBuf_t *bf; @@ -133,8 +129,8 @@ stat_t mp_exec_move() { * to make sure this actually all works out. move_time is the actual time of the * move, accel_time is the time valaue needed to compute the velocity - which * takes the initial velocity into account (move_time does not need to). - */ -/* --- State transitions - hierarchical state machine --- + * + * --- State transitions - hierarchical state machine --- * * bf->move_state transitions: * from _NEW to _RUN on first call (sub_state set to _OFF) @@ -150,7 +146,6 @@ stat_t mp_exec_move() { * Note: For a direct math implementation see build 357.xx or earlier * Builds 358 onward have only forward difference code */ - stat_t mp_exec_aline(mpBuf_t *bf) { if (bf->move_state == MOVE_OFF) return STAT_NOOP; @@ -169,6 +164,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { bf->nx->replannable = false; // prevent overplanning (Note 2) st_prep_null(); // call this to keep the loader happy if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & end cycle if planner is empty + return STAT_NOOP; } @@ -346,9 +342,9 @@ stat_t mp_exec_aline(mpBuf_t *bf) { #ifndef __JERK_EXEC static void _init_forward_diffs(float Vi, float Vt) { - float A = -6.0*Vi + 6.0*Vt; - float B = 15.0*Vi - 15.0*Vt; - float C = -10.0*Vi + 10.0*Vt; + float A = -6.0 * Vi + 6.0 * Vt; + float B = 15.0 * Vi - 15.0 * Vt; + float C = -10.0 * Vi + 10.0 * Vt; // D = 0 // E = 0 // F = Vi @@ -359,8 +355,8 @@ static void _init_forward_diffs(float Vi, float Vt) { float Bh_4 = B * h * h * h * h; float Ch_3 = C * h * h * h; - mr.forward_diff_5 = (121.0 / 16.0) * Ah_5 + 5.0 * Bh_4 + (13.0 / 4.0) * Ch_3; - mr.forward_diff_4 = (165.0 / 2.0) * Ah_5 + 29.0 * Bh_4 + 9.0 * Ch_3; + mr.forward_diff_5 = 121.0 / 16.0 * Ah_5 + 5.0 * Bh_4 + 13.0 / 4.0 * Ch_3; + mr.forward_diff_4 = 165.0 / 2.0 * Ah_5 + 29.0 * Bh_4 + 9.0 * Ch_3; mr.forward_diff_3 = 255.0 * Ah_5 + 48.0 * Bh_4 + 6.0 * Ch_3; mr.forward_diff_2 = 300.0 * Ah_5 + 24.0 * Bh_4; mr.forward_diff_1 = 120.0 * Ah_5; @@ -382,10 +378,10 @@ static void _init_forward_diffs(float Vi, float Vt) { } #endif -#ifdef __JERK_EXEC +#ifdef __JERK_EXEC static stat_t _exec_aline_head() { - if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) + if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) if (fp_ZERO(mr.head_length)) { mr.section = SECTION_BODY; return _exec_aline_body(); // skip ahead to the body generator @@ -398,11 +394,11 @@ static stat_t _exec_aline_head() { mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.entry_velocity) / mr.jerk); mr.midpoint_acceleration = 2 * (mr.cruise_velocity - mr.entry_velocity) / mr.accel_time; mr.segment_accel_time = mr.accel_time / (2 * mr.segments); // time to advance for each segment - mr.elapsed_accel_time = mr.segment_accel_time / 2; // elapsed time starting point (offset) + mr.elapsed_accel_time = mr.segment_accel_time / 2; // elapsed time starting point (offset) mr.segment_count = (uint32_t)mr.segments; if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position + return STAT_MINIMUM_TIME_MOVE; // exit without advancing position mr.section = SECTION_HEAD; mr.section_state = SECTION_1st_HALF; @@ -427,7 +423,7 @@ static stat_t _exec_aline_head() { if (_exec_aline_segment() == STAT_OK) { // OK means this section is done if ((fp_ZERO(mr.body_length)) && (fp_ZERO(mr.tail_length))) - return STAT_OK; // ends the move + return STAT_OK; // ends the move mr.section = SECTION_BODY; mr.section_state = SECTION_NEW; @@ -558,7 +554,6 @@ static stat_t _exec_aline_body() { #ifdef __JERK_EXEC - static stat_t _exec_aline_tail() { if (mr.section_state == SECTION_NEW) { // INITIALIZATION if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move @@ -601,7 +596,6 @@ static stat_t _exec_aline_tail() { #else // __JERK_EXEC -- run forward differencing math - static stat_t _exec_aline_tail() { if (mr.section_state == SECTION_NEW) { // INITIALIZATION if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move diff --git a/src/plan/line.c b/src/plan/line.c index 1638789..6a92cfc 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -45,21 +45,25 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]); static void _reset_replannable_list(); -/* Runtime-specific setters and getters - * - * mp_zero_segment_velocity() - correct velocity in last segment for reporting purposes - * mp_get_runtime_velocity() - returns current velocity (aggregate) - * mp_get_runtime_machine_position() - returns current axis position in machine coordinates - * mp_set_runtime_work_offset() - set offsets in the MR struct - * mp_get_runtime_work_position() - returns current axis position in work coordinates - * that were in effect at move planning time - */ +/// Correct velocity in last segment for reporting purposes void mp_zero_segment_velocity() {mr.segment_velocity = 0;} + + +/// Returns current velocity (aggregate) float mp_get_runtime_velocity() {return mr.segment_velocity;} + + +/// Returns current axis position in machine coordinates float mp_get_runtime_absolute_position(uint8_t axis) {return mr.position[axis];} + +/// Set offsets in the MR struct void mp_set_runtime_work_offset(float offset[]) {copy_vector(mr.gm.work_offset, offset);} + +/// Returns current axis position in work coordinates +/// that were in effect at move planning time float mp_get_runtime_work_position(uint8_t axis) {return mr.position[axis] - mr.gm.work_offset[axis];} + /* * Return TRUE if motion control busy (i.e. robot is moving) * @@ -86,12 +90,6 @@ uint8_t mp_get_runtime_busy() { * that are too short to move will accumulate and get executed once the accumulated error * exceeds the minimums. */ -/* - #define axis_length bf->body_length - #define axis_velocity bf->cruise_velocity - #define axis_tail bf->tail_length - #define longest_tail bf->head_length -*/ stat_t mp_aline(GCodeState_t *gm_in) { mpBuf_t *bf; // current move pointer float exact_stop = 0; // preset this value OFF @@ -257,13 +255,6 @@ stat_t mp_aline(GCodeState_t *gm_in) { } -/***** ALINE HELPERS ***** - * _calc_move_times() - * _plan_block_list() - * _get_junction_vmax() - * _reset_replannable_list() - */ - /* * Compute optimal and minimum move times into the gcode_state * @@ -321,7 +312,6 @@ stat_t mp_aline(GCodeState_t *gm_in) { * so that the elapsed time from the start to the end of the motion is T plus * any time required for acceleration or deceleration. */ - static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const float axis_square[]) { // gms = Gcode model state float inv_time = 0; // inverse time if doing a feed in G93 mode @@ -359,7 +349,6 @@ static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const if (tmp_time > 0) // collect minimum time if this axis is not zero gms->minimum_time = min(gms->minimum_time, tmp_time); - } gms->move_time = max4(inv_time, max_time, xyz_time, abc_time); @@ -416,8 +405,8 @@ static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const * bf->unit[] - block unit vector * bf->time - gets set later * bf->jerk - source of the other jerk variables. Used in mr. - */ -/* Notes: + * + * Notes: * [1] Whether or not a block is planned is controlled by the bf->replannable * setting (set TRUE if it should be). Replan flags are checked during the * backwards pass and prune the replan list to include only the the latest @@ -528,8 +517,8 @@ static void _reset_replannable_list() { * How to compute the radius using brute-force trig: * float theta = acos(costheta); * float radius = delta * sin(theta/2)/(1-sin(theta/2)); - */ -/* This version extends Chamnit's algorithm by computing a value for delta that takes + * + * This version extends Chamnit's algorithm by computing a value for delta that takes * the contributions of the individual axes in the move into account. This allows the * control radius to vary by axis. This is necessary to support axes that have * different dynamics; such as a Z axis that doesn't move as fast as X and Y (such as a @@ -544,7 +533,6 @@ static void _reset_replannable_list() { * Usum Length of sums Ux + Uy * d Delta of sums (Dx*Ux+DY*UY)/Usum */ - static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { float costheta = -a_unit[AXIS_X] * b_unit[AXIS_X] - @@ -590,8 +578,8 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { * Feedhold is executed as cm.hold_state transitions executed inside * _exec_aline() and main loop callbacks to these functions: * mp_plan_hold_callback() and mp_end_hold(). - */ -/* Holds work like this: + * + * Holds work like this: * * - Hold is asserted by calling cm_feedhold() (usually invoked via a ! char) * If hold_state is OFF and motion_state is RUNning it sets @@ -636,7 +624,6 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { * code in this module, but the code is so complicated I just left it * organized for clarity and hoped for the best from compiler optimization. */ - static float _compute_next_segment_velocity() { if (mr.section == SECTION_BODY) return mr.segment_velocity; #ifdef __JERK_EXEC diff --git a/src/plan/planner.c b/src/plan/planner.c index 0696044..aadc908 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -75,7 +75,7 @@ mpMoveRuntimeSingleton_t mr; // context for line runtime #define flag_vector unit // alias for vector of flags -// execution routines (NB: These are all called from the LO interrupt) +// Execution routines (NB: These are all called from the LO interrupt) static stat_t _exec_dwell(mpBuf_t *bf); static stat_t _exec_command(mpBuf_t *bf); @@ -102,11 +102,8 @@ void mp_flush_planner() { cm_set_motion_state(MOTION_STOP); } + /* - * mp_set_planner_position() - set planner position for a single axis - * mp_set_runtime_position() - set runtime position for a single axis - * mp_set_steps_to_runtime_position() - set encoder counts to the runtime position - * * Since steps are in motor space you have to run the position vector through inverse * kinematics to get the right numbers. This means that in a non-Cartesian robot changing * any position can result in changes to multiple step values. So this operation is provided @@ -117,7 +114,7 @@ void mp_flush_planner() { * * - mm.position - start and end position for planning * - mr.position - current position of runtime segment - * - mr.target - target position of runtime segment + * - mr.target - target position of runtime segment * - mr.endpoint - final target position of runtime segment * * Note that position is set immediately when called and may not be not an accurate representation @@ -125,9 +122,15 @@ void mp_flush_planner() { * still close to the starting point. */ +/// Set planner position for a single axis void mp_set_planner_position(uint8_t axis, const float position) {mm.position[axis] = position;} + + +/// Set runtime position for a single axis void mp_set_runtime_position(uint8_t axis, const float position) {mr.position[axis] = position;} + +/// Set encoder counts to the runtime position void mp_set_steps_to_runtime_position() { float step_position[MOTORS]; @@ -147,26 +150,24 @@ void mp_set_steps_to_runtime_position() { /************************************************************************************ - * mp_queue_command() - queue a synchronous Mcode, program control, or other command - * _exec_command() - callback to execute command - * - * How this works: - * - The command is called by the Gcode interpreter (cm_, e.g. an M code) - * - cm_ function calls mp_queue_command which puts it in the planning queue (bf buffer). - * This involves setting some parameters and registering a callback to the - * execution function in the canonical machine - * - the planning queue gets to the function and calls _exec_command() - * - ...which puts a pointer to the bf buffer in the prep stratuc (st_pre) - * - When the runtime gets to the end of the current activity (sending steps, counting a dwell) - * if executes mp_runtime_command... - * - ...which uses the callback function in the bf and the saved parameters in the vectors - * - To finish up mp_runtime_command() needs to free the bf buffer + * How this works: + * - The command is called by the Gcode interpreter (cm_, e.g. an M code) + * - cm_ function calls mp_queue_command which puts it in the planning queue (bf buffer). + * This involves setting some parameters and registering a callback to the + * execution function in the canonical machine + * - the planning queue gets to the function and calls _exec_command() + * - ...which puts a pointer to the bf buffer in the prep struct (st_pre) + * - When the runtime gets to the end of the current activity (sending steps, counting a dwell) + * if executes mp_runtime_command... + * - ...which uses the callback function in the bf and the saved parameters in the vectors + * - To finish up mp_runtime_command() needs to free the bf buffer * - * Doing it this way instead of synchronizing on queue empty simplifies the - * handling of feedholds, feed overrides, buffer flushes, and thread blocking, - * and makes keeping the queue full much easier - therefore avoiding Q starvation + * Doing it this way instead of synchronizing on queue empty simplifies the + * handling of feedholds, feed overrides, buffer flushes, and thread blocking, + * and makes keeping the queue full much easier - therefore avoiding starvation */ +/// Queue a synchronous Mcode, program control, or other command void mp_queue_command(void(*cm_exec)(float[], float[]), float *value, float *flag) { mpBuf_t *bf; @@ -189,6 +190,7 @@ void mp_queue_command(void(*cm_exec)(float[], float[]), float *value, float *fla } +/// callback to execute command static stat_t _exec_command(mpBuf_t *bf) { st_prep_command(bf); return STAT_OK; @@ -205,14 +207,12 @@ stat_t mp_runtime_command(mpBuf_t *bf) { } -/************************************************************************* - * mp_dwell() - queue a dwell - * _exec_dwell() - dwell execution - * - * Dwells are performed by passing a dwell move to the stepper drivers. +/* Dwells are performed by passing a dwell move to the stepper drivers. * When the stepper driver sees a dwell it times the dwell on a separate * timer than the stepper pulse timer. */ + +/// Queue a dwell stat_t mp_dwell(float seconds) { mpBuf_t *bf; @@ -228,6 +228,7 @@ stat_t mp_dwell(float seconds) { } +/// Dwell execution static stat_t _exec_dwell(mpBuf_t *bf) { st_prep_dwell((uint32_t)(bf->gm.move_time * 1000000)); // convert seconds to uSec if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & perform cycle_end if planner is empty @@ -235,6 +236,7 @@ static stat_t _exec_dwell(mpBuf_t *bf) { return STAT_OK; } + /**** PLANNER BUFFERS ***************************************************** * * Planner buffers are used to queue and operate on Gcode blocks. Each buffer @@ -255,43 +257,14 @@ static stat_t _exec_dwell(mpBuf_t *bf) { * The write buffer pointer only moves forward on _queue_write_buffer, and * the read buffer pointer only moves forward on free_read calls. * (test, get and unget have no effect) - * - * mp_get_planner_buffers_available() Returns # of available planner buffers - * - * mp_init_buffers() Initializes or resets buffers - * - * mp_get_write_buffer() Get pointer to next available write buffer - * Returns pointer or 0 if no buffer available. - * - * mp_unget_write_buffer() Free write buffer if you decide not to commit it. - * - * mp_commit_write_buffer() Commit the next write buffer to the queue - * Advances write pointer & changes buffer state - * WARNING: The calling routine must not use the write buffer - * once it has been queued as it may be processed and freed (wiped) - * before mp_queue_write_buffer() returns. - * - * mp_get_run_buffer() Get pointer to the next or current run buffer - * Returns a new run buffer if prev buf was ENDed - * Returns same buf if called again before ENDing - * Returns 0 if no buffer available - * The behavior supports continuations (iteration) - * - * mp_free_run_buffer() Release the run buffer & return to buffer pool. - * Returns true if queue is empty, false otherwise. - * This is useful for doing queue empty / end move functions. - * - * mp_get_prev_buffer(bf) Returns pointer to prev buffer in linked list - * mp_get_next_buffer(bf) Returns pointer to next buffer in linked list - * mp_get_first_buffer(bf) Returns pointer to first buffer, i.e. the running block - * mp_get_last_buffer(bf) Returns pointer to last buffer, i.e. last block (zero) - * mp_clear_buffer(bf) Zeroes the contents of the buffer - * mp_copy_buffer(bf,bp) Copies the contents of bp into bf - preserves links */ + +/// Returns # of available planner buffers uint8_t mp_get_planner_buffers_available() {return mb.buffers_available;} +/// Initializes or resets buffers void mp_init_buffers() { mpBuf_t *pv; @@ -312,7 +285,10 @@ void mp_init_buffers() { } -mpBuf_t *mp_get_write_buffer() { // get & clear a buffer +/// Get pointer to next available write buffer +/// Returns pointer or 0 if no buffer available. +mpBuf_t *mp_get_write_buffer() { + // get & clear a buffer if (mb.w->buffer_state == MP_BUFFER_EMPTY) { mpBuf_t *w = mb.w; mpBuf_t *nx = mb.w->nx; // save linked list pointers @@ -331,6 +307,7 @@ mpBuf_t *mp_get_write_buffer() { // get & clear a buffer } +/// Free write buffer if you decide not to commit it. void mp_unget_write_buffer() { mb.w = mb.w->pv; // queued --> write mb.w->buffer_state = MP_BUFFER_EMPTY; // not loading anymore @@ -338,9 +315,17 @@ void mp_unget_write_buffer() { } -/*** WARNING: The routine calling mp_commit_write_buffer() must not use the write buffer - once it has been queued. Action may start on the buffer immediately, - invalidating its contents ***/ +/* Commit the next write buffer to the queue + * Advances write pointer & changes buffer state + * + * WARNING: The calling routine must not use the write buffer + * once it has been queued as it may be processed and freed (wiped) + * before mp_queue_write_buffer() returns. + * + * WARNING: The routine calling mp_commit_write_buffer() must not use the write buffer + * once it has been queued. Action may start on the buffer immediately, + * invalidating its contents + */ void mp_commit_write_buffer(const uint8_t move_type) { mb.q->move_type = move_type; mb.q->move_state = MOVE_NEW; @@ -352,6 +337,12 @@ void mp_commit_write_buffer(const uint8_t move_type) { } +/* Get pointer to the next or current run buffer + * Returns a new run buffer if prev buf was ENDed + * Returns same buf if called again before ENDing + * Returns 0 if no buffer available + * The behavior supports continuations (iteration) + */ mpBuf_t *mp_get_run_buffer() { // CASE: fresh buffer; becomes running if queued or pending if ((mb.r->buffer_state == MP_BUFFER_QUEUED) || @@ -365,6 +356,10 @@ mpBuf_t *mp_get_run_buffer() { } +/* Release the run buffer & return to buffer pool. + * Returns true if queue is empty, false otherwise. + * This is useful for doing queue empty / end move functions. + */ uint8_t mp_free_run_buffer() { // EMPTY current run buf & adv to next mp_clear_buffer(mb.r); // clear it out (& reset replannable) mb.r = mb.r->nx; // advance to next run buffer @@ -378,11 +373,13 @@ uint8_t mp_free_run_buffer() { // EMPTY current run buf & adv to } +/// Returns pointer to first buffer, i.e. the running block mpBuf_t *mp_get_first_buffer() { return mp_get_run_buffer(); // returns buffer or 0 if nothing's running } +/// Returns pointer to last buffer, i.e. last block (zero) mpBuf_t *mp_get_last_buffer() { mpBuf_t *bf = mp_get_run_buffer(); mpBuf_t *bp; @@ -394,6 +391,7 @@ mpBuf_t *mp_get_last_buffer() { } +/// Zeroes the contents of the buffer void mp_clear_buffer(mpBuf_t *bf) { mpBuf_t *nx = bf->nx; // save pointers mpBuf_t *pv = bf->pv; @@ -403,6 +401,7 @@ void mp_clear_buffer(mpBuf_t *bf) { } +/// Copies the contents of bp into bf - preserves links void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp) { mpBuf_t *nx = bf->nx; // save pointers mpBuf_t *pv = bf->pv; diff --git a/src/plan/planner.h b/src/plan/planner.h index 8e37872..a40a862 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -97,21 +97,27 @@ enum sectionState { * Suggest 12 min. Limit is 255 */ #define PLANNER_BUFFER_POOL_SIZE 32 -#define PLANNER_BUFFER_HEADROOM 4 // buffers to reserve in planner before processing new input line +/// Buffers to reserve in planner before processing new input line +#define PLANNER_BUFFER_HEADROOM 4 -/* Some parameters for _generate_trapezoid() - * TRAPEZOID_ITERATION_MAX Max iterations for convergence in the HT asymmetric case. - * TRAPEZOID_ITERATION_ERROR_PERCENT Error percentage for iteration convergence. As percent - 0.01 = 1% - * TRAPEZOID_LENGTH_FIT_TOLERANCE Tolerance for "exact fit" for H and T cases - * TRAPEZOID_VELOCITY_TOLERANCE Adaptive velocity tolerance term - */ +// Parameters for _generate_trapezoid() + +/// Max iterations for convergence in the HT asymmetric case. #define TRAPEZOID_ITERATION_MAX 10 + +/// Error percentage for iteration convergence. As percent - 0.01 = 1% #define TRAPEZOID_ITERATION_ERROR_PERCENT ((float)0.10) -#define TRAPEZOID_LENGTH_FIT_TOLERANCE ((float)0.0001) // allowable mm of error in planning phase + +/// Tolerance for "exact fit" for H and T cases +/// allowable mm of error in planning phase +#define TRAPEZOID_LENGTH_FIT_TOLERANCE ((float)0.0001) + +/// Adaptive velocity tolerance term #define TRAPEZOID_VELOCITY_TOLERANCE (max(2, bf->entry_velocity / 100)) -typedef void (*cm_exec_t)(float[], float[]); // callback to canonical_machine execution function +/// callback to canonical_machine execution function +typedef void (*cm_exec_t)(float[], float[]); // All the enums that equal zero must be zero. Don't change this @@ -278,7 +284,10 @@ uint8_t mp_free_run_buffer(); mpBuf_t *mp_get_first_buffer(); mpBuf_t *mp_get_last_buffer(); -#define mp_get_prev_buffer(b) ((mpBuf_t *)(b->pv)) // use the macro instead +/// Returns pointer to prev buffer in linked list +#define mp_get_prev_buffer(b) ((mpBuf_t *)(b->pv)) + +/// Returns pointer to next buffer in linked list #define mp_get_next_buffer(b) ((mpBuf_t *)(b->nx)) void mp_clear_buffer(mpBuf_t *bf); diff --git a/src/plan/zoid.c b/src/plan/zoid.c index 8b2dbfd..62c1d98 100644 --- a/src/plan/zoid.c +++ b/src/plan/zoid.c @@ -62,8 +62,8 @@ * Note: The following conditions must be met on entry: * bf->length must be non-zero (filter these out upstream) * bf->entry_velocity <= bf->cruise_velocity >= bf->exit_velocity - */ -/* Classes of moves: + * + * Classes of moves: * * Requested-Fit - The move has sufficient length to achieve the target velocity * (cruise velocity). I.e: it will accommodate the acceleration / deceleration @@ -107,8 +107,8 @@ * T" Ve>Vx Ve is degraded (velocity step). Vx is met * B" line is very short but drawable; is treated as a body only * F force fit: This block is slowed down until it can be executed - */ -/* NOTE: The order of the cases/tests in the code is pretty important. Start with the + * + * NOTE: The order of the cases/tests in the code is pretty important. Start with the * shortest cases first and work up. Not only does this simplify the order of the tests, * but it reduces execution time when you need it most - when tons of pathologically * short Gcode blocks are being thrown at you. @@ -121,184 +121,187 @@ #define MIN_TAIL_LENGTH (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->exit_velocity)) #define MIN_BODY_LENGTH (MIN_SEGMENT_TIME_PLUS_MARGIN * bf->cruise_velocity) -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; +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; } - // 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; + 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;} - // 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 + // Rate-limited HT and HT' cases + if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited - if (((bf->cruise_velocity - bf->entry_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) && - ((bf->cruise_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE)) { + // 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; - 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 + // 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; + } - 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; - } + return; } - // Set head and tail lengths for evaluating the next cases + // Asymmetric HT' rate-limited case. This is relatively expensive but it's not called very often + 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); + } + + } 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 = 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; + 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; } - // 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; + 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 bf->head_length += bf->body_length; // HB reduces to H + } else bf->tail_length += bf->body_length; // BT reduces to T + + 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; - } + } else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length))) + bf->cruise_velocity = bf->entry_velocity; } + /* - * mp_get_target_length() - derive accel/decel length from delta V and jerk - * mp_get_target_velocity() - derive velocity achievable from delta V and length - * * This set of functions returns the fourth thing knowing the other three. * * Jm = the given maximum jerk @@ -338,12 +341,13 @@ void mp_calculate_trapezoid(mpBuf_t *bf) * return cube(deltaV / (pow(L, 0.66666666))); */ -float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf) -{ -// return Vi + Vf * sqrt(fabs(Vf - Vi) * bf->recip_jerk); // new formula - return fabs(Vi-Vf * sqrt(fabs(Vi-Vf) * bf->recip_jerk)); // old formula + +/// Derive accel/decel length from delta V and jerk +float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf) { + return fabs(Vi - Vf * sqrt(fabs(Vi - Vf) * bf->recip_jerk)); } + /* Regarding mp_get_target_velocity: * * We do some Newton-Raphson iterations to narrow it down. @@ -388,25 +392,28 @@ float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf) * J'(x) = (2*Vi*x - Vi² + 3*x²) / L² */ -#define GET_VELOCITY_ITERATIONS 0 // must be 0, 1, or 2 -float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf) -{ - // 0 iterations (a reasonable estimate) - float estimate = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; +#define GET_VELOCITY_ITERATIONS 0 // must be 0, 1, or 2 + +/// derive velocity achievable from delta V and length +float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf) { + // 0 iterations (a reasonable estimate) + float estimate = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; #if (GET_VELOCITY_ITERATIONS >= 1) - // 1st iteration - float L_squared = L*L; - float Vi_squared = Vi*Vi; - float J_z = ((estimate - Vi) * (Vi + estimate) * (Vi + estimate)) / L_squared - bf->jerk; - float J_d = (2*Vi*estimate - Vi_squared + 3*(estimate*estimate)) / L_squared; - estimate = estimate - J_z/J_d; + // 1st iteration + float L_squared = L * L; + float Vi_squared = Vi * Vi; + float J_z = (estimate - Vi) * (Vi + estimate) * (Vi + estimate) / L_squared - bf->jerk; + float J_d = (2 * Vi * estimate - Vi_squared + 3 * estimate * estimate) / L_squared; + estimate = estimate - J_z / J_d; #endif + #if (GET_VELOCITY_ITERATIONS >= 2) - // 2nd iteration - J_z = ((estimate - Vi) * (Vi + estimate) * (Vi + estimate)) / L_squared - bf->jerk; - J_d = (2*Vi*estimate - Vi_squared + 3*(estimate*estimate)) / L_squared; - estimate = estimate - J_z/J_d; + // 2nd iteration + J_z = (estimate - Vi) * (Vi + estimate) * (Vi + estimate) / L_squared - bf->jerk; + J_d = (2 * Vi * estimate - Vi_squared + 3 * estimate * estimate) / L_squared; + estimate = estimate - J_z / J_d; #endif - return estimate; + + return estimate; } diff --git a/src/stepper.c b/src/stepper.c index 0364e40..32b22de 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -385,7 +385,7 @@ ISR(TIMER_EXEC_ISR_vect) { // exec move SW interru /**************************************************************************************** * Loader sequencing code * st_request_load_move() - fires a software interrupt (timer) to request to load a move - * load_move interrupt - interrupt handler for running the loader + * load_move interrupt - interrupt handler for running the loader * * _load_move() can only be called be called from an ISR at the same or higher level as * the DDA or dwell ISR. A software interrupt has been provided to allow a non-ISR to -- 2.27.0