#include <stdbool.h>
#include <string.h>
-// 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.
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
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);
// 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.
*
* 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
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)
// 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)
_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
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
* 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];
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
* 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));
- }
}
#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;
* 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)
* 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;
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;
}
#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
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;
}
#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
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;
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;
#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
#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
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)
*
* 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
}
-/***** ALINE HELPERS *****
- * _calc_move_times()
- * _plan_block_list()
- * _get_junction_vmax()
- * _reset_replannable_list()
- */
-
/*
* Compute optimal and minimum move times into the gcode_state
*
* 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
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);
* 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
* 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
* 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] -
* 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
* 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
#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);
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
*
* - 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
* 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];
/************************************************************************************
- * 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_<command>, 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_<command>, 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;
}
+/// callback to execute command
static stat_t _exec_command(mpBuf_t *bf) {
st_prep_command(bf);
return STAT_OK;
}
-/*************************************************************************
- * 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;
}
+/// 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
return STAT_OK;
}
+
/**** PLANNER BUFFERS *****************************************************
*
* Planner buffers are used to queue and operate on Gcode blocks. Each buffer
* 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;
}
-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
}
+/// 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
}
-/*** 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;
}
+/* 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) ||
}
+/* 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
}
+/// 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;
}
+/// 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;
}
+/// 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;
* 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
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);
* 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
* T" Ve>Vx Ve is degraded (velocity step). Vx is met
* B" <short> line is very short but drawable; is treated as a body only
* F <too short> 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.
#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
* 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.
* 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;
}
/****************************************************************************************
* 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
projects / bbctrl-firmware / commitdiff