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
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;}
// 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
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
}
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);
}
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
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
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;
}
-/*** 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;
}
-/*** 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
}
-/* 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;
}
-/// 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;
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;
}
}
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
}
_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
#include <stdio.h>
-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();}
/// 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
#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:
*
*
* 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)
*
}
+#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,
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.
* 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
}
// 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;
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);
}
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);
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;}
static planner_state_t ps = {
- .flush_requested = true // Start out flushing
+ .flush_requested = true, // Start out flushing
};
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();
}
}
-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() {
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:
*
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);
void mp_request_flush();
void mp_request_resume();
void mp_request_optional_pause();
+void mp_request_step();
void mp_state_callback();
// update the model with actual position
mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
+
+ mp_set_cycle(CYCLE_MACHINING); // Default cycle
}
projects / bbctrl-firmware / commitdiff