From: Joseph Coffland Date: Sat, 10 Sep 2016 03:02:12 +0000 (-0700) Subject: Cleaned up line planning code X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=7201c1bc9b3c88d94a5e29db20466f2ba7864f58;p=bbctrl-firmware Cleaned up line planning code --- diff --git a/src/machine.c b/src/machine.c index 1c65d2b..aa6e59a 100644 --- a/src/machine.c +++ b/src/machine.c @@ -30,40 +30,35 @@ /* This code is a loose implementation of Kramer, Proctor and Messina's * machining functions as described in the NIST RS274/NGC v3 * - * The machine is the layer between the Gcode parser and - * the motion control code for a specific robot. It keeps state and - * executes commands - passing the stateless commands to the motion - * planning layer. + * The machine is the layer between the Gcode parser and the motion control code + * for a specific robot. It keeps state and executes commands - passing the + * stateless commands to the motion planning layer. * * Synchronizing command execution: * - * "Synchronous commands" are commands that affect the runtime need - * to be synchronized with movement. Examples include G4 dwells, - * program stops and ends, and most M commands. These are queued - * into the planner queue and execute from the queue. Synchronous - * commands work like this: + * "Synchronous commands" are commands that affect the runtime need to be + * synchronized with movement. Examples include G4 dwells, program stops and + * ends, and most M commands. These are queued into the planner queue and + * execute from the queue. Synchronous commands work like this: * - * - Call the mach_xxx_xxx() function which will do any input - * validation and return an error if it detects one. + * - Call the mach_xxx_xxx() function which will do any input validation and + * return an error if it detects one. * - * - The mach_ function calls mp_queue_command(). Arguments are a - * callback to the _exec_...() function, which is the runtime - * execution routine, and any arguments that are needed by the - * runtime. See typedef for *exec in planner.h for details + * - The mach_ function calls mp_queue_command(). Arguments are a callback to + * the _exec_...() function, which is the runtime execution routine, and any + * arguments that are needed by the runtime. See typedef for *exec in + * planner.h for details * - * - mp_queue_command() stores the callback and the args in a - * planner buffer. + * - mp_queue_command() stores the callback and the args in a planner buffer. * - * - When planner execution reaches the buffer it executes the - * callback w/ the args. Take careful note that the callback - * executes under an interrupt, so beware of variables that may - * need to be volatile. + * - When planner execution reaches the buffer it executes the callback w/ the + * args. Take careful note that the callback executes under an interrupt, + * so beware of variables that may need to be volatile. * - * Note: The synchronous command execution mechanism uses 2 - * vectors in the bf buffer to store and return values for the - * callback. It's obvious, but impractical to pass the entire bf - * buffer to the callback as some of these commands are actually - * executed locally and have no buffer. + * Note: The synchronous command execution mechanism uses 2 vectors in the bf + * buffer to store and return values for the callback. It's obvious, but + * impractical to pass the entire bf buffer to the callback as some of these + * commands are actually executed locally and have no buffer. */ #include "machine.h" @@ -485,110 +480,97 @@ void mach_finalize_move() { /* Compute optimal and minimum move times into the gcode_state * - * "Minimum time" is the fastest the move can be performed given - * the velocity constraints on each participating axis - regardless - * of the feed rate requested. The minimum time is the time limited - * by the rate-limiting axis. The minimum time is needed to compute - * the optimal time and is recorded for possible feed override - * computation. + * "Minimum time" is the fastest the move can be performed given the velocity + * constraints on each participating axis - regardless of the feed rate + * requested. The minimum time is the time limited by the rate-limiting + * axis. The minimum time is needed to compute the optimal time and is recorded + * for possible feed override computation. * - * "Optimal time" is either the time resulting from the requested - * feed rate or the minimum time if the requested feed rate is not - * achievable. Optimal times for rapids are always the minimum - * time. + * "Optimal time" is either the time resulting from the requested feed rate or + * the minimum time if the requested feed rate is not achievable. Optimal times + * for rapids are always the minimum time. * - * The gcode state must have targets set prior by having - * mach_set_target(). Axis modes are taken into account by this. + * The gcode state must have targets set prior by having mach_set_target(). Axis + * modes are taken into account by this. * * The following times are compared and the longest is returned: * - G93 inverse time (if G93 is active) * - time for coordinated move at requested feed rate * - time that the slowest axis would require for the move * - * Sets the following variables in the gcode_state struct - * - move_time is set to optimal time + * Sets the following variables in the gcode_state struct - move_time is set to + * optimal time * * NIST RS274NGC_v3 Guidance * - * The following is verbatim text from NIST RS274NGC_v3. As I - * interpret A for moves that combine both linear and rotational - * movement, the feed rate should apply to the XYZ movement, with - * the rotational axis (or axes) timed to start and end at the same - * time the linear move is performed. It is possible under this - * case for the rotational move to rate-limit the linear move. + * The following is verbatim text from NIST RS274NGC_v3. As I interpret A for + * moves that combine both linear and rotational movement, the feed rate should + * apply to the XYZ movement, with the rotational axis (or axes) timed to start + * and end at the same time the linear move is performed. It is possible under + * this case for the rotational move to rate-limit the linear move. * * 2.1.2.5 Feed Rate * - * The rate at which the controlled point or the axes move is - * nominally a steady rate which may be set by the user. In the - * Interpreter, the interpretation of the feed rate is as follows - * unless inverse time feed rate mode is being used in the - * RS274/NGC view (see Section 3.5.19). The machining - * functions view of feed rate, as described in Section 4.3.5.1, - * has conditions under which the set feed rate is applied - * differently, but none of these is used in the Interpreter. + * The rate at which the controlled point or the axes move is nominally a steady + * rate which may be set by the user. In the Interpreter, the interpretation of + * the feed rate is as follows unless inverse time feed rate mode is being used + * in the RS274/NGC view (see Section 3.5.19). The machining functions view of + * feed rate, as described in Section 4.3.5.1, has conditions under which the + * set feed rate is applied differently, but none of these is used in the + * Interpreter. * - * A. For motion involving one or more of the X, Y, and Z axes - * (with or without simultaneous rotational axis motion), the - * feed rate means length units per minute along the programmed - * XYZ path, as if the rotational axes were not moving. + * A. For motion involving one or more of the X, Y, and Z axes (with or without + * simultaneous rotational axis motion), the feed rate means length units + * per minute along the programmed XYZ path, as if the rotational axes were + * not moving. * - * B. For motion of one rotational axis with X, Y, and Z axes not - * moving, the feed rate means degrees per minute rotation of - * the rotational axis. + * B. For motion of one rotational axis with X, Y, and Z axes not moving, the + * feed rate means degrees per minute rotation of the rotational axis. * - * C. For motion of two or three rotational axes with X, Y, and Z - * axes not moving, the rate is applied as follows. Let dA, dB, - * and dC be the angles in degrees through which the A, B, and - * C axes, respectively, must move. Let D = sqrt(dA^2 + dB^2 + - * dC^2). Conceptually, D is a measure of total angular motion, - * using the usual Euclidean metric. Let T be the amount of - * time required to move through D degrees at the current feed - * rate in degrees per minute. The rotational axes should be - * moved in coordinated linear motion so that the elapsed time - * from the start to the end of the motion is T plus any time - * required for acceleration or deceleration. + * C. For motion of two or three rotational axes with X, Y, and Z axes not + * moving, the rate is applied as follows. Let dA, dB, and dC be the angles + * in degrees through which the A, B, and C axes, respectively, must move. + * Let D = sqrt(dA^2 + dB^2 + dC^2). Conceptually, D is a measure of total + * angular motion, using the usual Euclidean metric. Let T be the amount of + * time required to move through D degrees at the current feed rate in + * degrees per minute. The rotational axes should be moved in coordinated + * linear motion so that the elapsed time from the start to the end of the + * motion is T plus any time required for acceleration or deceleration. */ float mach_calc_move_time(const float axis_length[], const float axis_square[]) { - float inv_time = 0; // inverse time if doing a feed in G93 mode - float xyz_time = 0; // linear coordinated move at requested feed - float abc_time = 0; // rotary coordinated move at requested feed - float max_time = 0; // time required for the rate-limiting axis - float tmp_time = 0; // used in computation + float max_time = 0; - // compute times for feed motion + // Compute times for feed motion if (mach.gm.motion_mode != MOTION_MODE_RAPID) { if (mach.gm.feed_rate_mode == INVERSE_TIME_MODE) { - // feed rate was un-inverted to minutes by mach_set_feed_rate() - inv_time = mach.gm.feed_rate; + // Feed rate was un-inverted to minutes by mach_set_feed_rate() + max_time = mach.gm.feed_rate; mach.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE; } else { - // compute length of linear move in millimeters. Feed rate is provided as - // mm/min - xyz_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + + // Compute length of linear move in millimeters. Feed rate in mm/min. + max_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + axis_square[AXIS_Z]) / mach.gm.feed_rate; - // if no linear axes, compute length of multi-axis rotary move in degrees. + // If no linear axes, compute length of multi-axis rotary move in degrees. // Feed rate is provided as degrees/min - if (fp_ZERO(xyz_time)) - abc_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + + if (fp_ZERO(max_time)) + max_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + axis_square[AXIS_C]) / mach.gm.feed_rate; } } + // Compute time required for rate-limiting axis for (int axis = 0; axis < AXES; axis++) { - if (mach.gm.motion_mode == MOTION_MODE_RAPID) - tmp_time = fabs(axis_length[axis]) / mach.a[axis].velocity_max; + float time = fabs(axis_length[axis]) / + (mach.gm.motion_mode == MOTION_MODE_RAPID ? mach.a[axis].velocity_max : + mach.a[axis].feedrate_max); - else // MOTION_MODE_FEED - tmp_time = fabs(axis_length[axis]) / mach.a[axis].feedrate_max; - - max_time = max(max_time, tmp_time); + if (max_time < time) max_time = time; } - return max4(inv_time, max_time, xyz_time, abc_time); + return max_time < MIN_SEGMENT_TIME ? MIN_SEGMENT_TIME : max_time; } diff --git a/src/plan/arc.c b/src/plan/arc.c index 69ae55a..105788b 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -26,10 +26,9 @@ \******************************************************************************/ -/* This module actually contains some parts that belong ion the - * machine, and other parts that belong at the motion planner - * level, but the whole thing is treated as if it were part of the - * motion planner. +/* This module actually contains some parts that belong in the machine, and + * other parts that belong at the motion planner level, but the whole thing is + * treated as if it were part of the motion planner. */ #include "arc.h" @@ -44,8 +43,6 @@ #include -// See planner.h for MM_PER_ARC_SEGMENT and other arc setting #defines - typedef struct { run_state_t run_state; // runtime state machine sequence int32_t line; // gcode block line number @@ -59,7 +56,7 @@ typedef struct { uint8_t plane_axis_1; // arc plane axis 1 - e.g. Y for G17 uint8_t linear_axis; // linear axis (normal to plane) - int32_t segments; // count of running segments + uint32_t segments; // count of running segments float segment_theta; // angular motion per segment float segment_linear_travel; // linear motion per segment float center_0; // center at axis 0 (e.g. X for G17) @@ -69,7 +66,7 @@ typedef struct { arc_t arc = {0}; -/* Returns a naive estimate of arc execution time to inform segment +/*** Returns a naive 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 dimension, but the @@ -104,7 +101,7 @@ static float _estimate_arc_time(float length, float linear_travel, } -/* 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 @@ -201,8 +198,7 @@ static stat_t _compute_arc_offsets_from_radius(float offset[]) { 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) + // Invert sign of h_x2_div_d if circle is counter clockwise (see header notes) if (mach.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 @@ -240,9 +236,8 @@ static stat_t _compute_arc_offsets_from_radius(float offset[]) { */ static stat_t _compute_arc(const float position[], float offset[], float rotations, bool full_circle) { - // Compute radius. A non-zero radius value indicates a radius arc - if (fp_NOT_ZERO(arc.radius)) - _compute_arc_offsets_from_radius(offset); // indicates a radius arc + // Compute radius. A non-zero radius value indicates a radius arc + if (fp_NOT_ZERO(arc.radius)) _compute_arc_offsets_from_radius(offset); else // compute start radius arc.radius = hypotf(-offset[arc.plane_axis_0], -offset[arc.plane_axis_1]); @@ -256,8 +251,10 @@ static stat_t _compute_arc(const float position[], float offset[], // Compute end radius from the center of circle (offsets) to target endpoint float end_0 = arc.target[arc.plane_axis_0] - position[arc.plane_axis_0] - offset[arc.plane_axis_0]; + float end_1 = arc.target[arc.plane_axis_1] - position[arc.plane_axis_1] - offset[arc.plane_axis_1]; + // end radius - start radius float err = fabs(hypotf(end_0, end_1) - arc.radius); @@ -273,13 +270,13 @@ static stat_t _compute_arc(const float position[], float offset[], // g18_correction is used to invert G18 XZ plane arcs for proper CW // orientation float g18_correction = mach.gm.plane == PLANE_XZ ? -1 : 1; - float angular_travel = 0; if (full_circle) { // angular travel always starts as zero for full circles angular_travel = 0; - // handle the valid case of a full circle arc w/P=0 + + // handle the valid case of a full circle arc w/ P=0 if (fp_ZERO(rotations)) rotations = 1.0; } else { @@ -311,8 +308,7 @@ static stat_t _compute_arc(const float position[], float offset[], // Calculate travel in the depth axis of the helix and compute the time it // should take to perform the move length is the total mm of travel of // the helix (or just a planar arc) - float linear_travel = - arc.target[arc.linear_axis] - position[arc.linear_axis]; + float linear_travel = arc.target[arc.linear_axis] - position[arc.linear_axis]; float planar_travel = angular_travel * arc.radius; // hypot is insensitive to +/- signs float length = hypotf(planar_travel, linear_travel); @@ -325,23 +321,22 @@ static stat_t _compute_arc(const float position[], float offset[], // Find the minimum number of segments that meets these constraints... float segments_for_chordal_accuracy = - length / sqrt(4 * CHORDAL_TOLERANCE * - (2 * arc.radius - CHORDAL_TOLERANCE)); + length / sqrt(4 * CHORDAL_TOLERANCE * (2 * arc.radius - CHORDAL_TOLERANCE)); float segments_for_minimum_distance = length / ARC_SEGMENT_LENGTH; float segments_for_minimum_time = arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC; - float segments = - floor(min3(segments_for_chordal_accuracy, - segments_for_minimum_distance, - segments_for_minimum_time)); - + float segments = floor(min3(segments_for_chordal_accuracy, + segments_for_minimum_distance, + segments_for_minimum_time)); segments = max(segments, 1); // at least 1 segment - arc.segments = (int32_t)segments; + + arc.segments = (uint32_t)segments; arc.segment_theta = angular_travel / segments; arc.segment_linear_travel = linear_travel / segments; arc.center_0 = position[arc.plane_axis_0] - sin(arc.theta) * arc.radius; arc.center_1 = position[arc.plane_axis_1] - cos(arc.theta) * arc.radius; + // initialize the linear target arc.target[arc.linear_axis] = position[arc.linear_axis]; @@ -469,7 +464,7 @@ void mach_arc_callback() { arc.target[arc.plane_axis_1] = arc.center_1 + cos(arc.theta) * arc.radius; arc.target[arc.linear_axis] += arc.segment_linear_travel; - mp_aline(arc.target, arc.line); // run the line + mp_aline(arc.target, arc.line); // run the line if (!--arc.segments) arc.run_state = MOVE_OFF; } diff --git a/src/plan/line.c b/src/plan/line.c index a4b2c95..9f6b802 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -36,57 +36,36 @@ #include "machine.h" #include "stepper.h" #include "util.h" -#include "report.h" #include #include #include -/// common variables for planning (move master) -typedef struct { - float position[AXES]; // final move position for planning purposes - - float jerk; // jerk values cached from previous block - float recip_jerk; - float cbrt_jerk; -} move_master_t; - - -move_master_t mm = {{0}}; // context for line planning - - -/// Set planner position for a single axis -void mp_set_axis_position(int axis, float position) { - mm.position[axis] = position; -} - /* Sonny's algorithm - simple * - * Computes the maximum allowable junction speed by finding the - * velocity that will yield the centripetal acceleration in the - * corner_acceleration value. The value of delta sets the effective - * radius of curvature. Here's Sonny's (Sungeun K. Jeon's) - * explanation of what's going on: + * Computes the maximum allowable junction speed by finding the velocity that + * will yield the centripetal acceleration in the corner_acceleration value. The + * value of delta sets the effective radius of curvature. Here's Sonny's + * (Sungeun K. Jeon's) explanation of what's going on: * * "First let's assume that at a junction we only look a centripetal - * acceleration to simply things. At a junction of two lines, let's - * place a circle such that both lines are tangent to the circle. The - * circular segment joining the lines represents the path for - * constant centripetal acceleration. This creates a deviation from - * the path (let's call this delta), which is the distance from the - * junction to the edge of the circular segment. Delta needs to be - * defined, so let's replace the term max_jerk (see note 1) with - * max_junction_deviation, or "delta". This indirectly sets the - * radius of the circle, and hence limits the velocity by the - * centripetal acceleration. Think of the this as widening the race - * track. If a race car is driving on a track only as wide as a car, - * it'll have to slow down a lot to turn corners. If we widen the - * track a bit, the car can start to use the track to go into the - * turn. The wider it is, the faster through the corner it can go. + * acceleration to simply things. At a junction of two lines, let's place a + * circle such that both lines are tangent to the circle. The circular segment + * joining the lines represents the path for constant centripetal + * acceleration. This creates a deviation from the path (let's call this delta), + * which is the distance from the junction to the edge of the circular + * segment. Delta needs to be defined, so let's replace the term max_jerk (see + * note 1) with max_junction_deviation, or "delta". This indirectly sets the + * radius of the circle, and hence limits the velocity by the centripetal + * acceleration. Think of the this as widening the race track. If a race car is + * driving on a track only as wide as a car, it'll have to slow down a lot to + * turn corners. If we widen the track a bit, the car can start to use the track + * to go into the turn. The wider it is, the faster through the corner it can + * go. * - * (Note 1: "max_jerk" refers to the old grbl/marlin max_jerk" - * approximation term, not the TinyG jerk terms) + * (Note 1: "max_jerk" refers to the old grbl/marlin max_jerk" approximation + * term, not the TinyG jerk terms) * * If you do the geometry in terms of the known variables, you get: * @@ -106,29 +85,28 @@ void mp_set_axis_position(int axis, float position) { * * sin(theta/2) = +/-sqrt((1 - cos(theta)) / 2) * - * For our applications, this should always be positive. Now just - * plug the equations into the centripetal acceleration equation: - * v_c = sqrt(a_max*R). You'll see that there are only two sqrt - * computations and no sine/cosines." + * For our applications, this should always be positive. Now just plug the + * equations into the centripetal acceleration equation: v_c = + * sqrt(a_max*R). You'll see that there are only two sqrt computations and no + * sine/cosines." * * 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 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 screw driven Z axis on machine with a belt driven XY - - * like a Shapeoko), or rotary axes ABC that have completely - * different dynamics than their linear counterparts. + * 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 screw driven Z axis on machine with a belt driven XY - like + * a Shapeoko), or rotary axes ABC that have completely different dynamics than + * their linear counterparts. * - * The function takes the absolute values of the sum of the unit - * vector components as a measure of contribution to the move, then - * scales the delta values from the non-zero axes into a composite - * delta to be used for the move. Shown for an XY vector: + * The function takes the absolute values of the sum of the unit vector + * components as a measure of contribution to the move, then scales the delta + * values from the non-zero axes into a composite delta to be used for the + * move. Shown for an XY vector: * * U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i]) * Usum Length of sums Ux + Uy @@ -139,8 +117,8 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { for (int axis = 0; axis < AXES; axis++) costheta -= a_unit[axis] * b_unit[axis]; - if (costheta < -0.99) return 10000000; // straight line cases - if (costheta > 0.99) return 0; // reversal cases + if (costheta < -0.99) return 10000000; // straight line cases + if (0.99 < costheta) return 0; // reversal cases // Fuse the junction deviations into a vector sum float a_delta = 0; @@ -160,221 +138,176 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { } -/*** Plan a line with acceleration / deceleration +static float _calc_jerk(const float axis_square[], const float unit[]) { + /* Determine jerk value to use for the block. First find the axis for which + * the jerk cannot be exceeded - the 'jerk_axis'. This is the axis for which + * the time to decelerate from the target velocity to zero would be the + * longest. + * + * We can determine the "longest" deceleration in terms of time or distance. + * + * The math for time-to-decelerate T from speed S to speed E with constant + * jerk J is: + * + * T = 2 * sqrt((S - E) / J) + * + * Since E is always zero in this calculation, we can simplify: + * + * T = 2 * sqrt(S / J) + * + * The math for distance-to-decelerate l from speed S to speed E with + * constant jerk J is: + * + * l = (S + E) * sqrt((S - E) / J) + * + * Since E is always zero in this calculation, we can simplify: + * + * l = S * sqrt(S / J) + * + * The final value we want to know is which one is *longest*, compared to the + * others, so we don't need the actual value. This means that the scale of + * the value doesn't matter, so for T we can remove the "2 *" and for L we can + * remove the "S *". This leaves both to be simply "sqrt(S / J)". Since we + * don't need the scale, it doesn't matter what speed we're coming from, so S + * can be replaced with 1. + * + * However, we *do* need to compensate for the fact that each axis contributes + * differently to the move, so we will scale each contribution C[n] by the + * proportion of the axis movement length D[n]. Using that, we construct the + * following, with these definitions: + * + * J[n] = max_jerk for axis n + * D[n] = distance traveled for this move for axis n + * C[n] = "axis contribution" of axis n + * + * For each axis n: + * + * C[n] = sqrt(1 / J[n]) * D[n] + * + * Keeping in mind that we only need a rank compared to the other axes, we can + * further optimize the calculations: + * + * Square the expression to remove the square root: + * + * C[n]^2 = 1 / J[n] * D[n]^2 + * + * We don't care that C is squared, so we'll use it that way. Also note that + * we already have 1 / J[n] calculated for each axis. + */ + + float C; + float maxC = 0; + int jerk_axis = 0; + + for (int axis = 0; axis < AXES; axis++) + if (axis_square[axis]) { // Do not use fp_ZERO here + // Squaring axis_length ensures it's positive + C = axis_square[axis] * mach.a[axis].recip_jerk; + + if (maxC < C) { + maxC = C; + jerk_axis = axis; + } + } + + // Finally, the selected jerk term needs to be scaled by the + // reciprocal of the absolute value of the jerk_axis's unit + // vector term. This way when the move is finally decomposed into + // its constituent axes for execution the jerk for that axis will be + // at it's maximum value. + return mach.a[jerk_axis].jerk_max * JERK_MULTIPLIER / fabs(unit[jerk_axis]); +} + + +/// Compute cached jerk terms used by planning +static void _calc_and_cache_jerk_values(mp_buffer_t *bf) { + static float jerk = 0; + static float recip_jerk = 0; + static float cbrt_jerk = 0; + + if (JERK_MATCH_PRECISION < fabs(bf->jerk - jerk)) { // Tolerance comparison + jerk = bf->jerk; + recip_jerk = 1 / bf->jerk; + cbrt_jerk = cbrt(bf->jerk); + } + + bf->recip_jerk = recip_jerk; + bf->cbrt_jerk = cbrt_jerk; +} + + +static void _calc_max_velocities(mp_buffer_t *bf, float move_time) { + float junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit); + + bf->cruise_vmax = bf->length / move_time; // target velocity requested + bf->entry_vmax = min(bf->cruise_vmax, junction_velocity); + bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); + bf->exit_vmax = min(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax)); + bf->braking_velocity = bf->delta_vmax; + + // Zero out exact stop cases + if (mach_get_path_control() == PATH_EXACT_STOP) + bf->entry_vmax = bf->exit_vmax = 0; + else bf->replannable = true; +} + + +/*** Plan line acceleration / deceleration * - * This function uses constant jerk motion equations to plan - * acceleration and deceleration. Jerk is the rate of change of - * acceleration; it's the 1st derivative of acceleration, and the - * 3rd derivative of position. Jerk is a measure of impact to the - * machine. Controlling jerk smooths transitions between moves and - * allows for faster feeds while controlling machine oscillations - * and other undesirable side-effects. + * This function uses constant jerk motion equations to plan acceleration and + * deceleration. Jerk is the rate of change of acceleration; it's the 1st + * derivative of acceleration, and the 3rd derivative of position. Jerk is a + * measure of impact to the machine. Controlling jerk smooths transitions + * between moves and allows for faster feeds while controlling machine + * oscillations and other undesirable side-effects. * - * Note All math is done in absolute coordinates using single - * precision floating point (float). + * Notes: + * [1] All math is done in absolute coordinates using single precision floats. * - * Note: Returning a status that is not STAT_OK means the endpoint - * is NOT advanced. So lines that are too short to move will - * accumulate and get executed once the accumulated error exceeds - * the minimums. + * [2] Returning a status that is not STAT_OK means the endpoint is NOT + * advanced. So lines that are too short to move will accumulate and get + * executed once the accumulated error exceeds the minimums. */ stat_t mp_aline(const float target[], int32_t line) { - // Compute some reusable terms + // Compute axis and move lengths float axis_length[AXES]; float axis_square[AXES]; float length_square = 0; for (int axis = 0; axis < AXES; axis++) { - axis_length[axis] = target[axis] - mm.position[axis]; + axis_length[axis] = target[axis] - mp_get_axis_position(axis); axis_square[axis] = square(axis_length[axis]); length_square += axis_square[axis]; } float length = sqrt(length_square); - if (fp_ZERO(length)) { - report_request(); - return STAT_OK; - } - - // If mach_calc_move_time() says the move will take less than the - // minimum move time get a more accurate time estimate based on - // starting velocity and acceleration. The time of the move is - // determined by its initial velocity (Vi) and how much acceleration - // will occur. For this we need to look at the exit velocity of - // the previous block. There are 3 possible cases: - // - // (1) There is no previous block. - // Vi = 0 - // - // (2) Previous block is optimally planned. - // Vi = previous block's exit_velocity - // - // (3) Previous block is not optimally planned. - // Vi <= previous block's entry_velocity + delta_velocity - - // Set move time in state - float move_time = mach_calc_move_time(axis_length, axis_square); - - if (move_time < MIN_BLOCK_TIME) { - // Max velocity change for this move - float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; - float entry_velocity = 0; // pre-set as if no previous block - - mp_buffer_t *bf = mp_get_run_buffer(); - if (bf) { - if (bf->replannable) // not optimally planned - entry_velocity = bf->entry_velocity + bf->delta_vmax; - else entry_velocity = bf->exit_velocity; // optimally planned - } - - // Compute execution time for this move - move_time = 2 * length / (2 * entry_velocity + delta_velocity); - if (move_time < MIN_BLOCK_TIME) return STAT_MINIMUM_TIME_MOVE; - } + if (fp_ZERO(length)) return STAT_OK; // Get a buffer. Note, new buffers are initialized to zero. mp_buffer_t *bf = mp_get_write_buffer(); // current move pointer - if (!bf) return CM_ALARM(STAT_BUFFER_FULL_FATAL); // never fails + if (!bf) return CM_ALARM(STAT_BUFFER_FULL_FATAL); // should never fail - // Register callback to exec function + // Set buffer values bf->bf_func = mp_exec_aline; bf->length = length; - - // Copy model state into planner buffer copy_vector(bf->target, target); - // Compute the unit vector and find the right jerk to use (combined - // operations) To determine the jerk value to use for the block we - // want to find the axis for which the jerk cannot be exceeded - the - // 'jerk-limit' axis. This is the axis for which the time to - // decelerate from the target velocity to zero would be the longest. - // - // We can determine the "longest" deceleration in terms of time or distance. - // - // The math for time-to-decelerate T from speed S to speed E with constant - // jerk J is: - // - // T = 2 * sqrt((S - E) / J) - // - // Since E is always zero in this calculation, we can simplify: - // - // T = 2 * sqrt(S / J) - // - // The math for distance-to-decelerate l from speed S to speed E with - // constant jerk J is: - // - // l = (S + E) * sqrt((S - E) / J) - // - // Since E is always zero in this calculation, we can simplify: - // - // l = S * sqrt(S / J) - // - // The final value we want to know is which one is *longest*, - // compared to the others, so we don't need the actual value. This - // means that the scale of the value doesn't matter, so for T we - // can remove the "2 *" and For L we can remove the "S*". This - // leaves both to be simply "sqrt(S/J)". Since we don't need the - // scale, it doesn't matter what speed we're coming from, so S can - // be replaced with 1. - // - // However, we *do* need to compensate for the fact that each axis - // contributes differently to the move, so we will scale each - // contribution C[n] by the proportion of the axis movement length - // D[n] to the total length of the move L. Using that, we - // construct the following, with these definitions: - // - // J[n] = max_jerk for axis n - // D[n] = distance traveled for this move for axis n - // L = total length of the move in all axes - // C[n] = "axis contribution" of axis n - // - // For each axis n: - // - // C[n] = sqrt(1 / J[n]) * (D[n] / L) - // - // Keeping in mind that we only need a rank compared to the other - // axes, we can further optimize the calculations: - // - // Square the expression to remove the square root: - // - // C[n]^2 = (1 / J[n]) * (D[n] / L)^2 - // - // We don't care the C is squared, so we'll use it that way. - // - // Re-arranged to optimize divides for pre-calculated values, - // we create a value M that we compute once: - // - // M = 1 / L^2 - // - // Then we use it in the calculations for every axis: - // - // C[n] = 1 / J[n] * D[n]^2 * M - // - // Also note that we already have 1 / J[n] calculated for each axis, - // which simplifies it further. - // - // Finally, the selected jerk term needs to be scaled by the - // reciprocal of the absolute value of the jerk-limit axis's unit - // vector term. This way when the move is finally decomposed into - // its constituent axes for execution the jerk for that axis will be - // at it's maximum value. - - float C; // contribution term. C = T * a - float maxC = 0; - float recip_L2 = 1 / length_square; - int jerk_axis = 0; - + // Compute unit vector for (int axis = 0; axis < AXES; axis++) - // You cannot use the fp_XXX comparisons here! - if (fabs(axis_length[axis]) > 0) { - // compute unit vector term (zeros are already zero) - bf->unit[axis] = axis_length[axis] / bf->length; - // squaring axis_length ensures it's positive - C = axis_square[axis] * recip_L2 * mach.a[axis].recip_jerk; - - if (C > maxC) { - maxC = C; - jerk_axis = axis; // also needed for junction vmax calculation - } - } - - // set up and pre-compute the jerk terms needed for this round of planning - bf->jerk = mach.a[jerk_axis].jerk_max * JERK_MULTIPLIER / - fabs(bf->unit[jerk_axis]); // scale jerk - - // specialized comparison for tolerance of delta - if (fabs(bf->jerk - mm.jerk) > JERK_MATCH_PRECISION) { - mm.jerk = bf->jerk; // used before this point next time around - mm.recip_jerk = 1 / bf->jerk; // compute cached jerk terms used by planning - mm.cbrt_jerk = cbrt(bf->jerk); - } + bf->unit[axis] = axis_length[axis] / length; - bf->recip_jerk = mm.recip_jerk; - bf->cbrt_jerk = mm.cbrt_jerk; - - // exact stop cases already zeroed - float exact_stop = 0; - if (mach_get_path_control() != PATH_EXACT_STOP) { - bf->replannable = true; - exact_stop = 8675309; // an arbitrarily large floating point number - } - - // finish up the current block variables - float junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit); - bf->cruise_vmax = bf->length / move_time; // target velocity requested - bf->entry_vmax = min3(bf->cruise_vmax, junction_velocity, exact_stop); - bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); - bf->exit_vmax = - min3(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax), exact_stop); - bf->braking_velocity = bf->delta_vmax; + // Compute and cache jerk values + bf->jerk = _calc_jerk(axis_square, bf->unit); + _calc_and_cache_jerk_values(bf); - // NOTE: these next lines must remain in exact order. Position must update - // before committing the buffer. - mp_plan_block_list(bf); // plan block list - copy_vector(mm.position, target); // set the planner position + // Compute move time and velocities + float time = mach_calc_move_time(axis_length, axis_square); + _calc_max_velocities(bf, time); - // commit current block (must follow the position update) - mp_commit_write_buffer(line); + // Note, next the following lines must remain in order. + mp_plan_block_list(bf); // Plan block list + mp_set_position(target); // Set planner position before committing buffer + mp_commit_write_buffer(line); // Commit current block after position update return STAT_OK; } diff --git a/src/plan/line.h b/src/plan/line.h index d2ccbf0..1661db0 100644 --- a/src/plan/line.h +++ b/src/plan/line.h @@ -27,7 +27,7 @@ #pragma once -#include "machine.h" +#include "status.h" + -void mp_set_axis_position(int axis, float position); stat_t mp_aline(const float target[], int32_t line); diff --git a/src/plan/planner.c b/src/plan/planner.c index e3dfada..c98a070 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -69,9 +69,26 @@ #include +static float mp_position[AXES]; // final move position for planning purposes + + void planner_init() {mp_init_buffers();} +/// Set planner position for a single axis +void mp_set_axis_position(int axis, float position) { + mp_position[axis] = position; +} + + +float mp_get_axis_position(int axis) {return mp_position[axis];} + + +void mp_set_position(const float position[]) { + copy_vector(mp_position, position); +} + + /*** Flush all moves in the planner * * Does not affect the move currently running. Does not affect diff --git a/src/plan/planner.h b/src/plan/planner.h index db1417f..6e32c92 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -47,7 +47,6 @@ #define NOM_SEGMENT_TIME (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) #define MIN_SEGMENT_TIME (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) -#define MIN_BLOCK_TIME MIN_SEGMENT_TIME // minimum size Gcode block #define MIN_SEGMENT_TIME_PLUS_MARGIN \ ((MIN_SEGMENT_USEC + 1) / MICROSECONDS_PER_MINUTE) @@ -74,6 +73,9 @@ typedef enum { void planner_init(); +void mp_set_axis_position(int axis, float position); +float mp_get_axis_position(int axis); +void mp_set_position(const float position[]); void mp_flush_planner(); void mp_kinematics(const float travel[], float steps[]); void mp_plan_block_list(mp_buffer_t *bf); diff --git a/src/plan/runtime.c b/src/plan/runtime.c index 3bebf9b..7634886 100644 --- a/src/plan/runtime.c +++ b/src/plan/runtime.c @@ -141,7 +141,7 @@ void mp_runtime_set_steps_from_position() { * exist in several reference frames. The scheme to keep this * straight is: * - * - mm.position - start and end position for planning + * - mp_position - start and end position for planning * - rt.position - current position of runtime segment * - rt.steps.* - position in steps *