/* 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"
/* 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;
}
\******************************************************************************/
-/* 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"
#include <string.h>
-// 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
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)
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
}
-/* 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
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
*/
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]);
// 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);
// 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 {
// 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);
// 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];
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;
}
#include "machine.h"
#include "stepper.h"
#include "util.h"
-#include "report.h"
#include <string.h>
#include <stdbool.h>
#include <math.h>
-/// 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:
*
*
* 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
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;
}
-/*** 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;
}
projects / bbctrl-firmware / commitdiff