From 6fa288613e7e86a1b9bbde181b5ea698d2202607 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 28 Aug 2016 15:11:56 -0700 Subject: [PATCH] Documentation and code cleanup --- src/plan/exec.c | 192 ++++++++++++++-------------- src/plan/feedhold.c | 26 ++-- src/plan/line.c | 3 +- src/plan/planner.c | 299 ++++++++++++++++++++++---------------------- src/plan/planner.h | 4 +- src/plan/state.c | 84 ++++++------- src/plan/state.h | 1 + 7 files changed, 306 insertions(+), 303 deletions(-) diff --git a/src/plan/exec.c b/src/plan/exec.c index 9be4d7d..a1e5598 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -127,8 +127,7 @@ static stat_t _exec_aline_segment() { * 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: * @@ -139,43 +138,43 @@ static stat_t _exec_aline_segment() { * 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 @@ -183,11 +182,11 @@ static stat_t _exec_aline_segment() { * 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 @@ -196,49 +195,49 @@ static stat_t _exec_aline_segment() { * 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; @@ -633,6 +632,7 @@ static stat_t _exec_aline_head() { * 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 @@ -642,40 +642,43 @@ static stat_t _exec_aline_head() { * 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 @@ -683,9 +686,10 @@ static stat_t _exec_aline_head() { * 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 @@ -709,7 +713,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { // 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)) { @@ -757,8 +761,8 @@ stat_t mp_exec_aline(mpBuf_t *bf) { } } - // 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(); @@ -769,17 +773,17 @@ stat_t mp_exec_aline(mpBuf_t *bf) { 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 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 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(); } diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c index 1c56e4e..8983bfe 100644 --- a/src/plan/feedhold.c +++ b/src/plan/feedhold.c @@ -117,18 +117,14 @@ void mp_plan_hold_callback() { 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 @@ -156,8 +152,8 @@ void mp_plan_hold_callback() { 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; } @@ -179,7 +175,7 @@ void mp_plan_hold_callback() { 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; @@ -211,6 +207,6 @@ void mp_plan_hold_callback() { 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 } diff --git a/src/plan/line.c b/src/plan/line.c index 1b65a24..9bdbaed 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -372,8 +372,7 @@ stat_t mp_aline(MoveState_t *ms) { // 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); diff --git a/src/plan/planner.c b/src/plan/planner.c index ea21eab..e0d67d0 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -37,17 +37,16 @@ * 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 @@ -79,7 +78,7 @@ void planner_init() { } -/* 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 @@ -202,19 +201,30 @@ void mp_kinematics(const float travel[], float steps[]) { } -/* - * 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 @@ -224,7 +234,8 @@ void mp_kinematics(const float travel[], float steps[]) { * 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 @@ -232,6 +243,7 @@ void mp_kinematics(const float travel[], float steps[]) { * 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 * @@ -263,7 +275,8 @@ void mp_kinematics(const float travel[], float steps[]) { * * Various cases handled (H=head, B=body, T=tail) * - * Requested-Fit cases + * Requested-Fit cases: + * * HBT VeVx sufficient length exists for all parts (corner * case: HBT') * HB VeVx Ve is degraded (velocity step). Vx is met * B" line is very short but drawable; is treated as a @@ -292,25 +307,11 @@ void mp_kinematics(const float travel[], float steps[]) { * F force fit: This block is slowed down until it can * be executed * - * NOTE: The order of the cases/tests in the code is pretty - * important. Start with the 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 // @@ -520,13 +521,13 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { } -/* 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 @@ -537,8 +538,7 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { * (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 @@ -547,25 +547,21 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { * 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 @@ -581,24 +577,24 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { * 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. @@ -609,18 +605,17 @@ void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) { 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); @@ -643,89 +638,107 @@ void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) { } -/* 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. * @@ -733,38 +746,26 @@ float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf) { * 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; } diff --git a/src/plan/planner.h b/src/plan/planner.h index fe3f37a..95d46be 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -34,6 +34,8 @@ #include "buffer.h" #include "util.h" +#include + // Most of these factors are the result of a lot of tweaking. // Change with caution. @@ -145,7 +147,7 @@ void mp_set_runtime_work_offset(float offset[]); 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;} diff --git a/src/plan/state.c b/src/plan/state.c index 09dcc8c..0acb438 100644 --- a/src/plan/state.c +++ b/src/plan/state.c @@ -113,9 +113,7 @@ void mp_set_cycle(plannerCycle_t cycle) { } -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() { @@ -125,50 +123,32 @@ 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;} @@ -176,6 +156,27 @@ void mp_request_flush() {ps.flush_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; @@ -192,15 +193,14 @@ void mp_state_callback() { !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; @@ -213,5 +213,5 @@ void mp_state_callback() { } } - mp_plan_hold_callback(); // feedhold state machine + mp_plan_hold_callback(); // call feedhold planner } diff --git a/src/plan/state.h b/src/plan/state.h index e0656cd..0ea36bf 100644 --- a/src/plan/state.h +++ b/src/plan/state.h @@ -75,6 +75,7 @@ PGM_P mp_get_cycle_pgmstr(plannerCycle_t cycle); void mp_state_running(); void mp_state_idle(); void mp_state_estop(); + void mp_state_hold_callback(bool done); void mp_request_hold(); -- 2.27.0