--- /dev/null
+# Notes on the lifecycle of a movement
+
+## Parsing
+A move first starts off as a line of GCode which is parsed by
+``gc_gcode_parser()`` which in turn calls ``_normalize_gcode_block()``
+and ``_parse_gcode_block()``. ``_parse_gcode_block()`` sets flags in
+``mach.gf`` indicating which values have changed and the changed values in
+``mach.gn``. ``_parse_gcode_block()`` then calls ``_execute_gcode_block()``
+which calls ``mach_*()`` functions which modify the state of the GCode machine.
+
+## Queuing
+Some functions such as ``mach_straight_traverse()``, ``mach_straight_feed()``
+and ``mach_arc_feed()`` result in line moves being entered into the planner
+queue. Others enter dwells or commands or into the queue. Planner buffer
+entries encode everything needed to execute a move with out referring back to
+the machine model.
+
+Line moves are entered into the queue by calls to ``mp_aline()``. Arcs are
+converted to short straight line moves and are feed in as buffer room becomes
+available, until complete, via calls to ``mach_arc_callback()`` which results in
+multiple calls to ``mp_aline()``.
+
+``mp_aline()`` plans straight line movements by first calling
+``mach_calc_move_time()`` which uses the current GCode state to estimate the
+total time required to complete the move at the current feed rate and feed rate
+mode. If the move time is long enough, a buffer is allocated. Its jerk, max
+cruise velocity, max entry velocity, max delta velocity, max exit velocity and
+breaking velocity are all computed. The move velocities are planned by a
+call to ``mp_plan_block_list()``. Initially ``bf_func`` is set to
+``mp_exec_aline()`` and the buffer is committed to the queue by calling
+``mp_commit_write_buffer()``.
+
+## Planning
+### Backward planning pass
+``mp_plan_block_list()`` plans movements by first moving backwards through the
+planning buffer until either the last entry is reached or a buffer marked not
+``replannable`` is encountered. The ``breaking_velocity`` is propagated back
+during the backwards pass. Next, begins the forward planning pass.
+
+### Forward planning pass
+During the forward pass the entry velocity, cruise velocity and exit velocity
+are computed and ``mp_calculate_trapezoid()`` is called to (re)compute the
+velocity trapezoids of each buffer being considered. If a buffer's plan is
+deemed optimal then it is marked not ``replannable`` to avoid replanning later.
+
+### Trapezoid planning
+The call to ``mp_calculate_trapezoid()`` computes head, body and tail lengths
+for a single planner buffer. Entry, cruse and exit velocities may be modified
+to make the trapezoid fit with in the move length. Planning may result in a
+degraded trapezoid. I.e. one with out all three sides.
+
+## Execution
+The stepper motor driver interrupt routine calls ``mp_exec_move()`` to prepare
+the next move for execution. ``mp_exec_move()`` access the next buffer in the
+planner queue and calls the function pointed to by ``bf_func`` which is
+initially set to ``mp_exec_aline()`` during planning. Each call to
+``mp_exec_move()`` must prepare one and only one move fragment for the stepper
+driver. The planner buffer is executed repeatedly as long as ``bf_func``
+returns ``STAT_EAGAIN``.
+
+### Move initialization
+On the first call to ``mp_exec_aline()`` a call is made to
+``_exec_aline_init()``. This function may stop processing the move if a
+feedhold is in effect. It may also skip a move if it has zero length.
+Otherwise, it initializes the move runtime state (``mr``) by copying the move
+state from the planner buffer and initializing variables. In addition, it
+computes waypoints at the ends of each trapezoid section. Waypoints are used
+later for position correction which adjust position for rounding errors.
+
+### Move execution
+After move initialization ``mp_exec_aline()`` calls ``_exec_aline_head()``,
+``_exec_aline_body()`` and ``exec_aline_tail()`` on successive callbacks. Each
+of these functions are called repeatedly until the section finishes. They
+advance through states ``SECTION_NEW`` which initializes the section,
+``SECTION_1st_HALF`` which executes the first half of the S-curve and
+``SECTION_2nd_HALF``. If any sections have zero length they are skipped and
+execution is passed immediately to the next section. During each section
+forward differencing is used to map the trapezoid computed during the planning
+stage to a fifth-degree Bezier polynomial S-curve. The curve is used to find
+the next target position.
+
+``_exec_aline_segment()`` is called for each non-zero section to convert the
+computed target position to target steps by calling ``mp_kinematics()``. The
+move fragment is then passed to the stepper driver by a call to
+``st_prep_line()``. When a segment is complete ``_exec_aline_segment()``
+returns ``STAT_OK`` indicating the next segment should be loaded. When all
+non-zero segments have been executed, the move is complete.
+
+## Stepper driver
+Calls to ``st_prep_line()`` prepare short (~5ms) moves for execution by the
+stepper driver. The move time in clock ticks is computed from travel in steps
+and the move duration. Then ``motor_prep_move()`` is called for each motor.
+``motor_prep_move()`` may perform step correction, enables the motors if needed.
+It then computes the optimal step clock divisor, clock ticks and sets the move
+direction, taking the motor's configuration in to account.
+
+The stepper timer ISR, after ending the previous move, calls
+``motor_load_move()`` on each motor. This sets up and starts the motor clocks,
+sets the motor direction lines and accumulates and resets the step encoders.
+After (re)starting the motor clocks the ISR signals a lower level interrupt to
+call ``mp_exec_move()`` and load the next move fragment.
// Set target position for the segment
// If the segment ends on a section waypoint, synchronize to the
// head, body or tail end. Otherwise, if not at a section waypoint
- // compute target from segment time and velocity Don't do waypoint
+ // compute target from segment time and velocity. Don't do waypoint
// correction if you are going into a hold.
if (--mr.segment_count == 0 && mr.section_state == SECTION_2nd_HALF &&
mp_get_state() == STATE_RUNNING)
// now determine the target steps
mp_kinematics(mr.ms.target, mr.target_steps);
- // and compute the distances to be traveled
+ // and compute the distances, in steps, to be traveled
for (int i = 0; i < MOTORS; i++)
travel_steps[i] = mr.target_steps[i] - mr.position_steps[i];
*
* 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)
+ * 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
if (mr.section_state == SECTION_1st_HALF) {
if (_exec_aline_segment() == STAT_OK) {
// For forward differencing we should have one segment in
- // SECTION_1st_HALF. However, if it returns from that as STAT_OK, then
- // there was only one segment in this section. Show that we did complete
+ // SECTION_1st_HALF. However, if it returns from that as STAT_OK, then
+ // there was only one segment in this section. Show that we did complete
// section 2 ... effectively.
mr.section_state = SECTION_2nd_HALF;
return STAT_OK;
/// Helper for cruise section
-/// The body is broken into little segments even though it is a
-/// straight line so that feedholds can happen in the middle of a line
-/// with a minimum of latency
static stat_t _exec_aline_body() {
if (mr.section_state == SECTION_NEW) {
if (fp_ZERO(mr.body_length)) {
mr.section = SECTION_BODY;
- // uses PERIOD_2 so last segment detection works
+ // use SECTION_2nd_HALF so last segment detection works
mr.section_state = SECTION_2nd_HALF;
}
return STAT_EAGAIN;
}
+
#else // __JERK_EXEC
/// Helper for acceleration section
static stat_t _exec_aline_head() {
if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr)
if (fp_ZERO(mr.head_length)) {
mr.section = SECTION_BODY;
+
return _exec_aline_body(); // skip ahead to the body generator
}
}
// For forward differencing we should have one segment in
- // SECTION_1st_HALF However, if it returns from that as STAT_OK,
+ // SECTION_1st_HALF. However, if it returns from that as STAT_OK,
// then there was only one segment in this section.
// First half (concave part of accel curve)
if (mr.section_state == SECTION_1st_HALF) {
#endif // !__JERK_EXEC
+/// Initializes a new planner buffer
+static stat_t _exec_aline_init(mpBuf_t *bf) {
+ // Stop here if holding
+ if (mp_get_hold_state() == FEEDHOLD_HOLD) return STAT_NOOP;
+
+ // copy in the gcode model state
+ memcpy(&mr.ms, &bf->ms, sizeof(MoveState_t));
+ bf->replannable = false;
+ report_request(); // Executing line number has changed
+
+ // Remove zero length lines. Short lines have already been removed.
+ if (fp_ZERO(bf->length)) {
+ mr.move_state = MOVE_OFF; // reset mr buffer
+ mr.section_state = SECTION_OFF;
+ bf->nx->replannable = false; // prevent overplanning (Note 2)
+ mp_free_run_buffer(); // free buffer
+
+ return STAT_NOOP;
+ }
+
+ // Initialize the move runtime
+ bf->move_state = MOVE_RUN;
+ mr.move_state = MOVE_RUN;
+ mr.section = SECTION_HEAD;
+ mr.section_state = SECTION_NEW;
+ mr.jerk = bf->jerk;
+#ifdef __JERK_EXEC
+ mr.jerk_div2 = bf->jerk / 2; // only needed by __JERK_EXEC
+#endif
+ mr.head_length = bf->head_length;
+ mr.body_length = bf->body_length;
+ mr.tail_length = bf->tail_length;
+ mr.entry_velocity = bf->entry_velocity;
+ mr.cruise_velocity = bf->cruise_velocity;
+ mr.exit_velocity = bf->exit_velocity;
+
+ copy_vector(mr.unit, bf->unit);
+ copy_vector(mr.final_target, bf->ms.target); // save move final target
+
+ // Generate waypoints for position correction at section ends
+ for (int axis = 0; axis < AXES; axis++) {
+ mr.waypoint[SECTION_HEAD][axis] =
+ mr.position[axis] + mr.unit[axis] * mr.head_length;
+
+ mr.waypoint[SECTION_BODY][axis] =
+ mr.position[axis] + mr.unit[axis] * (mr.head_length + mr.body_length);
+
+ mr.waypoint[SECTION_TAIL][axis] =
+ mr.position[axis] + mr.unit[axis] *
+ (mr.head_length + mr.body_length + mr.tail_length);
+ }
+
+ return STAT_OK;
+}
+
+
/* Aline execution routines
*
* Everything here fires from interrupts and must be interrupt safe
*
* 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
- * move
- * STAT_xxxxx fatal error. Ends the move and frees the bf buffer
+ * 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 behaviors of the routines being exactly correct.
+ * 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
+ * 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.
*
* [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.
+ * being run by the steppers. Planning can overwrite the new move.
*
* Operation:
*
* 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
- * the move, accel_time is the time valaue needed to compute the velocity -
- * which takes the initial velocity into account (move_time does not need
- * to).
+ * to make sure this actually work out. 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).
*
* 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
+ * from _RUN to _OFF on final call or just remain _OFF
*
* mr.move_state transitions on first call from _OFF to one of _HEAD, _BODY,
* _TAIL. Within each section state may be:
stat_t mp_exec_aline(mpBuf_t *bf) {
if (bf->move_state == MOVE_OFF) return STAT_NOOP;
- // start a new move by setting up local context (singleton)
- if (mr.move_state == MOVE_OFF) {
- // stop here if holding
- if (mp_get_hold_state() == FEEDHOLD_HOLD) return STAT_NOOP;
-
- // initialization to process the new incoming bf buffer (Gcode block)
- // copy in the gcode model state
- memcpy(&mr.ms, &bf->ms, sizeof(MoveState_t));
- bf->replannable = false;
- report_request(); // Executing line number has changed
-
- // short lines have already been removed, look for an actual zero
- if (fp_ZERO(bf->length)) {
- mr.move_state = MOVE_OFF; // reset mr buffer
- mr.section_state = SECTION_OFF;
- // prevent overplanning (Note 2)
- bf->nx->replannable = false;
- // free buffer & end cycle if planner is empty
- mp_free_run_buffer();
-
- return STAT_NOOP;
- }
-
- bf->move_state = MOVE_RUN;
- mr.move_state = MOVE_RUN;
- mr.section = SECTION_HEAD;
- mr.section_state = SECTION_NEW;
- mr.jerk = bf->jerk;
-#ifdef __JERK_EXEC
- mr.jerk_div2 = bf->jerk / 2; // only needed by __JERK_EXEC
-#endif
- mr.head_length = bf->head_length;
- mr.body_length = bf->body_length;
- mr.tail_length = bf->tail_length;
-
- mr.entry_velocity = bf->entry_velocity;
- mr.cruise_velocity = bf->cruise_velocity;
- mr.exit_velocity = bf->exit_velocity;
-
- copy_vector(mr.unit, bf->unit);
- copy_vector(mr.final_target, bf->ms.target); // save move final target
-
- // generate the waypoints for position correction at section ends
- for (int axis = 0; axis < AXES; axis++) {
- mr.waypoint[SECTION_HEAD][axis] =
- mr.position[axis] + mr.unit[axis] * mr.head_length;
-
- mr.waypoint[SECTION_BODY][axis] =
- mr.position[axis] + mr.unit[axis] *
- (mr.head_length + mr.body_length);
+ stat_t status = STAT_OK;
- mr.waypoint[SECTION_TAIL][axis] =
- mr.position[axis] + mr.unit[axis] *
- (mr.head_length + mr.body_length + mr.tail_length);
- }
+ // Start a new move
+ if (mr.move_state == MOVE_OFF) {
+ status = _exec_aline_init(bf);
+ if (status != STAT_OK) return status;
}
// 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();
- else if (mr.section == SECTION_BODY) status = _exec_aline_body();
- else if (mr.section == SECTION_TAIL) status = _exec_aline_tail();
- else return CM_ALARM(STAT_INTERNAL_ERROR); // never supposed to get here
+ switch (mr.section) {
+ case SECTION_HEAD: status = _exec_aline_head(); break;
+ case SECTION_BODY: status = _exec_aline_body(); break;
+ case SECTION_TAIL: status = _exec_aline_tail(); break;
+ default: return CM_ALARM(STAT_INTERNAL_ERROR); // never supposed to get here
+ }
mp_state_hold_callback(status == STAT_OK);
/*** 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.
+ * 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
* bf->cruise_velocity - requested Vt, is often changed
* bf->exit_velocity - requested Vx, may change for degenerate cases
* bf->cruise_vmax - used in some comparisons
- * bf->delta_vmax - used to degrade velocity of pathologically
- * short blocks
+ * bf->delta_vmax - used to degrade velocity of short blocks
*
* Variables that may be set/updated are:
*
* Classes of moves:
*
* Requested-Fit - The move has sufficient length to achieve the
- * target velocity (cruise velocity). I.e: it will accommodate
+ * 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
+ * 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
+ * bf->cruise_velocity). The entry and exit velocities are
* satisfied.
*
* Degraded-Fit - The move does not have sufficient length to
* 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
+ * 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
+ * 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
* F <too short> force fit: This block is slowed down until it can
* be executed
*
- * Note: The order of the cases/tests in the code is important. Start with
+ * 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.
}
// B case: Velocities all match (or close enough)
- // This occurs frequently in normal gcode files with lots of short lines
- // This case is not really necessary, but saves lots of processing time
-
+ // This occurs frequently in normal gcode files with lots of short lines.
+ // This case is not really necessary, but saves lots of processing time.
if (((bf->cruise_velocity - bf->entry_velocity) <
TRAPEZOID_VELOCITY_TOLERANCE) &&
((bf->cruise_velocity - bf->exit_velocity) <
// H" and T" degraded-fit cases
// H' and T' requested-fit cases where the body residual is less than
// MIN_BODY_LENGTH
-
bf->body_length = 0;
float minimum_length =
mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf);
projects / bbctrl-firmware / commitdiff