--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ All rights reserved.
+
+ This file ("the software") is free software: you can redistribute it
+ and/or modify it under the terms of the GNU General Public License,
+ version 2 as published by the Free Software Foundation. You should
+ have received a copy of the GNU General Public License, version 2
+ along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "planner.h"
+#include "buffer.h"
+#include "util.h"
+#include "runtime.h"
+#include "state.h"
+#include "config.h"
+
+#include <string.h>
+#include <stdbool.h>
+#include <math.h>
+#include <stdio.h>
+
+
+typedef struct {
+ float unit[AXES]; // unit vector for axis scaling & planning
+ float final_target[AXES]; // final target, used to correct rounding errors
+ float waypoint[3][AXES]; // head/body/tail endpoints for correction
+
+ // copies of bf variables of same name
+ float head_length;
+ float body_length;
+ float tail_length;
+ float entry_velocity;
+ 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
+ moveSection_t section; // what section is the move in?
+ bool section_new; // true if it's a new section
+} mp_exec_t;
+
+
+static mp_exec_t ex = {{0}};
+
+
+static stat_t _exec_aline_segment() {
+ float target[AXES];
+
+ // 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 correction if you are going into a hold.
+ if (!--ex.segment_count && !ex.section_new && mp_get_state() == STATE_RUNNING)
+ copy_vector(target, ex.waypoint[ex.section]);
+
+ else {
+ float segment_length = ex.segment_velocity * ex.segment_time;
+
+ for (int i = 0; i < AXES; i++)
+ target[i] = mp_runtime_get_position(i) + ex.unit[i] * segment_length;
+ }
+
+ RITORNO(mp_runtime_move_to_target
+ (target, ex.segment_velocity, ex.segment_time));
+
+ // Return EAGAIN to continue or OK if this segment is done
+ return ex.segment_count ? STAT_EAGAIN : STAT_OK;
+}
+
+
+/*** 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.
+ */
+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;
+ float C = -10.0 * Vi + 10.0 * Vt;
+ // D = 0
+ // E = 0
+ // F = Vi
+
+ float h = 1 / segments;
+
+ float Ah_5 = A * h * h * h * h * h;
+ float Bh_4 = B * h * h * h * h;
+ float Ch_3 = C * h * h * h;
+
+ ex.forward_diff[4] = 121.0 / 16.0 * Ah_5 + 5.0 * Bh_4 + 13.0 / 4.0 * Ch_3;
+ ex.forward_diff[3] = 165.0 / 2.0 * Ah_5 + 29.0 * Bh_4 + 9.0 * Ch_3;
+ ex.forward_diff[2] = 255.0 * Ah_5 + 48.0 * Bh_4 + 6.0 * Ch_3;
+ ex.forward_diff[1] = 300.0 * Ah_5 + 24.0 * Bh_4;
+ ex.forward_diff[0] = 120.0 * Ah_5;
+
+ // Calculate the initial velocity by calculating V(h / 2)
+ float half_h = h / 2.0;
+ float half_Ch_3 = C * half_h * half_h * half_h;
+ float half_Bh_4 = B * half_h * half_h * half_h * half_h;
+ float half_Ah_5 = C * half_h * half_h * half_h * half_h * half_h;
+
+ return half_Ah_5 + half_Bh_4 + half_Ch_3 + Vi;
+}
+
+
+/// Common code for head and tail sections
+static stat_t _exec_aline_section(float length, float vin, float vout) {
+ if (ex.section_new) {
+ if (fp_ZERO(length)) return STAT_OK; // end the move
+
+ // len / avg. velocity
+ float move_time = 2 * length / (vin + vout);
+ ex.segments = ceil(uSec(move_time) / NOM_SEGMENT_USEC);
+ ex.segment_time = move_time / ex.segments;
+ ex.segment_count = (uint32_t)ex.segments;
+
+ if (vin == vout) ex.segment_velocity = vin;
+ else ex.segment_velocity = _init_forward_diffs(vin, vout, ex.segments);
+
+ if (ex.segment_time < MIN_SEGMENT_TIME)
+ return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position
+
+ // First segment
+ if (_exec_aline_segment() == STAT_OK) {
+ ex.section_new = false;
+ return STAT_OK;
+ }
+
+ ex.section_new = false;
+
+ } else {
+ if (vin != vout) ex.segment_velocity += ex.forward_diff[4];
+
+ // Subsequent segments
+ if (_exec_aline_segment() == STAT_OK) return STAT_OK;
+
+ if (vin != vout) {
+ ex.forward_diff[4] += ex.forward_diff[3];
+ ex.forward_diff[3] += ex.forward_diff[2];
+ ex.forward_diff[2] += ex.forward_diff[1];
+ ex.forward_diff[1] += ex.forward_diff[0];
+ }
+ }
+
+ return STAT_EAGAIN;
+}
+
+
+/// Callback for tail section
+static stat_t _exec_aline_tail() {
+ ex.section = SECTION_TAIL;
+ return
+ _exec_aline_section(ex.tail_length, ex.cruise_velocity, ex.exit_velocity);
+}
+
+
+/// Callback for body section
+static stat_t _exec_aline_body() {
+ ex.section = SECTION_BODY;
+
+ stat_t status =
+ _exec_aline_section(ex.body_length, ex.cruise_velocity, ex.cruise_velocity);
+
+ if (status == STAT_OK) {
+ if (ex.section_new) return _exec_aline_tail();
+
+ ex.section = SECTION_TAIL;
+ ex.section_new = true;
+
+ return STAT_EAGAIN;
+ }
+
+ return status;
+}
+
+
+/// Callback for head section
+static stat_t _exec_aline_head() {
+ ex.section = SECTION_HEAD;
+ stat_t status =
+ _exec_aline_section(ex.head_length, ex.entry_velocity, ex.cruise_velocity);
+
+ if (status == STAT_OK) {
+ if (ex.section_new) return _exec_aline_body();
+
+ ex.section = SECTION_BODY;
+ ex.section_new = true;
+
+ return STAT_EAGAIN;
+ }
+
+ return status;
+}
+
+
+static float _compute_next_segment_velocity() {
+ if (ex.section == SECTION_BODY) return ex.segment_velocity;
+ return ex.segment_velocity + ex.forward_diff[4];
+}
+
+
+/*** 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() {
+ mpBuf_t *bp = mp_get_run_buffer(); // working buffer pointer
+ if (!bp) return; // Oops! nothing's running
+
+ // Examine and process current buffer and compute length left for decel
+ float available_length =
+ get_axis_vector_length(ex.final_target, mp_runtime_get_position_vector());
+ // Compute next_segment velocity, velocity left to shed to brake to zero
+ float braking_velocity = _compute_next_segment_velocity();
+ // Distance to brake to zero from braking_velocity, bp is OK to use here
+ float braking_length = mp_get_target_length(braking_velocity, 0, bp);
+
+ // Hack to prevent Case 2 moves for perfect-fit decels. Happens in
+ // homing situations. The real fix: The braking velocity cannot
+ // simply be the ex.segment_velocity as this is the velocity of the
+ // last segment, not the one that's going to be executed next. The
+ // braking_velocity needs to be the velocity of the next segment
+ // that has not yet been computed. In the mean time, this hack will work.
+ if (available_length < braking_length && fp_ZERO(bp->exit_velocity))
+ braking_length = available_length;
+
+ // Replan to decelerate
+ ex.section = SECTION_TAIL;
+ ex.section_new = true;
+ ex.cruise_velocity = braking_velocity;
+ ex.hold_planned = true;
+
+ // Case 1: deceleration fits entirely into the length remaining in buffer
+ if (braking_length <= available_length) {
+ // Set to a tail to perform the deceleration
+ ex.exit_velocity = 0;
+ ex.tail_length = braking_length;
+
+ // Re-use bp+0 to be the hold point and to run the remaining block length
+ bp->length = available_length - braking_length;
+ bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp);
+ bp->entry_vmax = 0; // set bp+0 as hold point
+ bp->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer
+
+ } else { // Case 2: deceleration exceeds length remaining in buffer
+ // Replan to minimum (but non-zero) exit velocity
+ ex.tail_length = available_length;
+ ex.exit_velocity =
+ braking_velocity - mp_get_target_velocity(0, available_length, bp);
+ }
+}
+
+
+/// Initializes move runtime with a new planner buffer
+static stat_t _exec_aline_init(mpBuf_t *bf) {
+ // Remove zero length lines. Short lines have already been removed.
+ if (fp_ZERO(bf->length)) {
+ mp_runtime_set_busy(false);
+ bf->nx->replannable = false; // prevent overplanning (Note 2)
+ mp_free_run_buffer(); // free buffer
+
+ return STAT_NOOP;
+ }
+
+ // Take control of buffer
+ bf->move_state = MOVE_RUN;
+ bf->replannable = false;
+
+ // Initialize move
+ copy_vector(ex.unit, bf->unit);
+ copy_vector(ex.final_target, bf->ms.target);
+
+ ex.head_length = bf->head_length;
+ ex.body_length = bf->body_length;
+ ex.tail_length = bf->tail_length;
+ ex.entry_velocity = bf->entry_velocity;
+ ex.cruise_velocity = bf->cruise_velocity;
+ ex.exit_velocity = bf->exit_velocity;
+
+ ex.section = SECTION_HEAD;
+ ex.section_new = true;
+ ex.hold_planned = false;
+
+ // Update runtime
+ mp_runtime_set_busy(true);
+ mp_runtime_set_line(bf->ms.line);
+ mp_runtime_set_work_offsets(bf->ms.work_offset);
+
+ // Generate waypoints for position correction at section ends. This helps
+ // negate floating point errors in the forward differencing code.
+ for (int axis = 0; axis < AXES; axis++) {
+ ex.waypoint[SECTION_HEAD][axis] =
+ mp_runtime_get_position(axis) + ex.unit[axis] * ex.head_length;
+
+ ex.waypoint[SECTION_BODY][axis] = mp_runtime_get_position(axis) +
+ ex.unit[axis] * (ex.head_length + ex.body_length);
+
+ ex.waypoint[SECTION_TAIL][axis] = ex.final_target[axis];
+ }
+
+ return STAT_OK;
+}
+
+
+/* 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.
+ *
+ * [2] Solves a potential race condition where the current move ends but
+ * the new move has not started because the previous move is still
+ * being run by the steppers. Planning can overwrite the new move.
+ *
+ * Operation:
+ *
+ * Aline generates jerk-controlled S-curves as per Ed Red's course notes:
+ *
+ * http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf
+ * http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations
+ *
+ * A full trapezoid is divided into 5 periods. Periods 1 and 2 are the
+ * 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
+ *
+ * These routines play some games with the acceleration and move timing
+ * 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 remain _OFF
+ */
+stat_t mp_exec_aline(mpBuf_t *bf) {
+ if (bf->move_state == MOVE_OFF) return STAT_NOOP; // No more moves
+
+ stat_t status = STAT_OK;
+
+ // Start a new move
+ if (!mp_runtime_is_busy()) {
+ status = _exec_aline_init(bf);
+ if (status != STAT_OK) return status;
+ }
+
+ // Main segment dispatch. From this point on the contents of the bf buffer
+ // do not affect execution.
+ switch (ex.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;
+ }
+
+ // There are 3 things that can happen here depending on return conditions:
+ //
+ // status bf->move_state Description
+ // ----------- -------------- ----------------------------------
+ // STAT_EAGAIN <don't care> buffer has more segments to run
+ // STAT_OK MOVE_RUN ex and bf buffers are done
+ // STAT_OK MOVE_NEW ex done; bf must be run again
+ // (it's been reused)
+ if (status != STAT_EAGAIN) {
+ mp_runtime_set_busy(false);
+ bf->nx->replannable = false; // prevent overplanning (Note 2)
+ if (fp_ZERO(ex.exit_velocity)) ex.segment_velocity = 0;
+ // Note, _plan_hold() may change bf->move_state to reuse this buffer so it
+ // can plan the deceleration.
+ if (bf->move_state == MOVE_RUN) mp_free_run_buffer();
+ }
+
+ if (mp_get_state() == STATE_STOPPING &&
+ (status == STAT_OK || status == STAT_EAGAIN)) {
+ if (!mp_runtime_is_busy() && !ex.segment_velocity) mp_state_holding();
+ else if (mp_runtime_is_busy() && !ex.hold_planned) _plan_hold();
+ }
+
+ return status;
+}
+
+
+/// Dequeues buffer and executes move callback
+stat_t mp_exec_move() {
+ if (mp_get_state() == STATE_ESTOPPED) return STAT_MACHINE_ALARMED;
+ if (mp_get_state() == STATE_HOLDING) return STAT_NOOP;
+
+ mpBuf_t *bf = mp_get_run_buffer();
+ if (!bf) return STAT_NOOP; // nothing running
+ if (!bf->bf_func) return CM_ALARM(STAT_INTERNAL_ERROR);
+
+ return bf->bf_func(bf); // move callback
+}
typedef struct {
- bool active; // True if a move is running
- moveSection_t section; // what section is the move in?
- bool section_new; // true if it's a new section
-
- /// unit vector for axis scaling & planning
- float unit[AXES];
- /// final target for bf (used to correct rounding errors)
- float final_target[AXES];
- /// head/body/tail endpoints for correction
- float waypoint[3][AXES];
- /// current target (absolute target as steps)
- float target_steps[MOTORS];
- /// current position (target from previous segment)
- float position_steps[MOTORS];
- /// will align with next encoder sample (target 2nd previous segment)
- float commanded_steps[MOTORS];
- /// encoder position in steps - ideally the same as commanded_steps
- float encoder_steps[MOTORS];
- /// difference between encoder & commanded steps
- float following_error[MOTORS];
-
- /// copies of bf variables of same name
- float head_length;
- float body_length;
- float tail_length;
- float entry_velocity;
- 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
-
- float position[AXES]; // Current move position
- MoveState_t ms; // Current move state
-} mp_exec_t;
-
-
-mp_exec_t ex = {0};
-
-
-/// Return TRUE if motion control busy (i.e. robot is moving)
-/// Use this function to sync to the queue. If you wait until it returns
-/// FALSE you know the queue is empty and the motors have stopped.
-bool mp_runtime_is_busy() {
- return mp_get_state() != STATE_ESTOPPED && (st_is_busy() || ex.active);
-}
+ float target[MOTORS]; // Current absolute target in steps
+ float position[MOTORS]; // Current position, target from previous segment
+ float commanded[MOTORS]; // Should align with next encoder sample (2nd prev)
+ float encoder[MOTORS]; // Encoder steps, ideally the same as commanded steps
+ float error[MOTORS]; // Difference between encoder & commanded steps
+} mp_steps_t;
+
+
+typedef struct {
+ bool busy; // True if a move is running
+ int32_t line; // Current move GCode line number
+ float position[AXES]; // Current move position
+ float work_offset[AXES]; // Current move work offset
+ float velocity; // Current move velocity
+ mp_steps_t steps;
+} mp_runtime_t;
+
+static mp_runtime_t rt;
-int32_t mp_runtime_get_line() {return ex.ms.line;}
+bool mp_runtime_is_busy() {return rt.busy;}
+void mp_runtime_set_busy(bool busy) {rt.busy = busy;}
+int32_t mp_runtime_get_line() {return rt.line;}
+
+
+void mp_runtime_set_line(int32_t line) {
+ rt.line = line;
+ report_request();
+}
/// Returns current segment velocity
-float mp_runtime_get_velocity() {return ex.segment_velocity;}
+float mp_runtime_get_velocity() {return rt.velocity;}
/// Set encoder counts to the runtime position
-void mp_runtime_set_steps_to_position() {
- float step_position[MOTORS];
-
+void mp_runtime_set_steps_from_position() {
// Convert lengths to steps in floating point
- mp_kinematics(ex.position, step_position);
+ float step_position[MOTORS];
+ mp_kinematics(rt.position, step_position);
for (int motor = 0; motor < MOTORS; motor++) {
- ex.target_steps[motor] = ex.position_steps[motor] =
- ex.commanded_steps[motor] = step_position[motor];
+ rt.steps.target[motor] = rt.steps.position[motor] =
+ rt.steps.commanded[motor] = step_position[motor];
// Write steps to encoder register
motor_set_encoder(motor, step_position[motor]);
// Must be zero
- ex.following_error[motor] = 0;
+ rt.steps.error[motor] = 0;
}
}
/* Since steps are in motor space you have to run the position vector
- * through inverse kinematics to get the right numbers. This means
+ * through inverse kinematics to get the right numbers. This means
* that in a non-Cartesian robot changing any position can result in
- * changes to multiple step values. So this operation is provided as a
+ * changes to multiple step values. So this operation is provided as a
* single function and always uses the new position vector as an
* input.
*
* Keeping track of position is complicated by the fact that moves
- * exist in several reference frames. The scheme to keep this
+ * exist in several reference frames. The scheme to keep this
* straight is:
*
* - mm.position - start and end position for planning
- * - ex.position - current position of runtime segment
- * - ex.ms.target - target position of runtime segment
- * - ex.*_steps
+ * - rt.position - current position of runtime segment
+ * - rt.steps.* - position in steps
*
* Note that position is set immediately when called and may not be
- * not an accurate representation of the tool position. The motors
+ * an accurate representation of the tool position. The motors
* are still processing the action and the real tool position is
* still close to the starting point.
*/
/// Set runtime position for a single axis
void mp_runtime_set_position(uint8_t axis, const float position) {
- ex.position[axis] = position;
+ rt.position[axis] = position;
}
/// Returns current axis position in machine coordinates
-float mp_runtime_get_absolute_position(uint8_t axis) {return ex.position[axis];}
+float mp_runtime_get_position(uint8_t axis) {return rt.position[axis];}
+float *mp_runtime_get_position_vector() {return rt.position;}
-/// Set offsets
-void mp_runtime_set_work_offset(float offset[]) {
- copy_vector(ex.ms.work_offset, offset);
+/// Returns axis position in work coordinates that were in effect at plan time
+float mp_runtime_get_work_position(uint8_t axis) {
+ return rt.position[axis] - rt.work_offset[axis];
}
-/// Returns current axis position in work coordinates
-/// that were in effect at move planning time
-float mp_runtime_get_work_position(uint8_t axis) {
- return ex.position[axis] - ex.ms.work_offset[axis];
+/// Set offsets
+void mp_runtime_set_work_offsets(float offset[]) {
+ copy_vector(rt.work_offset, offset);
}
* segment. This lines them up in time with the encoder readings so a
* following error can be generated
*
- * The following_error term is positive if the encoder reading is
+ * The rt.steps.error term is positive if the encoder reading is
* greater than (ahead of) the commanded steps, and negative (behind)
* if the encoder reading is less than the commanded steps. The
* following error is not affected by the direction of movement - it's
* 90 100 -10 encoder is 10 steps behind commanded steps
* -100 -90 -10 encoder is 10 steps behind commanded steps
*/
-static stat_t _exec_aline_segment() {
- // 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 correction if you are going into a hold.
- if (!--ex.segment_count && !ex.section_new && mp_get_state() == STATE_RUNNING)
- copy_vector(ex.ms.target, ex.waypoint[ex.section]);
-
- else {
- float segment_length = ex.segment_velocity * ex.segment_time;
-
- for (int i = 0; i < AXES; i++)
- ex.ms.target[i] = ex.position[i] + ex.unit[i] * segment_length;
- }
-
- // Convert target position to steps. Bucket-brigade the old target
- // down the chain before getting the new target from kinematics.
+stat_t mp_runtime_move_to_target(float target[], float velocity, float time) {
+ // Bucket-brigade old target down the chain before getting new target
for (int i = 0; i < MOTORS; i++) {
// previous segment's position, delayed by 1 segment
- ex.commanded_steps[i] = ex.position_steps[i];
+ rt.steps.commanded[i] = rt.steps.position[i];
// previous segment's target becomes position
- ex.position_steps[i] = ex.target_steps[i];
+ rt.steps.position[i] = rt.steps.target[i];
// current encoder position (aligns to commanded_steps)
- ex.encoder_steps[i] = motor_get_encoder(i);
- ex.following_error[i] = ex.encoder_steps[i] - ex.commanded_steps[i];
+ rt.steps.encoder[i] = motor_get_encoder(i);
+ rt.steps.error[i] = rt.steps.encoder[i] - rt.steps.commanded[i];
}
- // Determine the target steps.
- mp_kinematics(ex.ms.target, ex.target_steps);
+ // Convert target position to steps.
+ mp_kinematics(target, rt.steps.target);
// Compute distances in steps to be traveled.
//
// transforming travel distance as opposed to simply subtracting steps.
float travel_steps[MOTORS];
for (int i = 0; i < MOTORS; i++)
- travel_steps[i] = ex.target_steps[i] - ex.position_steps[i];
+ travel_steps[i] = rt.steps.target[i] - rt.steps.position[i];
// Call the stepper prep function
- RITORNO(st_prep_line(travel_steps, ex.following_error, ex.segment_time));
-
- // Update position from target
- copy_vector(ex.position, ex.ms.target);
-
- // Return EAGAIN to continue or OK if this segment is done
- return ex.segment_count ? STAT_EAGAIN : STAT_OK;
-}
-
-
-/*** 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.
- */
-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;
- float C = -10.0 * Vi + 10.0 * Vt;
- // D = 0
- // E = 0
- // F = Vi
-
- float h = 1 / segments;
-
- float Ah_5 = A * h * h * h * h * h;
- float Bh_4 = B * h * h * h * h;
- float Ch_3 = C * h * h * h;
-
- ex.forward_diff[4] = 121.0 / 16.0 * Ah_5 + 5.0 * Bh_4 + 13.0 / 4.0 * Ch_3;
- ex.forward_diff[3] = 165.0 / 2.0 * Ah_5 + 29.0 * Bh_4 + 9.0 * Ch_3;
- ex.forward_diff[2] = 255.0 * Ah_5 + 48.0 * Bh_4 + 6.0 * Ch_3;
- ex.forward_diff[1] = 300.0 * Ah_5 + 24.0 * Bh_4;
- ex.forward_diff[0] = 120.0 * Ah_5;
-
- // Calculate the initial velocity by calculating V(h / 2)
- float half_h = h / 2.0;
- float half_Ch_3 = C * half_h * half_h * half_h;
- float half_Bh_4 = B * half_h * half_h * half_h * half_h;
- float half_Ah_5 = C * half_h * half_h * half_h * half_h * half_h;
-
- return half_Ah_5 + half_Bh_4 + half_Ch_3 + Vi;
-}
-
-
-/// Common code for head and tail sections
-static stat_t _exec_aline_section(float length, float vin, float vout) {
- if (ex.section_new) {
- if (fp_ZERO(length)) return STAT_OK; // end the move
-
- // len / avg. velocity
- ex.ms.move_time = 2 * length / (vin + vout);
- ex.segments = ceil(uSec(ex.ms.move_time) / NOM_SEGMENT_USEC);
- ex.segment_time = ex.ms.move_time / ex.segments;
- ex.segment_count = (uint32_t)ex.segments;
-
- if (vin == vout) ex.segment_velocity = vin;
- else ex.segment_velocity = _init_forward_diffs(vin, vout, ex.segments);
-
- if (ex.segment_time < MIN_SEGMENT_TIME)
- return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position
-
- // Do first segment
- if (_exec_aline_segment() == STAT_OK) {
- ex.section_new = false;
- return STAT_OK;
- }
-
- ex.section_new = false;
-
- } else { // Do subsequent segments
- if (vin != vout) ex.segment_velocity += ex.forward_diff[4];
-
- if (_exec_aline_segment() == STAT_OK) return STAT_OK;
-
- if (vin != vout) {
- ex.forward_diff[4] += ex.forward_diff[3];
- ex.forward_diff[3] += ex.forward_diff[2];
- ex.forward_diff[2] += ex.forward_diff[1];
- ex.forward_diff[1] += ex.forward_diff[0];
- }
- }
-
- return STAT_EAGAIN;
-}
-
-
-/// Callback for tail section
-static stat_t _exec_aline_tail() {
- ex.section = SECTION_TAIL;
- return
- _exec_aline_section(ex.tail_length, ex.cruise_velocity, ex.exit_velocity);
-}
+ RITORNO(st_prep_line(travel_steps, rt.steps.error, time));
+ // Update position and velocity
+ copy_vector(rt.position, target);
+ rt.velocity = velocity;
-/// Callback for body section
-static stat_t _exec_aline_body() {
- ex.section = SECTION_BODY;
-
- stat_t status =
- _exec_aline_section(ex.body_length, ex.cruise_velocity, ex.cruise_velocity);
-
- if (status == STAT_OK) {
- if (ex.section_new) return _exec_aline_tail();
-
- ex.section = SECTION_TAIL;
- ex.section_new = true;
-
- return STAT_EAGAIN;
- }
-
- return status;
-}
-
-
-/// Callback for head section
-static stat_t _exec_aline_head() {
- ex.section = SECTION_HEAD;
- stat_t status =
- _exec_aline_section(ex.head_length, ex.entry_velocity, ex.cruise_velocity);
-
- if (status == STAT_OK) {
- if (ex.section_new) return _exec_aline_body();
-
- ex.section = SECTION_BODY;
- ex.section_new = true;
-
- return STAT_EAGAIN;
- }
-
- return status;
-}
-
-
-static float _compute_next_segment_velocity() {
- if (ex.section == SECTION_BODY) return ex.segment_velocity;
- return ex.segment_velocity + ex.forward_diff[4];
-}
-
-
-/*** 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() {
- mpBuf_t *bp = mp_get_run_buffer(); // working buffer pointer
- if (!bp) return; // Oops! nothing's running
-
- // Examine and process current buffer and compute length left for decel
- float available_length = get_axis_vector_length(ex.final_target, ex.position);
- // Compute next_segment velocity, velocity left to shed to brake to zero
- float braking_velocity = _compute_next_segment_velocity();
- // Distance to brake to zero from braking_velocity, bp is OK to use here
- float braking_length = mp_get_target_length(braking_velocity, 0, bp);
-
- // Hack to prevent Case 2 moves for perfect-fit decels. Happens in
- // homing situations. The real fix: The braking velocity cannot
- // simply be the ex.segment_velocity as this is the velocity of the
- // last segment, not the one that's going to be executed next. The
- // braking_velocity needs to be the velocity of the next segment
- // that has not yet been computed. In the mean time, this hack will work.
- if (available_length < braking_length && fp_ZERO(bp->exit_velocity))
- braking_length = available_length;
-
- // Replan to decelerate
- ex.section = SECTION_TAIL;
- ex.section_new = true;
- ex.cruise_velocity = braking_velocity;
- ex.hold_planned = true;
-
- // Case 1: deceleration fits entirely into the length remaining in buffer
- if (braking_length <= available_length) {
- // Set to a tail to perform the deceleration
- ex.exit_velocity = 0;
- ex.tail_length = braking_length;
-
- // Re-use bp+0 to be the hold point and to run the remaining block length
- bp->length = available_length - braking_length;
- bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp);
- bp->entry_vmax = 0; // set bp+0 as hold point
- bp->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer
-
- } else { // Case 2: deceleration exceeds length remaining in buffer
- // Replan to minimum (but non-zero) exit velocity
- ex.tail_length = available_length;
- ex.exit_velocity =
- braking_velocity - mp_get_target_velocity(0, available_length, bp);
- }
-}
-
-
-/// Initializes move runtime with a new planner buffer
-static stat_t _exec_aline_init(mpBuf_t *bf) {
- // copy in the gcode model state
- memcpy(&ex.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)) {
- ex.active = false; // reset buffer
- bf->nx->replannable = false; // prevent overplanning (Note 2)
- mp_free_run_buffer(); // free buffer
-
- return STAT_NOOP;
- }
-
- // Initialize move runtime
- bf->move_state = MOVE_RUN;
- ex.active = true;
- ex.section = SECTION_HEAD;
- ex.section_new = true;
- ex.hold_planned = false;
-
- copy_vector(ex.unit, bf->unit);
- copy_vector(ex.final_target, bf->ms.target);
-
- ex.head_length = bf->head_length;
- ex.body_length = bf->body_length;
- ex.tail_length = bf->tail_length;
- ex.entry_velocity = bf->entry_velocity;
- ex.cruise_velocity = bf->cruise_velocity;
- ex.exit_velocity = bf->exit_velocity;
-
- // Generate waypoints for position correction at section ends. This helps
- // negate floating point errors in the forward differencing code.
- for (int axis = 0; axis < AXES; axis++) {
- ex.waypoint[SECTION_HEAD][axis] =
- ex.position[axis] + ex.unit[axis] * ex.head_length;
-
- ex.waypoint[SECTION_BODY][axis] =
- ex.position[axis] + ex.unit[axis] * (ex.head_length + ex.body_length);
-
- ex.waypoint[SECTION_TAIL][axis] = ex.final_target[axis];
- }
+ report_request(); // Position and possibly velocity have changed
return STAT_OK;
}
-
-
-/* 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.
- *
- * [2] Solves a potential race condition where the current move ends but
- * the new move has not started because the previous move is still
- * being run by the steppers. Planning can overwrite the new move.
- *
- * Operation:
- *
- * Aline generates jerk-controlled S-curves as per Ed Red's course notes:
- *
- * http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf
- * http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations
- *
- * A full trapezoid is divided into 5 periods. Periods 1 and 2 are the
- * 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
- *
- * These routines play some games with the acceleration and move timing
- * 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 remain _OFF
- */
-stat_t mp_runtime_exec_aline(mpBuf_t *bf) {
- if (bf->move_state == MOVE_OFF) return STAT_NOOP; // No more moves
-
- stat_t status = STAT_OK;
-
- // Start a new move
- if (!ex.active) {
- status = _exec_aline_init(bf);
- if (status != STAT_OK) return status;
- }
-
- // Main segment dispatch. From this point on the contents of the bf buffer
- // do not affect execution.
- switch (ex.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;
- }
-
- // There are 3 things that can happen here depending on return conditions:
- //
- // status bf->move_state Description
- // ----------- -------------- ----------------------------------
- // STAT_EAGAIN <don't care> buffer has more segments to run
- // STAT_OK MOVE_RUN ex and bf buffers are done
- // STAT_OK MOVE_NEW ex done; bf must be run again
- // (it's been reused)
- if (status != STAT_EAGAIN) {
- ex.active = false; // reset buffer
- bf->nx->replannable = false; // prevent overplanning (Note 2)
- if (fp_ZERO(ex.exit_velocity)) ex.segment_velocity = 0;
- // Note, _plan_hold() may change bf->move_state to reuse this buffer so it
- // can plan the deceleration.
- if (bf->move_state == MOVE_RUN) mp_free_run_buffer();
- }
-
- if (mp_get_state() == STATE_STOPPING &&
- (status == STAT_OK || status == STAT_EAGAIN)) {
- if (!ex.active && !ex.segment_velocity) mp_state_holding();
- else if (ex.active && !ex.hold_planned) _plan_hold();
- }
-
- return status;
-}
-
-
-/// Dequeues buffer and executes move callback
-stat_t mp_runtime_exec_move() {
- if (mp_get_state() == STATE_ESTOPPED) return STAT_MACHINE_ALARMED;
- if (mp_get_state() == STATE_HOLDING) return STAT_NOOP;
-
- mpBuf_t *bf = mp_get_run_buffer();
- if (!bf) return STAT_NOOP; // nothing running
- if (!bf->bf_func) return CM_ALARM(STAT_INTERNAL_ERROR);
-
- return bf->bf_func(bf); // move callback
-}
projects / bbctrl-firmware / commitdiff