* We are using a quintic (fifth-degree) Bezier polynomial for the
* velocity curve. This gives us a "linear pop" velocity curve;
* with pop being the sixth derivative of position: velocity - 1st,
- * acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop -
- * 6th
+ * acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop - 6th
*
* The Bezier curve takes the form:
*
* the control points, and B_0(t) through B_5(t) are the Bernstein
* basis as follows:
*
- * B_0(t) = (1 - t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1
- * B_1(t) = 5(1 - t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t
- * B_2(t) = 10(1 - t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2
- * B_3(t) = 10(1 - t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3
- * B_4(t) = 5(1 - t) * t^4 = -5t^5 + 5t^4
- * B_5(t) = t^5 = t^5
+ * B_0(t) = (1 - t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1
+ * B_1(t) = 5(1 - t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t
+ * B_2(t) = 10(1 - t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2
+ * B_3(t) = 10(1 - t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3
+ * B_4(t) = 5(1 - t) * t^4 = -5t^5 + 5t^4
+ * B_5(t) = t^5 = t^5
*
- * ^ ^ ^ ^ ^ ^
- * A B C D E F
+ * ^ ^ ^ ^ ^ ^
+ * A B C D E F
*
* We use forward-differencing to calculate each position through the curve.
* This requires a formula of the form:
*
- * V_f(t) = A * t^5 + B * t^4 + C * t^3 + D * t^2 + E * t + F
+ * V_f(t) = A * t^5 + B * t^4 + C * t^3 + D * t^2 + E * t + F
*
- * Looking at the above B_0(t) through B_5(t) expanded forms, if we
- * take the coefficients of t^5 through t of the Bezier form of V(t),
- * we can determine that:
+ * Looking at the above B_0(t) through B_5(t) expanded forms, if we
+ * take the coefficients of t^5 through t of the Bezier form of V(t),
+ * we can determine that:
*
- * A = -P_0 + 5 * P_1 - 10 * P_2 + 10 * P_3 - 5 * P_4 + P_5
- * B = 5 * P_0 - 20 * P_1 + 30 * P_2 - 20 * P_3 + 5 * P_4
- * C = -10 * P_0 + 30 * P_1 - 30 * P_2 + 10 * P_3
- * D = 10 * P_0 - 20 * P_1 + 10 * P_2
- * E = - 5 * P_0 + 5 * P_1
- * F = P_0
+ * A = -P_0 + 5 * P_1 - 10 * P_2 + 10 * P_3 - 5 * P_4 + P_5
+ * B = 5 * P_0 - 20 * P_1 + 30 * P_2 - 20 * P_3 + 5 * P_4
+ * C = -10 * P_0 + 30 * P_1 - 30 * P_2 + 10 * P_3
+ * D = 10 * P_0 - 20 * P_1 + 10 * P_2
+ * E = - 5 * P_0 + 5 * P_1
+ * F = P_0
*
* Now, since we will (currently) *always* want the initial
* acceleration and jerk values to be 0, We set P_i = P_0 = P_1 =
* P_2 (initial velocity), and P_t = P_3 = P_4 = P_5 (target
* velocity), which, after simplification, resolves to:
*
- * A = - 6 * P_i + 6 * P_t
- * B = 15 * P_i - 15 * P_t
- * C = -10 * P_i + 10 * P_t
- * D = 0
- * E = 0
- * F = P_i
+ * A = - 6 * P_i + 6 * P_t
+ * B = 15 * P_i - 15 * P_t
+ * C = -10 * P_i + 10 * P_t
+ * D = 0
+ * E = 0
+ * F = P_i
*
* Given an interval count of I to get from P_i to P_t, we get the
* parametric "step" size of h = 1/I. We need to calculate the
* inital velocity V = P_i, then we iterate over the following I
* times:
*
- * V += F_5
- * F_5 += F_4
- * F_4 += F_3
- * F_3 += F_2
- * F_2 += F_1
+ * V += F_5
+ * F_5 += F_4
+ * F_4 += F_3
+ * F_3 += F_2
+ * F_2 += F_1
*
* See
* http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417
* the formulas somewhat. I'll not go into the long-winded
* step-by-step here, but it gives the resulting formulas:
*
- * a = A, b = B, c = C, d = D, e = E, f = F
+ * a = A, b = B, c = C, d = D, e = E, f = F
*
- * F_5(t + h) - F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 +
- * (10ah^3 + 6bh^2 + 3ch)t^2 + (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 +
- * bh^4 + ch^3 + dh^2 + eh
+ * F_5(t + h) - F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 +
+ * (10ah^3 + 6bh^2 + 3ch)t^2 + (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 +
+ * bh^4 + ch^3 + dh^2 + eh
*
- * a = 5ah
- * b = 10ah^2 + 4bh
- * c = 10ah^3 + 6bh^2 + 3ch
- * d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh
+ * a = 5ah
+ * b = 10ah^2 + 4bh
+ * c = 10ah^3 + 6bh^2 + 3ch
+ * d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh
*
- * (After substitution, simplification, and rearranging):
+ * After substitution, simplification, and rearranging:
*
- * F_4(t + h) - F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 +
- * (70ah^4 + 24bh^3 + 6ch^2)t + 30ah^5 + 14bh^4 + 6ch^3 + 2dh^2
+ * F_4(t + h) - F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 +
+ * (70ah^4 + 24bh^3 + 6ch^2)t + 30ah^5 + 14bh^4 + 6ch^3 + 2dh^2
*
- * a = 20ah^2
- * b = 60ah^3 + 12bh^2
- * c = 70ah^4 + 24bh^3 + 6ch^2
+ * a = 20ah^2
+ * b = 60ah^3 + 12bh^2
+ * c = 70ah^4 + 24bh^3 + 6ch^2
*
- * (After substitution, simplification, and rearranging):
+ * After substitution, simplification, and rearranging:
*
- * F_3(t+h)-F_3(t) = (60ah^3)t^2 + (180ah^4 + 24bh^3)t + 150ah^5 +
- * 36bh^4 + 6ch^3
+ * F_3(t + h) - F_3(t) = (60ah^3)t^2 + (180ah^4 + 24bh^3)t + 150ah^5 +
+ * 36bh^4 + 6ch^3
*
- * (You get the picture...)
+ * You get the picture...
*
- * F_2(t + h) - F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4
- * F_1(t + h) - F_1(t) = 120ah^5
+ * F_2(t + h) - F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4
+ * F_1(t + h) - F_1(t) = 120ah^5
*
- * Normally, we could then assign t = 0, use the A-F values from
- * above, and get out initial F_* values. However, for the sake of
- * "averaging" the velocity of each segment, we actually want to have
- * the initial V be be at t = h/2 and iterate I-1 times. So, the
- * resulting F_* values are (steps not shown):
+ * Normally, we could then assign t = 0, use the A-F values from
+ * above, and get out initial F_* values. However, for the sake of
+ * "averaging" the velocity of each segment, we actually want to have
+ * the initial V be be at t = h/2 and iterate I-1 times. So, the
+ * resulting F_* values are (steps not shown):
*
- * F_5 = 121Ah^5 / 16 + 5Bh^4 + 13Ch^3 / 4 + 2Dh^2 + Eh
- * F_4 = 165Ah^5 / 2 + 29Bh^4 + 9Ch^3 + 2Dh^2
- * F_3 = 255Ah^5 + 48Bh^4 + 6Ch^3
- * F_2 = 300Ah^5 + 24Bh^4
- * F_1 = 120Ah^5
+ * F_5 = 121Ah^5 / 16 + 5Bh^4 + 13Ch^3 / 4 + 2Dh^2 + Eh
+ * F_4 = 165Ah^5 / 2 + 29Bh^4 + 9Ch^3 + 2Dh^2
+ * F_3 = 255Ah^5 + 48Bh^4 + 6Ch^3
+ * F_2 = 300Ah^5 + 24Bh^4
+ * F_1 = 120Ah^5
*
- * Note that with our current control points, D and E are actually 0.
+ * Note that with our current control points, D and E are actually 0.
*/
static void _init_forward_diffs(float Vi, float Vt) {
float A = -6.0 * Vi + 6.0 * Vt;
* Everything here fires from interrupts and must be interrupt safe
*
* Returns:
+ *
* STAT_OK move is done
* STAT_EAGAIN move is not finished - has more segments to run
* STAT_NOOP cause no operation from the steppers - do not load the
* This routine is called from the (LO) interrupt level. The interrupt
* sequencing relies on the behaviors of the routines being exactly correct.
* Each call to _exec_aline() must execute and prep *one and only one*
- * segment. If the segment is the not the last segment in the bf buffer the
- * _aline() must return STAT_EAGAIN. If it's the last segment it must return
+ * segment. If the segment is not the last segment in the bf buffer the
+ * _aline() returns STAT_EAGAIN. If it's the last segment it returns
* STAT_OK. If it encounters a fatal error that would terminate the move it
- * should return a valid error code. Failure to obey this will introduce
- * subtle and very difficult to diagnose bugs (trust me on this).
+ * returns a valid error code.
+ *
+ * Notes:
*
- * Note 1 Returning STAT_OK ends the move and frees the bf buffer.
- * Returning STAT_OK at this point does NOT advance position meaning
- * any position error will be compensated by the next move.
+ * [1] Returning STAT_OK ends the move and frees the bf buffer.
+ * Returning STAT_OK at does NOT advance position meaning
+ * any position error will be compensated by the next move.
*
- * Note 2 Solves a potential race condition where the current move ends but
- * the new move has not started because the previous move is still
- * being run by the steppers. Planning can overwrite the new move.
+ * [2] Solves a potential race condition where the current move ends but
+ * the new move has not started because the previous move is still
+ * being run by the steppers. Planning can overwrite the new move.
*
* Operation:
+ *
* Aline generates jerk-controlled S-curves as per Ed Red's course notes:
+ *
* http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf
* http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations
*
- * A full trapezoid is divided into 5 periods Periods 1 and 2 are the
+ * A full trapezoid is divided into 5 periods. Periods 1 and 2 are the
* first and second halves of the acceleration ramp (the concave and convex
* parts of the S curve in the "head"). Periods 3 and 4 are the first
* and second parts of the deceleration ramp (the tail). There is also
* a period for the constant-velocity plateau of the trapezoid (the body).
- * There are various degraded trapezoids possible, including 2 section
- * combinations (head and tail; head and body; body and tail), and single
- * sections - any one of the three.
+ * There are many possible degenerate trapezoids where any of the 5 periods
+ * may be zero length but note that either none or both of a ramping pair can
+ * be zero.
*
* The equations that govern the acceleration and deceleration ramps are:
*
- * Period 1 V = Vi + Jm*(T^2)/2
- * Period 2 V = Vh + As*T - Jm*(T^2)/2
- * Period 3 V = Vi - Jm*(T^2)/2
- * Period 4 V = Vh + As*T + Jm*(T^2)/2
+ * Period 1 V = Vi + Jm * (T^2) / 2
+ * Period 2 V = Vh + As * T - Jm * (T^2) / 2
+ * Period 3 V = Vi - Jm * (T^2) / 2
+ * Period 4 V = Vh + As * T + Jm * (T^2) / 2
*
* These routines play some games with the acceleration and move timing
* to make sure this actually all works out. move_time is the actual time of
* 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)
* from _RUN to _OFF on final call
* or just remains _OFF
// copy in the gcode model state
memcpy(&mr.ms, &bf->ms, sizeof(MoveState_t));
bf->replannable = false;
- report_request();
+ report_request(); // Executing line number has changed
// short lines have already been removed, look for an actual zero
if (fp_ZERO(bf->length)) {
}
}
- // main dispatcher to process segments
- // from this point on the contents of the bf buffer do not affect execution
+ // Main segment processing dispatch. From this point on the contents of the
+ // bf buffer do not affect execution.
stat_t status = STAT_OK;
if (mr.section == SECTION_HEAD) status = _exec_aline_head();
mp_state_hold_callback(status == STAT_OK);
// There are 3 things that can happen here depending on return conditions:
- // status bf->move_state Description
- // ----------- -------------- ------------------------------------
- // STAT_EAGAIN <don't care> mr buffer has more segments to run
- // STAT_OK MOVE_RUN mr and bf buffers are done
- // STAT_OK MOVE_NEW mr done; bf must be run again
- // (it's been reused)
- if (status == STAT_EAGAIN) report_request();
- else {
- mr.move_state = MOVE_OFF; // reset mr buffer
+ //
+ // status bf->move_state Description
+ // ----------- -------------- ----------------------------------
+ // STAT_EAGAIN <don't care> mr buffer has more segments to run
+ // STAT_OK MOVE_RUN mr and bf buffers are done
+ // STAT_OK MOVE_NEW mr done; bf must be run again
+ // (it's been reused)
+ if (status != STAT_EAGAIN) {
+ mr.move_state = MOVE_OFF; // reset mr buffer
mr.section_state = SECTION_OFF;
- bf->nx->replannable = false; // prevent overplanning (Note 2)
+ bf->nx->replannable = false; // prevent overplanning (Note 2)
if (bf->move_state == MOVE_RUN) mp_free_run_buffer();
}
mpBuf_t *bp = mp_get_run_buffer(); // working buffer pointer
if (!bp) return; // Oops! nothing's running
- uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx
- float mr_available_length; // length left in mr buffer for deceleration
- float braking_velocity; // velocity left to shed to brake to zero
- float braking_length; // distance to brake to zero from braking_velocity
+ // examine and process mr buffer and compute length left for decel
+ float mr_available_length =
+ get_axis_vector_length(mr.final_target, mr.position);
- // examine and process mr buffer
- mr_available_length = get_axis_vector_length(mr.final_target, mr.position);
-
- // compute next_segment velocity
- braking_velocity = _compute_next_segment_velocity();
- // bp is OK to use here
- braking_length = mp_get_target_length(braking_velocity, 0, bp);
+ // compute next_segment velocity, velocity left to shed to brake to zero
+ float braking_velocity = _compute_next_segment_velocity();
+ // distance to brake to zero from braking_velocity, bp is OK to use here
+ float braking_length = mp_get_target_length(braking_velocity, 0, bp);
// Hack to prevent Case 2 moves for perfect-fit decels. Happens in
// homing situations. The real fix: The braking velocity cannot
bp->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer
_reset_replannable_list(); // make it replan all the blocks
- mp_plan_block_list(mp_get_last_buffer(), &mr_flag);
- mp_set_hold_state(FEEDHOLD_DECEL); // set state to decelerate and exit
+ mp_plan_block_list(mp_get_last_buffer(), true);
+ mp_set_hold_state(FEEDHOLD_DECEL); // set state to decelerate and exit
return;
}
for (int i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) {
mp_copy_buffer(bp, bp->nx); // copy bp+1 into bp+0, and onward
- // TODO What about dwells? Should be stopped when we reach a dwell.
+ // TODO What about dwells? Shouldn't we be stopped when we reach a dwell.
if (bp->move_type != MOVE_TYPE_ALINE) { // skip any non-move buffers
bp = mp_get_next_buffer(bp); // point to next buffer
continue;
bp->exit_vmax = bp->delta_vmax;
_reset_replannable_list(); // replan all the blocks
- mp_plan_block_list(mp_get_last_buffer(), &mr_flag);
+ mp_plan_block_list(mp_get_last_buffer(), true);
mp_set_hold_state(FEEDHOLD_DECEL); // set state to decelerate and exit
}
// NOTE: these next lines must remain in exact order. Position must update
// before committing the buffer.
- uint8_t mr_flag = false;
- mp_plan_block_list(bf, &mr_flag); // replan block list
+ mp_plan_block_list(bf, false); // replan block list
copy_vector(mm.position, bf->ms.target); // set the planner position
// commit current block (must follow the position update)
mp_commit_write_buffer(ms->line, MOVE_TYPE_ALINE);
* that are re-entered multiple times until a particular operation
* is complete. These functions have 2 parts - the initial call,
* which sets up the local context, and callbacks (continuations)
- * that are called from the main loop (in controller.c).
+ * that are called from the main loop.
*
* One important concept is isolation of the three layers of the
- * data model - the Gcode model (gm), planner model (bf queue &
- * mm), and runtime model (mr). These are designated as "model",
- * "planner" and "runtime" in function names.
+ * data model - the Gcode model (gm), planner model (bf queue & mm),
+ * and runtime model (mr).
*
- * The Gcode model is owned by the machine and should
- * only be accessed by mach_xxxx() functions. Data from the Gcode
- * model is transferred to the planner by the mp_xxx() functions
- * called by the machine.
+ * The Gcode model is owned by the machine and should only be
+ * accessed by mach_xxxx() functions. Data from the Gcode model is
+ * transferred to the planner by the mp_xxx() functions called by
+ * the machine.
*
* The planner should only use data in the planner model. When a
* move (block) is ready for execution the planner data is
}
-/* Flush all moves in the planner and all arcs
+/*** Flush all moves in the planner and all arcs
*
* Does not affect the move currently running in mr. Does not affect
* mm or gm model positions. This function is designed to be called
}
-/*
- * This rather brute-force and long-ish function sets section lengths
+// The minimum lengths are dynamic and depend on the velocity. These
+// expressions evaluate to the minimum lengths for the current velocity
+// settings. Note: The head and tail lengths are 2 minimum segments, the body
+// is 1 min segment.
+#define MIN_HEAD_LENGTH \
+ (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->entry_velocity))
+#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)
+
+
+/*** This rather brute-force and long-ish function sets section lengths
* and velocities based on the line length and velocities
* requested. It modifies the incoming bf buffer and returns accurate
* head, body and tail lengths, and accurate or reasonably approximate
- * velocities. We care about accuracy on lengths, less so for velocity
- * (as long as velocity err's on the side of too slow).
+ * velocities. We care about accuracy on lengths, less so for velocity,
+ * as long as velocity errs on the side of too slow.
*
* Note: We need the velocities to be set even for zero-length
* sections (Note: sections, not moves) so we can compute entry and
* exits for adjacent sections.
*
* Inputs used are:
+ *
* bf->length - actual block length, length is never changed
* bf->entry_velocity - requested Ve, entry velocity is never changed
* bf->cruise_velocity - requested Vt, is often changed
* short blocks
*
* Variables that may be set/updated are:
- * bf->entry_velocity - requested Ve
+ *
+ * bf->entry_velocity - requested Ve
* bf->cruise_velocity - requested Vt
* bf->exit_velocity - requested Vx
* bf->head_length - bf->length allocated to head
* bf->tail_length - bf->length allocated to tail
*
* 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
*
*
* Various cases handled (H=head, B=body, T=tail)
*
- * Requested-Fit cases
+ * Requested-Fit cases:
+ *
* HBT Ve<Vt>Vx sufficient length exists for all parts (corner
* case: HBT')
* HB Ve<Vt=Vx head accelerates to cruise - exits at full speed
* B Ve=Vt=Vx Velocities are close to each other and within
* matching tolerance
*
- * Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot
+ * Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot:
+ *
* HT (Ve=Vx)<Vt symmetric case. Split the length and compute Vt.
* HT' (Ve!=Vx)<Vt asymmetric case. Find H and T by successive
* approximation.
* H' body length < min body length - subsume body into head length
* T' body length < min body length - subsume body into tail length
*
- * Degraded fit cases - line is too short to satisfy both Ve and Vx
+ * Degraded fit cases - line is too short to satisfy both Ve and Vx:
+ *
* H" Ve<Vx Ve is degraded (velocity step). Vx is met
* T" Ve>Vx Ve is degraded (velocity step). Vx is met
* B" <short> line is very short but drawable; is treated as a
* 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 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.
+ * Note: The order of the cases/tests in the code is 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.
*/
-
-// The minimum lengths are dynamic and depend on the velocity.
-// These expressions evaluate to the minimum lengths for the current velocity
-// settings.
-// Note: The head and tail lengths are 2 minimum segments, the body is 1 min
-// segment.
-#define MIN_HEAD_LENGTH \
- (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->entry_velocity))
-#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)
-
-/// calculate trapezoid parameters
void mp_calculate_trapezoid(mpBuf_t *bf) {
// RULE #1 of mp_calculate_trapezoid(): Don't change bf->length
//
}
-/* Plans the entire block list
+/*** Plans the entire block list
*
* The block list is the circular buffer of planner buffers
* (bf's). The block currently being planned is the "bf" block. The
* "first block" is the next block to execute; queued immediately
* behind the currently executing block, aka the "running" block.
- * In some cases there is no first block because the list is empty
+ * In some cases, there is no first block because the list is empty
* or there is only one block and it is already running.
*
* If blocks following the first block are already optimally
* (effective) first block and the bf. It sets entry, exit and
* cruise v's from vmax's then calls trapezoid generation.
*
- * Variables that must be provided in the mpBuffers that will be
- * processed:
+ * Variables that must be provided in the mpBuffers that will be processed:
*
* bf (function arg) - end of block list (last block in time)
* bf->replannable - start of block list set by last FALSE value
* be set to length=0, entry_vmax=0 and exit_vmax=0
* and are treated as a momentary stop (plan to zero
* and from zero).
- *
* bf->length - provides block length
* bf->entry_vmax - used during forward planning to set entry velocity
* bf->cruise_vmax - used during forward planning to set cruise velocity
* bf->exit_vmax - used during forward planning to set exit velocity
* bf->delta_vmax - used during forward planning to set exit velocity
- *
* bf->recip_jerk - used during trapezoid generation
* bf->cbrt_jerk - used during trapezoid generation
*
* Variables that will be set during processing:
*
* bf->replannable - set if the block becomes optimally planned
- *
* bf->braking_velocity - set during backward planning
* bf->entry_velocity - set during forward planning
* bf->cruise_velocity - set during forward planning
* bf->exit_velocity - set during forward planning
- *
* bf->head_length - set during trapezoid generation
* bf->body_length - set during trapezoid generation
* bf->tail_length - set during trapezoid generation
* 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 blocks that require planning
- *
- * In normal operation the first block (currently running
- * block) is not replanned, but may be for feedholds and feed
- * overrides. In these cases the prep routines modify the
- * contents of the mr buffer and re-shuffle the block list,
- * re-enlisting the current bf buffer with new parameters.
- * These routines also set all blocks in the list to be
- * replannable so the list can be recomputed regardless of
- * exact stops and previous replanning optimizations.
+ * 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 blocks that require planning
+ *
+ * In normal operation the first block (currently running
+ * block) is not replanned, but may be for feedholds and feed
+ * overrides. In these cases the prep routines modify the
+ * contents of the mr buffer and re-shuffle the block list,
+ * re-enlisting the current bf buffer with new parameters.
+ * These routines also set all blocks in the list to be
+ * replannable so the list can be recomputed regardless of
+ * exact stops and previous replanning optimizations.
*
* [2] The mr_flag is used to tell replan to account for mr
- * buffer's exit velocity (Vx) mr's Vx is always found in the
- * provided bf buffer. Used to replan feedholds
+ * buffer's exit velocity (Vx) mr's Vx is always found in the
+ * provided bf buffer. Used to replan feedholds
*/
-void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) {
+void mp_plan_block_list(mpBuf_t *bf, bool mr_flag) {
mpBuf_t *bp = bf;
// Backward planning pass. Find first block and update the braking velocities.
min(bp->nx->entry_vmax, bp->nx->braking_velocity) + bp->delta_vmax;
}
- // forward planning pass - recomputes trapezoids in the list from the first
+ // Forward planning pass - recomputes trapezoids in the list from the first
// block to the bf block.
while ((bp = mp_get_next_buffer(bp)) != bf) {
- if (bp->pv == bf || *mr_flag) {
+ if (bp->pv == bf || mr_flag) {
bp->entry_velocity = bp->entry_vmax; // first block in the list
- *mr_flag = false;
+ mr_flag = false;
} else bp->entry_velocity = bp->pv->exit_velocity; // other blocks in list
bp->cruise_velocity = bp->cruise_vmax;
- bp->exit_velocity = min4(bp->exit_vmax,
- bp->nx->entry_vmax,
+ bp->exit_velocity = min4(bp->exit_vmax, bp->nx->entry_vmax,
bp->nx->braking_velocity,
bp->entry_velocity + bp->delta_vmax);
}
-/* This set of functions returns the fourth thing knowing the other three.
+/*** Derive accel/decel length from delta V and jerk
+ *
+ * A convenient function for determining the optimal_length (L) of a line
+ * given the initial velocity (Vi), final velocity (Vf) and maximum jerk (Jm).
+ *
+ * Definitions:
*
* Jm = the given maximum jerk
* T = time of the entire move
* Vi = initial velocity
* Vf = final velocity
- * T = 2*sqrt((Vt-Vi)/Jm)
- * As = The acceleration at inflection point between convex and concave
- * portions of the S-curve.
- * As = (Jm*T)/2
+ * T = time
+ * As = accel at inflection point between convex & concave portions of S-curve
* Ar = ramp acceleration
- * Ar = As/2 = (Jm*T)/4
- *
- * mp_get_target_length() is a convenient function for determining the
- * optimal_length (L) of a line given the initial velocity (Vi), final
- * velocity (Vf) and maximum jerk (Jm).
- *
- * The length (distance) equation is derived from:
- *
- * a) L = (Vf-Vi) * T - (Ar*T^2)/2 ... which becomes b) with
- * substitutions for Ar and T
- * b) L = (Vf-Vi) * 2*sqrt((Vf-Vi)/Jm) - (2*sqrt((Vf-Vi)/Jm) * (Vf-Vi))/2
- * c) L = (Vf-Vi)^(3/2) / sqrt(Jm) ...is an alternate form of b)
- * (see Wolfram Alpha)
- * c') L = (Vf-Vi) * sqrt((Vf-Vi)/Jm) ... second alternate form; requires
- * Vf >= Vi
- *
- * Notes: Ar = (Jm*T)/4 Ar is ramp acceleration
- * T = 2*sqrt((Vf-Vi)/Jm) T is time
- * Assumes Vi, Vf and L are positive or zero
- * Cannot assume Vf>=Vi due to rounding errors and use of
- * PLANNER_VELOCITY_TOLERANCE necessitating the introduction of
- * fabs()
- *
- * mp_get_target_velocity() is a convenient function for determining Vf
- * target velocity for a given the initial velocity (Vi), length (L), and
- * maximum jerk (Jm). Equation d) is b) solved for Vf. Equation e) is
- * c) solved for Vf. Use e) (obviously)
- *
- * d) Vf = (sqrt(L)*(L/sqrt(1/Jm))^(1/6)+(1/Jm)^(1/4)*Vi)/(1/Jm)^(1/4)
- * e) Vf = L^(2/3) * Jm^(1/3) + Vi
- *
- * FYI: Here's an expression that returns the jerk for a given deltaV and L:
- * return cube(deltaV / (pow(L, 0.66666666)));
+ *
+ * Formulas:
+ *
+ * T = 2 * sqrt((Vt - Vi) / Jm)
+ * As = (Jm * T) / 2
+ * Ar = As / 2 = (Jm * T) / 4
+ *
+ * Then the length (distance) equation is:
+ *
+ * a) L = (Vf - Vi) * T - (Ar * T^2) / 2
+ *
+ * Substituting for Ar and T:
+ *
+ * b) L = (Vf - Vi) * 2 * sqrt((Vf - Vi) / Jm) -
+ * (2 * sqrt((Vf - Vi) / Jm) * (Vf - Vi)) / 2
+ *
+ * Reducing b). See Wolfram Alpha:
+ *
+ * c) L = (Vf - Vi)^(3/2) / sqrt(Jm)
+ *
+ * Assuming Vf >= Vi [Note 2]:
+ *
+ * d) L = (Vf - Vi) * sqrt((Vf - Vi) / Jm)
+ *
+ * Notes:
+ *
+ * [1] Assuming Vi, Vf and L are positive or zero.
+ * [2] Cannot assume Vf >= Vi due to rounding errors and use of
+ * PLANNER_VELOCITY_TOLERANCE necessitating the introduction of fabs()
*/
-
-/// 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:
+#define GET_VELOCITY_ITERATIONS 0 // must be zero or more
+
+/*** Derive velocity achievable from delta V and length
+ *
+ * A convenient function for determining Vf target velocity for a given
+ * initial velocity (Vi), length (L), and maximum jerk (Jm). Equation e) is
+ * b) solved for Vf. Equation f) is c) solved for Vf. Use f) (obviously)
+ *
+ * e) Vf = (sqrt(L) * (L / sqrt(1 / Jm))^(1/6) +
+ * (1 / Jm)^(1/4) * Vi) / (1 / Jm)^(1/4)
+ *
+ * f) Vf = L^(2/3) * Jm^(1/3) + Vi
+ *
+ * FYI: Here's an expression that returns the jerk for a given deltaV and L:
+ *
+ * cube(deltaV / (pow(L, 0.66666666)));
*
* We do some Newton-Raphson iterations to narrow it down.
* We need a formula that includes known variables except the one we want to
* find, and has a root [Z(x) = 0] at the value (x) we are looking for.
*
- * Z(x) = zero at x -- we calculate the value from the knowns and the
- * estimate (see below) and then subtract the known
- * value to get zero (root) if x is the correct value.
- * Vi = initial velocity (known)
- * Vf = estimated final velocity
- * J = jerk (known)
- * L = length (know)
+ * Z(x) = zero at x
+ *
+ * We calculate the value from the knowns and the estimate (see below) and then
+ * subtract the known value to get zero (root) if x is the correct value.
+ *
+ * Vi = initial velocity (known)
+ * Vf = estimated final velocity
+ * J = jerk (known)
+ * L = length (know)
*
* There are (at least) two such functions we can use:
- * L from J, Vi, and Vf
- * L = sqrt((Vf - Vi) / J) (Vi + Vf)
*
- * Replacing Vf with x, and subtracting the known L:
- * 0 = sqrt((x - Vi) / J) (Vi + x) - L
- * Z(x) = sqrt((x - Vi) / J) (Vi + x) - L
+ * L from J, Vi, and Vf:
+ *
+ * L = sqrt((Vf - Vi) / J) * (Vi + Vf)
+ *
+ * Replacing Vf with x, and subtracting the known L we get:
+ *
+ * 0 = sqrt((x - Vi) / J) * (Vi + x) - L
+ * Z(x) = sqrt((x - Vi) / J) * (Vi + x) - L
*
- * Or
- * J from L, Vi, and Vf
- * J = ((Vf - Vi) (Vi + Vf)^2) / L^2
+ * Or J from L, Vi, and Vf:
*
- * Replacing Vf with x, and subtracting the known J:
- * 0 = ((x - Vi) (Vi + x)^2) / L^2 - J
- * Z(x) = ((x - Vi) (Vi + x)^2) / L^2 - J
+ * J = ((Vf - Vi) * (Vi + Vf)^2) / L^2
*
- * L doesn't resolve to the value very quickly (it graphs near-vertical).
+ * Replacing Vf with x, and subtracting the known J we get:
+ *
+ * 0 = ((x - Vi) * (Vi + x)^2) / L^2 - J
+ * Z(x) = ((x - Vi) * (Vi + x)^2) / L^2 - J
+ *
+ * L doesn't resolve to the value very quickly (its graph is nearly vertical).
* So, we'll use J, which resolves in < 10 iterations, often in only two or
* three with a good estimate.
*
* they are for both the (unused) L and the (used) J formulas above:
*
* J > 0, Vi > 0, Vf > 0
- * SqrtDeltaJ = sqrt((x - Vi) * J)
- * SqrtDeltaOverJ = sqrt((x - Vi) / J)
- * L'(x) = SqrtDeltaOverJ + (Vi + x) / (2*J) + (Vi + x) / (2 * SqrtDeltaJ)
+ * A = sqrt((x - Vi) * J)
+ * B = sqrt((x - Vi) / J)
+ *
+ * L'(x) = B + (Vi + x) / (2 * J) + (Vi + x) / (2 * A)
*
* J'(x) = (2 * Vi * x - Vi^2 + 3 * x^2) / L^2
*/
-
-#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;
-#endif
+ float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate
+
+#if (GET_VELOCITY_ITERATIONS > 0)
+ float L2 = L * L;
+ float Vi2 = Vi * Vi;
-#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;
+ for (int i = 0; i < GET_VELOCITY_ITERATIONS; i++)
+ // x' = x - Z(x) / J'(x)
+ x = x - ((x - Vi) * square(Vi + x) / L2 - bf->jerk) /
+ ((2 * Vi * x - Vi2 + 3 * x * x) / L2);
#endif
- return estimate;
+ return x;
}
#include "buffer.h"
#include "util.h"
+#include <stdbool.h>
+
// Most of these factors are the result of a lot of tweaking.
// Change with caution.
void mp_zero_segment_velocity();
uint8_t mp_get_runtime_busy();
void mp_kinematics(const float travel[], float steps[]);
-void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag);
+void mp_plan_block_list(mpBuf_t *bf, bool mr_flag);
float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf);
float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf);
inline int32_t mp_get_line() {return mr.ms.line;}
}
-void mp_set_hold_state(holdState_t hold) {
- ps.hold = hold;
-}
+void mp_set_hold_state(holdState_t hold) {ps.hold = hold;}
void mp_state_running() {
void mp_state_idle() {
mp_set_state(STATE_READY);
- mp_set_hold_state(FEEDHOLD_OFF); // if in feedhold, end it
- ps.start_requested = false; // cancel any pending cycle start request
+ mp_set_hold_state(FEEDHOLD_OFF); // if in feedhold, end it
+ ps.start_requested = false; // cancel any pending start request
mp_zero_segment_velocity(); // for reporting purposes
}
-void mp_state_estop() {
- mp_set_state(STATE_ESTOPPED);
-}
+void mp_state_estop() {mp_set_state(STATE_ESTOPPED);}
+/// Called by the planner to update feedhold state in sync with planning
void mp_state_hold_callback(bool done) {
- // Feedhold processing. Refer to state.h for state machine
- // Catch the feedhold request and start the planning the hold
- if (mp_get_hold_state() == FEEDHOLD_SYNC) mp_set_hold_state(FEEDHOLD_PLAN);
-
- // Look for the end of the decel to go into HOLD state
- if (mp_get_hold_state() == FEEDHOLD_DECEL && done) {
- mp_set_hold_state(FEEDHOLD_HOLD);
- mp_set_state(STATE_HOLDING);
- }
-}
+ switch (mp_get_hold_state()) {
+ case FEEDHOLD_OFF: case FEEDHOLD_PLAN: case FEEDHOLD_HOLD: break;
+ // Catch the feedhold request and start planning the hold
+ case FEEDHOLD_SYNC: mp_set_hold_state(FEEDHOLD_PLAN); break;
-/* Feedholds, queue flushes and cycles starts are all related. The request
- * functions set flags for these. The sequencing callback interprets the flags
- * according to the following rules:
- *
- * Feedhold request received during motion is honored
- * Feedhold request received during a feedhold is ignored and reset
- * Feedhold request received during a motion stop is ignored and reset
- *
- * Queue flush request received during motion is ignored but not reset
- * Queue flush request received during a feedhold is deferred until
- * the feedhold enters a HOLD state (i.e. until deceleration is complete)
- * Queue flush request received during a motion stop is honored
- *
- * Cycle start request received during motion is ignored and reset
- * Cycle start request received during a feedhold is deferred until
- * the feedhold enters a HOLD state (i.e. until deceleration is complete)
- * If a queue flush request is also present the queue flush is done first
- * Cycle start request received during a motion stop is honored and starts
- * to run anything in the planner queue
- */
+ case FEEDHOLD_DECEL:
+ // Look for the end of the decel to go into HOLD state
+ if (done) {
+ mp_set_hold_state(FEEDHOLD_HOLD);
+ mp_set_state(STATE_HOLDING);
+ }
+ break;
+ }
+}
void mp_request_hold() {ps.hold_requested = true;}
void mp_request_start() {ps.start_requested = true;}
+/*** Feedholds, queue flushes and starts are all related. The request functions
+ * set flags. The callback interprets the flags according to these rules:
+ *
+ * A hold request received:
+ * - during motion is honored
+ * - during a feedhold is ignored and reset
+ * - when already stopped is ignored and reset
+ *
+ * A flush request received:
+ * - during motion is ignored but not reset
+ * - during a feedhold is deferred until the feedhold enters HOLD state.
+ * I.e. until deceleration is complete.
+ * - when stopped or holding and the planner is not busy, is honored
+ *
+ * A start request received:
+ * - during motion is ignored and reset
+ * - during a feedhold is deferred until the feedhold enters a HOLD state.
+ * I.e. until deceleration is complete. If a queue flush request is also
+ * present the queue flush is done first
+ * - when stopped is honored and starts to run anything in the planner queue
+ */
void mp_state_callback() {
if (ps.hold_requested) {
ps.hold_requested = false;
!mp_get_runtime_busy()) {
ps.flush_requested = false;
- mp_flush_planner(); // flush planner queue
+ mp_flush_planner();
// NOTE: The following uses low-level mp calls for absolute position
for (int axis = 0; axis < AXES; axis++)
- // set mm from mr
mach_set_position(axis, mp_get_runtime_absolute_position(axis));
}
- // Don't start cycle when stopping
+ // Don't start while stopping
if (ps.start_requested && mp_get_state() != STATE_STOPPING) {
ps.start_requested = false;
}
}
- mp_plan_hold_callback(); // feedhold state machine
+ mp_plan_hold_callback(); // call feedhold planner
}
void mp_state_running();
void mp_state_idle();
void mp_state_estop();
+
void mp_state_hold_callback(bool done);
void mp_request_hold();
projects / bbctrl-firmware / commitdiff