From c7c420179c6283c30545a2e931fcb79613f3657f Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Mon, 5 Sep 2016 03:06:49 -0700 Subject: [PATCH] Split exec.c from runtime.c --- src/homing.c | 2 +- src/machine.c | 10 +- src/motor.c | 2 +- src/plan/arc.c | 2 +- src/plan/exec.c | 569 ++++++++++++++++++++++++++++++++++++++++ src/plan/exec.h | 34 +++ src/plan/line.c | 5 +- src/plan/runtime.c | 640 +++++---------------------------------------- src/plan/runtime.h | 14 +- src/plan/state.c | 5 +- src/probing.c | 10 +- src/stepper.c | 3 +- src/varcb.c | 2 +- 13 files changed, 696 insertions(+), 602 deletions(-) create mode 100644 src/plan/exec.c create mode 100644 src/plan/exec.h diff --git a/src/homing.c b/src/homing.c index d49ebb8..876dd24 100644 --- a/src/homing.c +++ b/src/homing.c @@ -407,6 +407,6 @@ void mach_homing_cycle_start_no_set() { /// Main loop callback for running the homing cycle void mach_homing_callback() { - if (mp_get_cycle() != CYCLE_HOMING || mp_runtime_is_busy()) return; + if (mp_get_cycle() != CYCLE_HOMING || mp_get_state() != STATE_READY) return; hm.func(hm.axis); // execute the current homing move } diff --git a/src/machine.c b/src/machine.c index e8787bd..ef2f556 100644 --- a/src/machine.c +++ b/src/machine.c @@ -699,7 +699,7 @@ static void _exec_offset(float *value, float *flag) { offsets[axis] = mach.offset[coord_system][axis] + mach.origin_offset[axis] * (mach.origin_offset_enable ? 1 : 0); - mp_runtime_set_work_offset(offsets); + mp_runtime_set_work_offsets(offsets); mach_set_work_offsets(); // set work offsets in the Gcode model } @@ -727,7 +727,7 @@ void mach_set_position(int axis, float position) { mach.ms.target[axis] = position; mp_set_planner_position(axis, position); mp_runtime_set_position(axis, position); - mp_runtime_set_steps_to_position(); + mp_runtime_set_steps_from_position(); } @@ -767,7 +767,7 @@ static void _exec_absolute_origin(float *value, float *flag) { mach_set_homed(axis, true); // G28.3 is not considered homed until here } - mp_runtime_set_steps_to_position(); + mp_runtime_set_steps_from_position(); } @@ -831,7 +831,7 @@ stat_t mach_straight_traverse(float target[], float flags[]) { if (status != STAT_OK) return CM_ALARM(status); // prep and plan the move - mach_set_work_offsets(&mach.gm); // capture fully resolved offsets to state + mach_set_work_offsets(); // capture fully resolved offsets to state mach.ms.line = mach.gm.line; // copy line number mp_aline(&mach.ms); // send the move to the planner mach_finalize_move(); @@ -927,7 +927,7 @@ stat_t mach_straight_feed(float target[], float flags[]) { if (status != STAT_OK) return CM_ALARM(status); // prep and plan the move - mach_set_work_offsets(&mach.gm); // capture fully resolved offsets to state + mach_set_work_offsets(); // capture fully resolved offsets to state mach.ms.line = mach.gm.line; // copy line number status = mp_aline(&mach.ms); // send the move to the planner mach_finalize_move(); diff --git a/src/motor.c b/src/motor.c index 97a7e56..47c53e1 100644 --- a/src/motor.c +++ b/src/motor.c @@ -159,7 +159,7 @@ ISR(TCE1_CCA_vect) { void motor_init() { // Reset position - mp_runtime_set_steps_to_position(); + mp_runtime_set_steps_from_position(); // Enable DMA DMA.CTRL = DMA_RESET_bm; diff --git a/src/plan/arc.c b/src/plan/arc.c index e97398d..f24b7b8 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -437,7 +437,7 @@ stat_t mach_arc_feed(float target[], float flags[], // arc endpoints // now get down to the rest of the work setting up the arc for execution mach.gm.motion_mode = motion_mode; - mach_set_work_offsets(&mach.gm); // resolved offsets to gm + mach_set_work_offsets(); // Update resolved offsets mach.ms.line = mach.gm.line; // copy line number memcpy(&arc.ms, &mach.ms, sizeof(MoveState_t)); // context to arc singleton diff --git a/src/plan/exec.c b/src/plan/exec.c new file mode 100644 index 0000000..d360579 --- /dev/null +++ b/src/plan/exec.c @@ -0,0 +1,569 @@ +/******************************************************************************\ + + 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 . + + 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 + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "planner.h" +#include "buffer.h" +#include "util.h" +#include "runtime.h" +#include "state.h" +#include "config.h" + +#include +#include +#include +#include + + +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 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 +} diff --git a/src/plan/exec.h b/src/plan/exec.h new file mode 100644 index 0000000..cc9d174 --- /dev/null +++ b/src/plan/exec.h @@ -0,0 +1,34 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + 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 . + + 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 + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "buffer.h" + + +stat_t mp_exec_move(); +stat_t mp_exec_aline(mpBuf_t *bf); diff --git a/src/plan/line.c b/src/plan/line.c index d80585b..eccfb46 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -30,6 +30,7 @@ #include "line.h" #include "planner.h" +#include "exec.h" #include "runtime.h" #include "buffer.h" #include "machine.h" @@ -159,7 +160,7 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { } -/* Plan a line with acceleration / deceleration +/*** Plan a line with acceleration / deceleration * * This function uses constant jerk motion equations to plan * acceleration and deceleration. Jerk is the rate of change of @@ -237,7 +238,7 @@ stat_t mp_aline(MoveState_t *ms) { if (!bf) return CM_ALARM(STAT_BUFFER_FULL_FATAL); // never fails // Register callback to exec function - bf->bf_func = mp_runtime_exec_aline; + bf->bf_func = mp_exec_aline; bf->length = length; // Copy model state into planner buffer diff --git a/src/plan/runtime.c b/src/plan/runtime.c index 8459d15..ef0c680 100644 --- a/src/plan/runtime.c +++ b/src/plan/runtime.c @@ -43,103 +43,77 @@ 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. */ @@ -147,24 +121,24 @@ void mp_runtime_set_steps_to_position() { /// 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); } @@ -176,7 +150,7 @@ float mp_runtime_get_work_position(uint8_t axis) { * 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 @@ -188,35 +162,20 @@ float mp_runtime_get_work_position(uint8_t axis) { * 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. // @@ -225,487 +184,16 @@ static stat_t _exec_aline_segment() { // 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 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 -} diff --git a/src/plan/runtime.h b/src/plan/runtime.h index f726dc1..66275af 100644 --- a/src/plan/runtime.h +++ b/src/plan/runtime.h @@ -27,18 +27,20 @@ #pragma once -#include "buffer.h" +#include "status.h" #include bool mp_runtime_is_busy(); +void mp_runtime_set_busy(bool busy); int32_t mp_runtime_get_line(); +void mp_runtime_set_line(int32_t line); float mp_runtime_get_velocity(); -void mp_runtime_set_steps_to_position(); +void mp_runtime_set_steps_from_position(); void mp_runtime_set_position(uint8_t axis, const float position); +float mp_runtime_get_position(uint8_t axis); +float *mp_runtime_get_position_vector(); float mp_runtime_get_work_position(uint8_t axis); -float mp_runtime_get_absolute_position(uint8_t axis); -void mp_runtime_set_work_offset(float offset[]); -stat_t mp_runtime_exec_move(); -stat_t mp_runtime_exec_aline(mpBuf_t *bf); +void mp_runtime_set_work_offsets(float offset[]); +stat_t mp_runtime_move_to_target(float target[], float velocity, float time); diff --git a/src/plan/state.c b/src/plan/state.c index 003f298..40c99e2 100644 --- a/src/plan/state.c +++ b/src/plan/state.c @@ -33,6 +33,7 @@ #include "runtime.h" #include "buffer.h" #include "arc.h" +#include "stepper.h" #include "report.h" @@ -120,7 +121,7 @@ bool mp_is_resuming() {return ps.resume_requested;} bool mp_is_quiescent() { return (mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING) && - !mp_runtime_is_busy(); + !st_is_busy() && !mp_runtime_is_busy(); } @@ -185,7 +186,7 @@ void mp_state_callback() { // Reset to actual machine position. Otherwise machine is set to the // position of the last queued move. for (int axis = 0; axis < AXES; axis++) - mach_set_position(axis, mp_runtime_get_absolute_position(axis)); + mach_set_position(axis, mp_runtime_get_position(axis)); } // Resume diff --git a/src/probing.c b/src/probing.c index 3fe2bcd..69eee2b 100644 --- a/src/probing.c +++ b/src/probing.c @@ -79,11 +79,9 @@ static struct pbProbingSingleton pb = {0}; * * Another Note: When coding a cycle (like this one) you must wait * until the last move has actually been queued (or has finished) - * before declaring the cycle to be done. Otherwise there is a nasty - * race condition in the tg_controller() that will accept the next - * command before the position of the final move has been recorded in - * the Gcode model. That's what the call to mp_runtime_is_busy() is - * about. + * before declaring the cycle to be done. Otherwise there is a nasty + * race condition that will accept the next command before the position + * of the final move has been recorded in the Gcode model. */ @@ -220,7 +218,7 @@ stat_t mach_straight_probe(float target[], float flags[]) { /// main loop callback for running the homing cycle void mach_probe_callback() { // sync to planner move ends - if (!mach_is_probing() || mp_runtime_is_busy()) return; + if (!mach_is_probing() || mp_get_state() != STATE_READY) return; pb.func(); // execute the current homing move } diff --git a/src/stepper.c b/src/stepper.c index a773939..15cb871 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -32,6 +32,7 @@ #include "config.h" #include "machine.h" #include "plan/runtime.h" +#include "plan/exec.h" #include "plan/command.h" #include "motor.h" #include "hardware.h" @@ -86,7 +87,7 @@ uint8_t st_is_busy() {return st.busy;} /// Interrupt handler for calling move exec function. /// ADC channel 0 triggered by load ISR as a "software" interrupt. ISR(ADCB_CH0_vect) { - mp_runtime_exec_move(); + mp_exec_move(); ADCB_CH0_INTCTRL = 0; st.requesting = false; } diff --git a/src/varcb.c b/src/varcb.c index 8876c90..e723bf7 100644 --- a/src/varcb.c +++ b/src/varcb.c @@ -34,7 +34,7 @@ #include "plan/buffer.h" -float get_position(int index) {return mp_runtime_get_absolute_position(index);} +float get_position(int index) {return mp_runtime_get_position(index);} float get_velocity() {return mp_runtime_get_velocity();} float get_speed() {return mach_get_spindle_speed();} float get_feed() {return mach_get_feed_rate();} -- 2.27.0