From 8e17b06f5c80d1c1eb5a0246beb86732d591561d Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Tue, 13 Sep 2016 03:21:31 -0700 Subject: [PATCH] Implemented stepping --- src/command.c | 3 +- src/config.h | 16 +- src/homing.c | 2 + src/machine.c | 2 +- src/plan/buffer.h | 1 + src/plan/calibrate.c | 74 ++++---- src/plan/exec.c | 419 ++++++++++++++++++++++--------------------- src/plan/jog.c | 2 + src/plan/line.c | 3 +- src/plan/planner.c | 187 ++++++++++--------- src/plan/planner.h | 10 +- src/plan/runtime.c | 2 +- src/plan/state.c | 17 +- src/plan/state.h | 1 + src/probing.c | 2 + 15 files changed, 402 insertions(+), 339 deletions(-) diff --git a/src/command.c b/src/command.c index 6f7e483..b14a392 100644 --- a/src/command.c +++ b/src/command.c @@ -77,7 +77,7 @@ static void command_i2c_cb(i2c_cmd_t cmd, uint8_t *data, uint8_t length) { case I2C_PAUSE: mp_request_hold(); break; case I2C_OPTIONAL_PAUSE: mp_request_optional_pause(); break; case I2C_RUN: mp_request_start(); break; - case I2C_STEP: break; // TODO + case I2C_STEP: mp_request_step(); break; case I2C_FLUSH: mp_request_flush(); break; case I2C_REPORT: report_request_full(); break; case I2C_HOME: break; // TODO @@ -223,6 +223,7 @@ void command_callback() { case 0: break; // Empty line case '{': status = vars_parser(_cmd); break; case '$': status = command_parser(_cmd); break; + case '%': break; // GCode program separator, ignore it default: if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;} diff --git a/src/config.h b/src/config.h index 59f3472..e81f842 100644 --- a/src/config.h +++ b/src/config.h @@ -384,14 +384,22 @@ typedef enum { // Planner -/// Should be at least the number of buffers requires to support optimal -/// planning in the case of very short lines or arc segments. Suggest 12 min. -/// Limit is 255. -#define PLANNER_BUFFER_POOL_SIZE 32 +/// Should be at least the number of buffers required to support optimal +/// planning in the case of very short lines or arc segments. Suggest no less +/// than 12. Maximum is 255 with out also changing the type of mb.space. Must +/// leave headroom for stack. +#define PLANNER_BUFFER_POOL_SIZE 48 /// Buffers to reserve in planner before processing new input line #define PLANNER_BUFFER_HEADROOM 4 +/// Minimum number of filled buffers before timeout until execution starts +#define PLANNER_EXEC_MIN_FILL 4 + +/// Delay before executing new buffers unless PLANNER_EXEC_MIN_FILL is met +/// This gives the planner a chance to make a good plan before execution starts +#define PLANNER_EXEC_DELAY 250 // In ms + // I2C #define I2C_DEV TWIC diff --git a/src/homing.c b/src/homing.c index 3d96757..22d9dd1 100644 --- a/src/homing.c +++ b/src/homing.c @@ -186,6 +186,8 @@ static void _homing_finalize_exit() { mach_set_feed_mode(hm.saved_feed_mode); mach_set_feed_rate(hm.saved_feed_rate); mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); + + mp_set_cycle(CYCLE_MACHINING); // Default cycle } diff --git a/src/machine.c b/src/machine.c index 246be38..21f9362 100644 --- a/src/machine.c +++ b/src/machine.c @@ -950,7 +950,7 @@ void mach_program_end() { mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE); mach.gm.spindle_mode = SPINDLE_OFF; spindle_set(SPINDLE_OFF, 0); - mach_flood_coolant_control(false); // M9 + mach_flood_coolant_control(false); // M9 mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // G94 mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); } diff --git a/src/plan/buffer.h b/src/plan/buffer.h index 9212194..4230bcc 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -57,6 +57,7 @@ typedef struct mp_buffer_t { // See Planning Velocity Notes buffer_state_t state; // buffer state bool replannable; // true if move can be re-planned + bool hold; // hold at the start of this block float value; // used in dwell and other callbacks diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c index 18e873a..a341d00 100644 --- a/src/plan/calibrate.c +++ b/src/plan/calibrate.c @@ -80,51 +80,53 @@ static stat_t _exec_calibrate(mp_buffer_t *bf) { const float time = MIN_SEGMENT_TIME; // In minutes const float max_delta_v = JOG_ACCELERATION * time; - if (rtc_expired(cal.wait)) - switch (cal.state) { - case CAL_START: { - cal.axis = motor_get_axis(cal.motor); - cal.state = CAL_ACCEL; - cal.velocity = 0; - cal.stall_valid = false; - cal.stalled = false; - cal.reverse = false; - - tmc2660_set_stallguard_threshold(cal.motor, 8); - cal.wait = rtc_get_time() + CAL_WAIT_TIME; - - break; - } + do { + if (rtc_expired(cal.wait)) + switch (cal.state) { + case CAL_START: { + cal.axis = motor_get_axis(cal.motor); + cal.state = CAL_ACCEL; + cal.velocity = 0; + cal.stall_valid = false; + cal.stalled = false; + cal.reverse = false; + + tmc2660_set_stallguard_threshold(cal.motor, 8); + cal.wait = rtc_get_time() + CAL_WAIT_TIME; + + break; + } + + case CAL_ACCEL: + if (CAL_MIN_VELOCITY < cal.velocity) cal.stall_valid = true; - case CAL_ACCEL: - if (CAL_MIN_VELOCITY < cal.velocity) cal.stall_valid = true; + if (cal.velocity < CAL_MIN_VELOCITY || CAL_TARGET_SG < cal.stallguard) + cal.velocity += max_delta_v; - if (cal.velocity < CAL_MIN_VELOCITY || CAL_TARGET_SG < cal.stallguard) - cal.velocity += max_delta_v; + if (cal.stalled) { + if (cal.reverse) { + int32_t steps = -motor_get_encoder(cal.motor); + float mm = (float)steps / motor_get_steps_per_unit(cal.motor); + STATUS_DEBUG("%"PRIi32" steps %0.2f mm", steps, mm); - if (cal.stalled) { - if (cal.reverse) { - int32_t steps = -motor_get_encoder(cal.motor); - float mm = (float)steps / motor_get_steps_per_unit(cal.motor); - STATUS_DEBUG("%"PRIi32" steps %0.2f mm", steps, mm); + tmc2660_set_stallguard_threshold(cal.motor, 63); - tmc2660_set_stallguard_threshold(cal.motor, 63); + mp_set_cycle(CYCLE_MACHINING); // Default cycle - return STAT_OK; // Done + return STAT_NOOP; // Done, no move queued - } else { - motor_set_encoder(cal.motor, 0); + } else { + motor_set_encoder(cal.motor, 0); - cal.reverse = true; - cal.velocity = 0; - cal.stall_valid = false; - cal.stalled = false; + cal.reverse = true; + cal.velocity = 0; + cal.stall_valid = false; + cal.stalled = false; + } } + break; } - break; - } - - if (!cal.velocity) return STAT_EAGAIN; + } while (fp_ZERO(cal.velocity)); // Repeat if computed velocity was zero // Compute travel float travel[AXES] = {0}; // In mm diff --git a/src/plan/exec.c b/src/plan/exec.c index c2f0d39..f097a75 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -55,14 +55,14 @@ typedef struct { float cruise_velocity; float exit_velocity; - float segments; // number of segments in line or arc - uint32_t segment_count; // count of running segments - float segment_velocity; // computed velocity for aline segment - float segment_time; // actual time increment per aline segment - float forward_diff[5]; // forward difference levels - bool hold_planned; // true when a feedhold has been planned - move_section_t section; // what section is the move in? - bool section_new; // true if it's a new section + float segments; // number of segments in line or arc + uint32_t segment_count; // count of running segments + float segment_velocity; // computed velocity for aline segment + float segment_time; // actual time increment per aline segment + float forward_diff[5]; // forward difference levels + bool hold_planned; // true when a feedhold has been planned + move_section_t section; // what section is the move in? + bool section_new; // true if it's a new section } mp_exec_t; @@ -94,123 +94,120 @@ static stat_t _exec_aline_segment() { } -/*** Forward difference math explained: - * - * 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 - * - * The Bezier curve takes the form: - * - * V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + - * P_4 * B_4(t) + P_5 * B_5(t) - * - * Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are - * 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 - * - * ^ ^ ^ ^ ^ ^ - * 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 - * - * 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 - * - * 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 - * - * 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 - * initial value of forward differences (F_0 - F_5) such that 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 - * - * See - * http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417 - * for an example of how to calculate F_0 - F_5 for a cubic bezier - * curve. Since this is a quintic bezier curve, we need to extend - * 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 - * - * 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 - * - * 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 - * - * a = 20ah^2 - * b = 60ah^3 + 12bh^2 - * c = 70ah^4 + 24bh^3 + 6ch^2 - * - * 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 - * - * 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 - * - * 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 - * - * Note that with our current control points, D and E are actually 0. - */ +/// Forward differencing math +/// +/// 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 +/// +/// The Bezier curve takes the form: +/// +/// V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + +/// P_4 * B_4(t) + P_5 * B_5(t) +/// +/// Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are +/// 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 +/// +/// ^ ^ ^ ^ ^ ^ +/// 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 +/// +/// 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 +/// +/// 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 +/// +/// 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 initial value of forward +/// differences (F_0 - F_5) such that 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 +/// +/// See +/// http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417 +/// for an example of how to calculate F_0 - F_5 for a cubic bezier curve. Since +/// this is a quintic bezier curve, we need to extend 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 +/// +/// 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 +/// +/// 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 +/// +/// a = 20ah^2 +/// b = 60ah^3 + 12bh^2 +/// c = 70ah^4 + 24bh^3 + 6ch^2 +/// +/// 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 +/// +/// 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 +/// +/// 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 +/// +/// Note that with our current control points, D and E are actually 0. static float _init_forward_diffs(float Vi, float Vt, float segments) { float A = -6.0 * Vi + 6.0 * Vt; float B = 15.0 * Vi - 15.0 * Vt; @@ -342,18 +339,17 @@ static float _compute_next_segment_velocity() { } -/*** Replan current move to execute hold - * - * Holds are initiated by the planner entering STATE_STOPPING. In which case - * _plan_hold() is called to replan the current move towards zero. If it is - * unable to plan to zero in the remaining length of the current move it will - * decelerate as much as possible and then wait for the next move. Once it - * is possible to plan to zero velocity in the current move the remaining length - * is put into the run buffer, which is still allocated, and the run buffer - * becomes the hold point. The hold is left by a start request in state.c. At - * this point the remaining buffers, if any, are replanned from zero up to - * speed. - */ +/// Replan current move to execute hold +/// +/// Holds are initiated by the planner entering STATE_STOPPING. In which case +/// _plan_hold() is called to replan the current move towards zero. If it is +/// unable to plan to zero in the remaining length of the current move it will +/// decelerate as much as possible and then wait for the next move. Once it is +/// possible to plan to zero velocity in the current move the remaining length +/// is put into the run buffer, which is still allocated, and the run buffer +/// becomes the hold point. The hold is left by a start request in state.c. At +/// this point the remaining buffers, if any, are replanned from zero up to +/// speed. static void _plan_hold() { mp_buffer_t *bf = mp_queue_get_head(); // working buffer pointer if (!bf) return; // Oops! nothing's running @@ -440,58 +436,57 @@ static stat_t _exec_aline_init(mp_buffer_t *bf) { } -/* Aline execution routines - * - * 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 stepper operation - do not load the move - * STAT_xxxxx fatal error. Ends the move and frees the bf buffer - * - * This routine is called from the (LO) interrupt level. The interrupt - * sequencing relies on the correct behavior of these routines. - * Each call to _exec_aline() must execute and prep *one and only one* - * 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 - * returns a valid error code. - * - * Notes: - * - * [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. - * - * 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 - * 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 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 - * - * move_time is the actual time of the move, accel_time is the time value - * needed to compute the velocity taking the initial velocity into account. - * move_time does not need to. - */ +/// Aline execution routines +/// +/// 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 stepper operation - do not load the move +/// STAT_xxxxx fatal error. Ends the move and frees the bf buffer +/// +/// This routine is called from the (LO) interrupt level. The interrupt +/// sequencing relies on the correct behavior of these routines. +/// Each call to _exec_aline() must execute and prep *one and only one* +/// 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 +/// returns a valid error code. +/// +/// Notes: +/// +/// [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. +/// +/// 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 +/// 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 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 +/// +/// move_time is the actual time of the move, accel_time is the time value +/// needed to compute the velocity taking the initial velocity into account. +/// move_time does not need to. stat_t mp_exec_aline(mp_buffer_t *bf) { stat_t status = STAT_OK; @@ -519,20 +514,26 @@ stat_t mp_exec_aline(mp_buffer_t *bf) { } -/// Dequeues buffer and executes move callback +/// Dequeues buffers, initializes them, executes their callbacks and cleans up. +/// +/// This is the guts of the planner runtime execution. Because this routine is +/// run in an interrupt the state changes must be carefully ordered. stat_t mp_exec_move() { + // Check if we can run a buffer mp_buffer_t *bf = mp_queue_get_head(); if (mp_get_state() == STATE_ESTOPPED || mp_get_state() == STATE_HOLDING || !bf) { mp_runtime_set_velocity(0); mp_runtime_set_busy(false); + return STAT_NOOP; // Nothing running } + // Process new buffers if (bf->state == BUFFER_NEW) { // On restart wait a bit to give planner queue a chance to fill - if (!mp_runtime_is_busy() && mp_queue_get_fill() < 4 && - !rtc_expired(bf->ts + 250)) return STAT_NOOP; + if (!mp_runtime_is_busy() && mp_queue_get_fill() < PLANNER_EXEC_MIN_FILL && + !rtc_expired(bf->ts + PLANNER_EXEC_DELAY)) return STAT_NOOP; // Take control of buffer bf->state = BUFFER_INIT; @@ -542,38 +543,44 @@ stat_t mp_exec_move() { mp_runtime_set_line(bf->line); } - stat_t status = bf->cb(bf); // Move callback + // Execute the buffer + stat_t status = bf->cb(bf); - // Busy only if a move was queued + // Signal that we are busy only if a move was queued. This means that + // nonstop buffers, i.e. non-plan-to-zero commands, will not cause the + // runtime to enter the busy state. This causes mp_exec_move() to continue + // to wait above for the planner buffer to fill when a new stream starts + // with some nonstop buffers. If this weren't so, the code below + // which marks the next buffer not replannable would lock the first move + // buffer and cause it to be unnecessarily planned to zero. if (status == STAT_EAGAIN || status == STAT_OK) mp_runtime_set_busy(true); + // Process finished buffers if (status != STAT_EAGAIN) { - // Enter HOLDING state - if (mp_get_state() == STATE_STOPPING && - fp_ZERO(mp_runtime_get_velocity())) { - mp_state_holding(); - } + // Signal that we've encountered a stopping point + if (fp_ZERO(mp_runtime_get_velocity()) && + (mp_get_state() == STATE_STOPPING || bf->hold)) mp_state_holding(); - // Handle buffer run state + // Handle buffer restarts and deallocation if (bf->state == BUFFER_RESTART) bf->state = BUFFER_NEW; else { - // Solves a potential race condition where the current move ends but - // the new move has not started because the current move is still - // being run by the steppers. Planning can overwrite the new move. + // Solves a potential race condition where the current buffer ends but + // the new buffer has not started because the current one is still + // being run by the steppers. Planning can overwrite the new buffer. + // See notes above. mp_buffer_next(bf)->replannable = false; mp_queue_pop(); // Release buffer // Enter READY state if (mp_queue_is_empty()) mp_state_idle(); - - mp_set_cycle(CYCLE_MACHINING); // Default cycle } } + // Convert return status for stepper.c switch (status) { case STAT_NOOP: return STAT_EAGAIN; // Tell caller to call again - case STAT_EAGAIN: return STAT_OK; // Move queued, call again later + case STAT_EAGAIN: return STAT_OK; // A move was queued, call again later default: return status; } } diff --git a/src/plan/jog.c b/src/plan/jog.c index 4463301..d78f78a 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -92,6 +92,8 @@ static stat_t _exec_jog(mp_buffer_t *bf) { for (int axis = 0; axis < AXES; axis++) mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); + mp_set_cycle(CYCLE_MACHINING); // Default cycle + return STAT_NOOP; // Done, no move executed } diff --git a/src/plan/line.c b/src/plan/line.c index 053d90c..a236e9f 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -309,7 +309,8 @@ stat_t mp_aline(const float target[], int32_t line) { _calc_max_velocities(bf, time); // Note, the following lines must remain in order. - mp_plan_block_list(bf); // Plan block list + bf->line = line; // Planner needs then when planning steps + mp_plan(bf); // Plan block list mp_set_position(target); // Set planner position before committing buffer mp_queue_push(mp_exec_aline, line); // After position update diff --git a/src/plan/planner.c b/src/plan/planner.c index aebc97a..5552e23 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -71,7 +71,13 @@ #include -static float mp_position[AXES]; // final move position for planning purposes +typedef struct { + float position[AXES]; // final move position for planning purposes + bool plan_steps; // if true plan one GCode line at a time +} planner_t; + + +static planner_t mp = {{0}}; void mp_init() {mp_queue_init();} @@ -79,18 +85,21 @@ void mp_init() {mp_queue_init();} /// Set planner position for a single axis void mp_set_axis_position(int axis, float position) { - mp_position[axis] = position; + mp.position[axis] = position; } -float mp_get_axis_position(int axis) {return mp_position[axis];} +float mp_get_axis_position(int axis) {return mp.position[axis];} void mp_set_position(const float position[]) { - copy_vector(mp_position, position); + copy_vector(mp.position, position); } +void mp_set_plan_steps(bool plan_steps) {mp.plan_steps = plan_steps;} + + /*** Flush all moves in the planner * * Does not affect the move currently running. Does not affect @@ -135,16 +144,17 @@ void mp_kinematics(const float travel[], float steps[]) { #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 - * errs on the side of too slow. +/*** Calculate move acceleration / deceleration * - * 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. + * 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 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: * @@ -171,29 +181,25 @@ void mp_kinematics(const float travel[], float steps[]) { * * 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 profile in the given length. - * - * Rate-Limited-Fit - The move does not have sufficient length to - * achieve target velocity. In this case the cruise velocity - * will be set lower than the requested velocity (incoming - * bf->cruise_velocity). The entry and exit velocities are - * satisfied. - * - * Degraded-Fit - The move does not have sufficient length to - * transition from the entry velocity to the exit velocity in - * the available length. These velocities are not negotiable, - * so a degraded solution is found. - * - * In worst cases, the move cannot be executed as the required - * execution time is less than the minimum segment time. The - * first degradation is to reduce the move to a body-only - * segment with an average velocity. If that still doesn't fit - * then the move velocity is reduced so it fits into a minimum - * segment. This will reduce the velocities in that region of - * the planner buffer as the moves are replanned to that - * worst-case move. + * Requested-Fit - The move has sufficient length to achieve the target + * velocity (cruise velocity). I.e it will accommodate the acceleration / + * deceleration profile in the given length. + * + * Rate-Limited-Fit - The move does not have sufficient length to achieve + * target velocity. In this case the cruise velocity will be set lower than + * the requested velocity (incoming bf->cruise_velocity). The entry and + * exit velocities are satisfied. + * + * Degraded-Fit - The move does not have sufficient length to transition from + * the entry velocity to the exit velocity in the available length. These + * velocities are not negotiable, so a degraded solution is found. + * + * In worst cases, the move cannot be executed as the required execution + * time is less than the minimum segment time. The first degradation is to + * reduce the move to a body-only segment with an average velocity. If that + * still doesn't fit then the move velocity is reduced so it fits into a + * minimum segment. This will reduce the velocities in that region of the + * planner buffer as the moves are replanned to that worst-case move. * * Various cases handled (H=head, B=body, T=tail) * @@ -440,19 +446,22 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { } +#if 0 +/// Prints the entire planning queue as comma separated values embedded in +/// JSON ``msg`` entries. Used for debugging. void mp_print_queue(mp_buffer_t *bf) { - printf_P(PSTR("{\"msg\":\",id,replannable,callback," + printf_P(PSTR("{\"msg\":\"id,replannable,callback," "length,head_length,body_length,tail_length," "entry_velocity,cruise_velocity,exit_velocity,braking_velocity," - "entry_vmax,cruise_vmax,exit_vmax,\"}\n")); + "entry_vmax,cruise_vmax,exit_vmax\"}\n")); int i = 0; mp_buffer_t *bp = bf; while (bp) { - printf_P(PSTR("{\"msg\":\",%d,%d,0x%04x," + printf_P(PSTR("{\"msg\":\"%d,%d,0x%04x," "%0.2f,%0.2f,%0.2f,%0.2f," "%0.2f,%0.2f,%0.2f,%0.2f," - "%0.2f,%0.2f,%0.2f,\"}\n"), + "%0.2f,%0.2f,%0.2f\"}\n"), i++, bp->replannable, bp->cb, bp->length, bp->head_length, bp->body_length, bp->tail_length, bp->entry_velocity, bp->cruise_velocity, bp->exit_velocity, @@ -465,12 +474,13 @@ void mp_print_queue(mp_buffer_t *bf) { while (!usart_tx_empty()) continue; } +#endif -/*** Plans the entire block list +/*** Plans the entire queue * - * 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 + * The block list is the circular buffer of planner buffers (bl's). The block + * currently being planned is the "bl" 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 or there is only one block and it is already running. @@ -479,83 +489,91 @@ void mp_print_queue(mp_buffer_t *bf) { * replannable) the first block that is not optimally planned becomes the * effective first block. * - * mp_plan_block_list() plans all blocks between and including the (effective) - * first block and the bf. It sets entry, exit and cruise v's from vmax's then + * mp_plan() plans all blocks between and including the (effective) + * first block and the bl. It sets entry, exit and cruise v's from vmax's then * calls trapezoid generation. * * Variables that must be provided in the mp_buffer_t 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 + * bl (function arg) - end of block list (last block in time) + * bl->replannable - start of block list set by last FALSE value * [Note 1] - * bf->move_type - typically MOVE_TYPE_ALINE. Other move_types should + * bl->move_type - typically MOVE_TYPE_ALINE. Other move_types should * 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 + * bl->length - provides block length + * bl->entry_vmax - used during forward planning to set entry velocity + * bl->cruise_vmax - used during forward planning to set cruise velocity + * bl->exit_vmax - used during forward planning to set exit velocity + * bl->delta_vmax - used during forward planning to set exit velocity + * bl->recip_jerk - used during trapezoid generation + * bl->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 + * bl->replannable - set if the block becomes optimally planned + * bl->braking_velocity - set during backward planning + * bl->entry_velocity - set during forward planning + * bl->cruise_velocity - set during forward planning + * bl->exit_velocity - set during forward planning + * bl->head_length - set during trapezoid generation + * bl->body_length - set during trapezoid generation + * bl->tail_length - set during trapezoid generation * * Variables that are ignored but here's what you would expect them to be: * - * bf->state - BUFFER_NEW for all blocks but the earliest - * bf->target[] - block target position - * bf->unit[] - block unit vector - * bf->jerk - source of the other jerk variables. + * bl->state - BUFFER_NEW for all blocks but the earliest + * bl->target[] - block target position + * bl->unit[] - block unit vector + * bl->jerk - source of the other jerk variables. * * Notes: * - * [1] Whether or not a block is planned is controlled by the bf->replannable + * [1] Whether or not a block is planned is controlled by the bl->replannable * setting. Replan flags are checked during the backwards pass. They prune * the replan list to include only 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 (ex) buffer and re-shuffle - * the block list, re-enlisting the current bf buffer with new parameters. + * the block list, re-enlisting the current bl 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. */ -void mp_plan_block_list(mp_buffer_t *bf) { - mp_buffer_t *bp = bf; +void mp_plan(mp_buffer_t *bl) { + mp_buffer_t *bp = bl; // Backward planning pass. Find first block and update braking velocities. // By the end bp points to the buffer before the first block. mp_buffer_t *next = bp; - while ((bp = mp_buffer_prev(bp)) != bf) { + while ((bp = mp_buffer_prev(bp)) != bl) { if (!bp->replannable) break; + bp->braking_velocity = min(next->entry_vmax, next->braking_velocity) + bp->delta_vmax; + next = bp; } - // Forward planning pass. Recompute trapezoids from the first block to bf. + // Forward planning pass. Recompute trapezoids from the first block to bl. mp_buffer_t *prev = bp; - while ((bp = mp_buffer_next(bp)) != bf) { + while ((bp = mp_buffer_next(bp)) != bl) { mp_buffer_t *next = mp_buffer_next(bp); - bp->entry_velocity = prev == bf ? bp->entry_vmax : prev->exit_velocity; + bp->entry_velocity = prev == bl ? bp->entry_vmax : prev->exit_velocity; bp->cruise_velocity = bp->cruise_vmax; bp->exit_velocity = min4(bp->exit_vmax, next->entry_vmax, next->braking_velocity, bp->entry_velocity + bp->delta_vmax); + if (mp.plan_steps && bp->line != next->line) { + bp->exit_velocity = 0; + bp->hold = true; + + } else bp->hold = false; + mp_calculate_trapezoid(bp); // Test for optimally planned trapezoids by checking exit conditions @@ -569,17 +587,18 @@ void mp_plan_block_list(mp_buffer_t *bf) { } // Finish last block - bf->entry_velocity = prev->exit_velocity; - bf->cruise_velocity = bf->cruise_vmax; - bf->exit_velocity = 0; + bl->entry_velocity = prev->exit_velocity; + bl->cruise_velocity = bl->cruise_vmax; + bl->exit_velocity = 0; - mp_calculate_trapezoid(bf); - - //mp_print_queue(bf); + mp_calculate_trapezoid(bl); } -void mp_replan_blocks() { +void mp_replan_all() { + ASSERT(mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING); + + // Get next buffer mp_buffer_t *bf = mp_queue_get_head(); if (!bf) return; @@ -589,12 +608,12 @@ void mp_replan_blocks() { while (true) { bp->replannable = true; mp_buffer_t *next = mp_buffer_next(bp); - if (next->state == BUFFER_OFF || next == bf) break; + if (next->state == BUFFER_OFF || next == bf) break; // Avoid wrap around bp = next; } // Plan blocks - mp_plan_block_list(bp); + mp_plan(bp); } diff --git a/src/plan/planner.h b/src/plan/planner.h index 9c8262a..f657829 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -79,13 +79,19 @@ typedef enum { void mp_init(); + void mp_set_axis_position(int axis, float position); float mp_get_axis_position(int axis); void mp_set_position(const float position[]); +void mp_set_plan_steps(bool plan_steps); + void mp_flush_planner(); void mp_kinematics(const float travel[], float steps[]); -void mp_plan_block_list(mp_buffer_t *bf); -void mp_replan_blocks(); + +void mp_plan(mp_buffer_t *bf); +void mp_replan_all(); + void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line); + float mp_get_target_length(float Vi, float Vf, const mp_buffer_t *bf); float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf); diff --git a/src/plan/runtime.c b/src/plan/runtime.c index bc95b3e..5040173 100644 --- a/src/plan/runtime.c +++ b/src/plan/runtime.c @@ -75,7 +75,7 @@ typedef struct { distance_mode_t arc_distance_mode; } mp_runtime_t; -static mp_runtime_t rt; +static mp_runtime_t rt = {0}; bool mp_runtime_is_busy() {return rt.busy;} diff --git a/src/plan/state.c b/src/plan/state.c index 97e7419..b53ef6f 100644 --- a/src/plan/state.c +++ b/src/plan/state.c @@ -54,7 +54,7 @@ typedef struct { static planner_state_t ps = { - .flush_requested = true // Start out flushing + .flush_requested = true, // Start out flushing }; @@ -104,6 +104,7 @@ mp_cycle_t mp_get_cycle() {return ps.cycle;} static void _set_state(mp_state_t state) { if (ps.state == state) return; // No change if (ps.state == STATE_ESTOPPED) return; // Can't leave EStop state + if (state == STATE_READY) mp_runtime_set_line(0); ps.state = state; report_request(); } @@ -160,7 +161,10 @@ void mp_state_optional_pause() { } -void mp_state_holding() {_set_state(STATE_HOLDING);} +void mp_state_holding() { + _set_state(STATE_HOLDING); + mp_set_plan_steps(false); +} void mp_state_running() { @@ -183,6 +187,12 @@ void mp_request_resume() {if (ps.flush_requested) ps.resume_requested = true;} void mp_request_optional_pause() {ps.optional_pause_requested = true;} +void mp_request_step() { + mp_set_plan_steps(true); + ps.start_requested = true; +} + + /*** Feedholds, queue flushes and starts are all related. Request functions * set flags. The callback interprets the flags according to these rules: * @@ -242,7 +252,8 @@ void mp_state_callback() { if (mp_get_state() == STATE_HOLDING) { // Check if any moves are buffered if (!mp_queue_is_empty()) { - mp_replan_blocks(); + // Always replan when coming out of a hold + mp_replan_all(); _set_state(STATE_RUNNING); } else _set_state(STATE_READY); diff --git a/src/plan/state.h b/src/plan/state.h index bc129d4..a6a62aa 100644 --- a/src/plan/state.h +++ b/src/plan/state.h @@ -88,5 +88,6 @@ void mp_request_start(); void mp_request_flush(); void mp_request_resume(); void mp_request_optional_pause(); +void mp_request_step(); void mp_state_callback(); diff --git a/src/probing.c b/src/probing.c index e37bbee..75a2135 100644 --- a/src/probing.c +++ b/src/probing.c @@ -100,6 +100,8 @@ static void _probe_restore_settings() { // update the model with actual position mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); + + mp_set_cycle(CYCLE_MACHINING); // Default cycle } -- 2.27.0