From: Joseph Coffland Date: Mon, 5 Sep 2016 03:08:25 +0000 (-0700) Subject: Function renaming, planner reorg and runtime state encapsulation. X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=2986e914c14e02376d92488bfabf0d8d0cde1fb9;p=bbctrl-firmware Function renaming, planner reorg and runtime state encapsulation. --- diff --git a/src/homing.c b/src/homing.c index 5c26baf..d49ebb8 100644 --- a/src/homing.c +++ b/src/homing.c @@ -32,6 +32,7 @@ #include "report.h" #include "plan/planner.h" +#include "plan/runtime.h" #include "plan/state.h" #include @@ -148,7 +149,7 @@ static struct hmHomingSingleton hm = {0}; * 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. That's what the call - * to mach_isbusy() is about. + * to mach_is_busy() is about. */ @@ -232,7 +233,7 @@ static void _homing_axis_set_zero(int8_t axis) { hm.homed[axis] = true; } else // do not set axis if in G28.4 cycle - mach_set_position(axis, mp_get_runtime_work_position(axis)); + mach_set_position(axis, mp_runtime_get_work_position(axis)); mach_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value @@ -406,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_get_runtime_busy()) return; + if (mp_get_cycle() != CYCLE_HOMING || mp_runtime_is_busy()) return; hm.func(hm.axis); // execute the current homing move } diff --git a/src/machine.c b/src/machine.c index 6fa9bcd..e8787bd 100644 --- a/src/machine.c +++ b/src/machine.c @@ -80,6 +80,7 @@ #include "homing.h" #include "plan/planner.h" +#include "plan/runtime.h" #include "plan/dwell.h" #include "plan/command.h" #include "plan/arc.h" @@ -698,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_set_runtime_work_offset(offsets); + mp_runtime_set_work_offset(offsets); mach_set_work_offsets(); // set work offsets in the Gcode model } @@ -717,19 +718,16 @@ static void _exec_offset(float *value, float *flag) { * frame than the one you are now going to set. These functions should * only be called during initialization sequences and during cycles * (such as homing cycles) when you know there are no more moves in - * the planner and that all motion has stopped. Use - * mp_get_runtime_busy() to be sure the system is quiescent. + * the planner and that all motion has stopped. */ void mach_set_position(int axis, float position) { - if ((mp_get_state() != STATE_HOLDING && mp_get_state() != STATE_READY) || - mp_get_runtime_busy()) - CM_ALARM(STAT_INTERNAL_ERROR); + if (!mp_is_quiescent()) CM_ALARM(STAT_INTERNAL_ERROR); mach.position[axis] = position; mach.ms.target[axis] = position; mp_set_planner_position(axis, position); - mp_set_runtime_position(axis, position); - mp_set_steps_to_runtime_position(); + mp_runtime_set_position(axis, position); + mp_runtime_set_steps_to_position(); } @@ -765,11 +763,11 @@ void mach_set_absolute_origin(float origin[], float flag[]) { static void _exec_absolute_origin(float *value, float *flag) { for (int axis = 0; axis < AXES; axis++) if (fp_TRUE(flag[axis])) { - mp_set_runtime_position(axis, value[axis]); + mp_runtime_set_position(axis, value[axis]); mach_set_homed(axis, true); // G28.3 is not considered homed until here } - mp_set_steps_to_runtime_position(); + mp_runtime_set_steps_to_position(); } diff --git a/src/motor.c b/src/motor.c index 3e91b0d..97a7e56 100644 --- a/src/motor.c +++ b/src/motor.c @@ -34,8 +34,9 @@ #include "stepper.h" #include "tmc2660.h" #include "estop.h" +#include "util.h" -#include "plan/planner.h" +#include "plan/runtime.h" #include "plan/calibrate.h" #include @@ -158,7 +159,7 @@ ISR(TCE1_CCA_vect) { void motor_init() { // Reset position - mp_set_steps_to_runtime_position(); + mp_runtime_set_steps_to_position(); // Enable DMA DMA.CTRL = DMA_RESET_bm; diff --git a/src/plan/command.c b/src/plan/command.c index 89c8a6a..b043887 100644 --- a/src/plan/command.c +++ b/src/plan/command.c @@ -36,9 +36,9 @@ * - When the planning queue gets to the function it calls _exec_command() * which loads a pointer to the bf buffer in stepper.c's next move. * - When the runtime gets to the end of the current activity (sending steps, - * counting a dwell) it executes mp_runtime_command which uses the callback + * counting a dwell) it executes mp_command_callback which uses the callback * function in the bf and the saved parameters in the vectors. - * - To finish up mp_runtime_command() frees the bf buffer. + * - To finish up mp_command_callback() frees the bf buffer. * * Doing it this way instead of synchronizing on queue empty simplifies the * handling of feedholds, feed overrides, buffer flushes, and thread blocking, @@ -82,7 +82,7 @@ void mp_queue_command(mach_exec_t mach_exec, float *value, float *flag) { } -void mp_runtime_command(mpBuf_t *bf) { +void mp_command_callback(mpBuf_t *bf) { // Use values & flags stored in mp_queue_command() bf->mach_func(bf->ms.target, bf->unit); diff --git a/src/plan/command.h b/src/plan/command.h index 599bf11..a337a30 100644 --- a/src/plan/command.h +++ b/src/plan/command.h @@ -30,4 +30,4 @@ #include "plan/buffer.h" void mp_queue_command(mach_exec_t mach_exec, float *value, float *flag); -void mp_runtime_command(mpBuf_t *bf); +void mp_command_callback(mpBuf_t *bf); diff --git a/src/plan/exec.c b/src/plan/exec.c deleted file mode 100644 index 9a424d8..0000000 --- a/src/plan/exec.c +++ /dev/null @@ -1,589 +0,0 @@ -/******************************************************************************\ - - 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 "stepper.h" -#include "motor.h" -#include "util.h" -#include "report.h" -#include "state.h" -#include "config.h" - -#include -#include -#include -#include - - -/*** Segment runner - * - * Notes on step error correction: - * - * The commanded_steps are the target_steps delayed by one more - * 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 - * 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 - * purely a statement of relative position. Examples: - * - * Encoder Commanded Following Err - * 100 90 +10 encoder is 10 steps ahead of commanded steps - * -90 -100 +10 encoder is 10 steps ahead of commanded steps - * 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() { - float travel_steps[MOTORS]; - - // 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 (!--mr.segment_count && !mr.section_new && mp_get_state() == STATE_RUNNING) - copy_vector(mr.ms.target, mr.waypoint[mr.section]); - - else { - float segment_length = mr.segment_velocity * mr.segment_time; - - for (int i = 0; i < AXES; i++) - mr.ms.target[i] = mr.position[i] + mr.unit[i] * segment_length; - } - - // Convert target position to steps. Bucket-brigade the old target - // down the chain before getting the new target from kinematics - // - // The direct manipulation of steps to compute travel_steps only - // works for Cartesian kinematics. Other kinematics may require - // transforming travel distance as opposed to simply subtracting - // steps. - for (int i = 0; i < MOTORS; i++) { - // previous segment's position, delayed by 1 segment - mr.commanded_steps[i] = mr.position_steps[i]; - // previous segment's target becomes position - mr.position_steps[i] = mr.target_steps[i]; - // current encoder position (aligns to commanded_steps) - mr.encoder_steps[i] = motor_get_encoder(i); - mr.following_error[i] = mr.encoder_steps[i] - mr.commanded_steps[i]; - } - - // now determine the target steps - mp_kinematics(mr.ms.target, mr.target_steps); - - // and compute the distances, in steps, to be traveled - for (int i = 0; i < MOTORS; i++) - travel_steps[i] = mr.target_steps[i] - mr.position_steps[i]; - - // Call the stepper prep function - RITORNO(st_prep_line(travel_steps, mr.following_error, mr.segment_time)); - - // update position from target - copy_vector(mr.position, mr.ms.target); - - if (!mr.segment_count) return STAT_OK; // this section has run all segments - return STAT_EAGAIN; // this section still has more segments to run -} - - -/*** 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; - - mr.forward_diff[4] = 121.0 / 16.0 * Ah_5 + 5.0 * Bh_4 + 13.0 / 4.0 * Ch_3; - mr.forward_diff[3] = 165.0 / 2.0 * Ah_5 + 29.0 * Bh_4 + 9.0 * Ch_3; - mr.forward_diff[2] = 255.0 * Ah_5 + 48.0 * Bh_4 + 6.0 * Ch_3; - mr.forward_diff[1] = 300.0 * Ah_5 + 24.0 * Bh_4; - mr.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 (mr.section_new) { - if (fp_ZERO(length)) return STAT_OK; // end the move - - // len / avg. velocity - mr.ms.move_time = 2 * length / (vin + vout); - mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC); - mr.segment_time = mr.ms.move_time / mr.segments; - mr.segment_count = (uint32_t)mr.segments; - - if (vin == vout) mr.segment_velocity = vin; - else mr.segment_velocity = _init_forward_diffs(vin, vout, mr.segments); - - if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position - - // Do first segment - if (_exec_aline_segment() == STAT_OK) { - mr.section_new = false; - return STAT_OK; - } - - mr.section_new = false; - - } else { // Do subsequent segments - if (vin != vout) mr.segment_velocity += mr.forward_diff[4]; - - if (_exec_aline_segment() == STAT_OK) return STAT_OK; - - if (vin != vout) { - mr.forward_diff[4] += mr.forward_diff[3]; - mr.forward_diff[3] += mr.forward_diff[2]; - mr.forward_diff[2] += mr.forward_diff[1]; - mr.forward_diff[1] += mr.forward_diff[0]; - } - } - - return STAT_EAGAIN; -} - - -/// Callback for tail section -static stat_t _exec_aline_tail() { - mr.section = SECTION_TAIL; - return - _exec_aline_section(mr.tail_length, mr.cruise_velocity, mr.exit_velocity); -} - - -/// Callback for body section -static stat_t _exec_aline_body() { - mr.section = SECTION_BODY; - - stat_t status = - _exec_aline_section(mr.body_length, mr.cruise_velocity, mr.cruise_velocity); - - if (status == STAT_OK) { - if (mr.section_new) return _exec_aline_tail(); - - mr.section = SECTION_TAIL; - mr.section_new = true; - - return STAT_EAGAIN; - } - - return status; -} - - -/// Callback for head section -static stat_t _exec_aline_head() { - mr.section = SECTION_HEAD; - stat_t status = - _exec_aline_section(mr.head_length, mr.entry_velocity, mr.cruise_velocity); - - if (status == STAT_OK) { - if (mr.section_new) return _exec_aline_body(); - - mr.section = SECTION_BODY; - mr.section_new = true; - - return STAT_EAGAIN; - } - - return status; -} - - -static float _compute_next_segment_velocity() { - if (mr.section == SECTION_BODY) return mr.segment_velocity; - return mr.segment_velocity + mr.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 mr buffer and compute length left for decel - float available_length = get_axis_vector_length(mr.final_target, mr.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 mr.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 mr to decelerate - mr.section = SECTION_TAIL; - mr.section_new = true; - mr.cruise_velocity = braking_velocity; - mr.hold_planned = true; - - // Case 1: deceleration fits entirely into the length remaining in mr buffer - if (braking_length <= available_length) { - // set mr to a tail to perform the deceleration - mr.exit_velocity = 0; - mr.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 mr buffer - // Replan mr to minimum (but non-zero) exit velocity - mr.tail_length = available_length; - mr.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(&mr.ms, &bf->ms, sizeof(MoveState_t)); - bf->replannable = false; - report_request(); // Executing line number has changed - - // Remove zero length lines. Short lines have already been removed. - if (fp_ZERO(bf->length)) { - mr.active = false; // reset mr 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; - mr.active = true; - mr.section = SECTION_HEAD; - mr.section_new = true; - mr.hold_planned = false; - - copy_vector(mr.unit, bf->unit); - copy_vector(mr.final_target, bf->ms.target); - - mr.head_length = bf->head_length; - mr.body_length = bf->body_length; - mr.tail_length = bf->tail_length; - mr.entry_velocity = bf->entry_velocity; - mr.cruise_velocity = bf->cruise_velocity; - mr.exit_velocity = bf->exit_velocity; - mr.jerk = bf->jerk; - - // 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++) { - mr.waypoint[SECTION_HEAD][axis] = - mr.position[axis] + mr.unit[axis] * mr.head_length; - - mr.waypoint[SECTION_BODY][axis] = - mr.position[axis] + mr.unit[axis] * (mr.head_length + mr.body_length); - - mr.waypoint[SECTION_TAIL][axis] = mr.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 (!mr.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 (mr.section) { - case SECTION_HEAD: status = _exec_aline_head(); break; - case SECTION_BODY: status = _exec_aline_body(); break; - case SECTION_TAIL: status = _exec_aline_tail(); break; - } - - // There are 3 things that can happen here depending on return conditions: - // - // status bf->move_state Description - // ----------- -------------- ---------------------------------- - // STAT_EAGAIN mr buffer has more segments to run - // STAT_OK MOVE_RUN mr and bf buffers are done - // STAT_OK MOVE_NEW mr done; bf must be run again - // (it's been reused) - if (status != STAT_EAGAIN) { - mr.active = false; // reset mr buffer - bf->nx->replannable = false; // prevent overplanning (Note 2) - if (fp_ZERO(mr.exit_velocity)) mr.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 (!mr.active && !mr.segment_velocity) mp_state_holding(); - else if (mr.active && !mr.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 deleted file mode 100644 index c73bb1c..0000000 --- a/src/plan/exec.h +++ /dev/null @@ -1,33 +0,0 @@ -/******************************************************************************\ - - 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/jog.c b/src/plan/jog.c index 408f8f2..fa3b4b4 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -81,7 +81,7 @@ static stat_t _exec_jog(mpBuf_t *bf) { // Compute travel from velocity travel[axis] = time * jr.current_velocity[axis]; - if (travel[axis]) done = false; + if (!fp_ZERO(travel[axis])) done = false; } // Check if we are done @@ -138,10 +138,7 @@ uint8_t command_jog(int argc, char *argv[]) { if (!mp_jog_busy()) { // Should always be at least one free buffer mpBuf_t *bf = mp_get_write_buffer(); - if (!bf) { - CM_ALARM(STAT_BUFFER_FULL_FATAL); - return 0; - } + if (!bf) return STAT_BUFFER_FULL_FATAL; // Start mp_set_cycle(CYCLE_JOGGING); @@ -149,5 +146,5 @@ uint8_t command_jog(int argc, char *argv[]) { mp_commit_write_buffer(-1, MOVE_TYPE_COMMAND); } - return 0; + return STAT_OK; } diff --git a/src/plan/line.c b/src/plan/line.c index e710e73..d80585b 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -30,7 +30,7 @@ #include "line.h" #include "planner.h" -#include "exec.h" +#include "runtime.h" #include "buffer.h" #include "machine.h" #include "stepper.h" @@ -237,7 +237,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_exec_aline; + bf->bf_func = mp_runtime_exec_aline; bf->length = length; // Copy model state into planner buffer diff --git a/src/plan/planner.c b/src/plan/planner.c index 4a20248..2fd1cb6 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -41,7 +41,7 @@ * * One important concept is isolation of the three layers of the * data model - the Gcode model (gm), planner model (bf queue & mm), - * and runtime model (mr). + * and runtime model (exec.c). * * The Gcode model is owned by the machine and should only be * accessed by mach_xxxx() functions. Data from the Gcode model is @@ -69,9 +69,6 @@ #include -mpMoveRuntimeSingleton_t mr = {0}; // context for line runtime - - void planner_init() { mp_init_buffers(); } @@ -79,7 +76,7 @@ void planner_init() { /*** Flush all moves in the planner * - * Does not affect the move currently running in mr. Does not affect + * Does not affect the move currently running. Does not affect * mm or gm model positions. This function is designed to be called * during a hold to reset the planner. This function should not usually * be directly called. Call mp_request_flush() instead. @@ -89,86 +86,6 @@ void mp_flush_planner() { } -/* Since steps are in motor space you have to run the position vector - * 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 - * 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 - * straight is: - * - * - mm.position - start and end position for planning - * - mr.position - current position of runtime segment - * - mr.ms.target - target position of runtime segment - * - mr.endpoint - final target position of runtime segment - * - * Note that position is set immediately when called and may not be - * not 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_set_runtime_position(uint8_t axis, const float position) { - mr.position[axis] = position; -} - - -/// Set encoder counts to the runtime position -void mp_set_steps_to_runtime_position() { - float step_position[MOTORS]; - - // convert lengths to steps in floating point - mp_kinematics(mr.position, step_position); - - for (int motor = 0; motor < MOTORS; motor++) { - mr.target_steps[motor] = step_position[motor]; - mr.position_steps[motor] = step_position[motor]; - mr.commanded_steps[motor] = step_position[motor]; - - // write steps to encoder register - motor_set_encoder(motor, step_position[motor]); - - // must be zero - mr.following_error[motor] = 0; - } -} - - -/// Returns current velocity (aggregate) -float mp_get_runtime_velocity() {return mr.segment_velocity;} - - -/// Returns current axis position in machine coordinates -float mp_get_runtime_absolute_position(uint8_t axis) {return mr.position[axis];} - - -/// Set offsets in the MR struct -void mp_set_runtime_work_offset(float offset[]) { - copy_vector(mr.ms.work_offset, offset); -} - - -/// Returns current axis position in work coordinates -/// that were in effect at move planning time -float mp_get_runtime_work_position(uint8_t axis) { - return mr.position[axis] - mr.ms.work_offset[axis]; -} - - -/// 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. -uint8_t mp_get_runtime_busy() { - if (mp_get_state() == STATE_ESTOPPED) return false; - return st_runtime_isbusy() || mr.active; -} - - /* Performs axis mapping & conversion of length units to steps (and deals * with inhibited axes) * @@ -563,7 +480,7 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { * bf->ms.target[] - block target position * bf->unit[] - block unit vector * bf->time - gets set later - * bf->jerk - source of the other jerk variables. Used in mr. + * bf->jerk - source of the other jerk variables. * * Notes: * @@ -574,8 +491,8 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { * * In normal operation the first block (currently running * block) is not replanned, but may be for feedholds and feed - * overrides. In these cases the prep routines modify the - * contents of the mr buffer and re-shuffle the block list, + * overrides. In these cases the prep routines modify the + * contents of the (ex) buffer and re-shuffle the block list, * re-enlisting the current bf buffer with new parameters. * These routines also set all blocks in the list to be * replannable so the list can be recomputed regardless of diff --git a/src/plan/planner.h b/src/plan/planner.h index 4f91da2..e64308b 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -73,66 +73,10 @@ typedef enum { } moveSection_t; -typedef struct mpMoveRuntimeSingleton { // persistent runtime variables - 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]; - /// current move position - float position[AXES]; - /// head/body/tail endpoints for correction - float waypoint[3][AXES]; - /// current MR target (absolute target as steps) - float target_steps[MOTORS]; - /// current MR 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 jerk; // max linear jerk - - 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 - - MoveState_t ms; -} mpMoveRuntimeSingleton_t; - - -// Reference global scope structures -extern mpMoveRuntimeSingleton_t mr; // context for line runtime - - void planner_init(); void mp_flush_planner(); -void mp_set_runtime_position(uint8_t axis, const float position); -void mp_set_steps_to_runtime_position(); -float mp_get_runtime_velocity(); -float mp_get_runtime_work_position(uint8_t axis); -float mp_get_runtime_absolute_position(uint8_t axis); -void mp_set_runtime_work_offset(float offset[]); -uint8_t mp_get_runtime_busy(); void mp_kinematics(const float travel[], float steps[]); void mp_plan_block_list(mpBuf_t *bf); void mp_replan_blocks(); float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf); float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf); -inline int32_t mp_get_line() {return mr.ms.line;} diff --git a/src/plan/runtime.c b/src/plan/runtime.c new file mode 100644 index 0000000..8459d15 --- /dev/null +++ b/src/plan/runtime.c @@ -0,0 +1,711 @@ +/******************************************************************************\ + + 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 "stepper.h" +#include "motor.h" +#include "util.h" +#include "report.h" +#include "state.h" +#include "config.h" + +#include +#include +#include +#include + + +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); +} + + +int32_t mp_runtime_get_line() {return ex.ms.line;} + + +/// Returns current segment velocity +float mp_runtime_get_velocity() {return ex.segment_velocity;} + + +/// Set encoder counts to the runtime position +void mp_runtime_set_steps_to_position() { + float step_position[MOTORS]; + + // Convert lengths to steps in floating point + mp_kinematics(ex.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]; + + // Write steps to encoder register + motor_set_encoder(motor, step_position[motor]); + + // Must be zero + ex.following_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 + * that in a non-Cartesian robot changing any position can result in + * 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 + * 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 + * + * Note that position is set immediately when called and may not be + * not 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; +} + + +/// Returns current axis position in machine coordinates +float mp_runtime_get_absolute_position(uint8_t axis) {return ex.position[axis];} + + +/// Set offsets +void mp_runtime_set_work_offset(float offset[]) { + copy_vector(ex.ms.work_offset, offset); +} + + +/// 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]; +} + + +/*** Segment runner + * + * Notes on step error correction: + * + * The commanded_steps are the target_steps delayed by one more + * 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 + * 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 + * purely a statement of relative position. Examples: + * + * Encoder Commanded Following Err + * 100 90 +10 encoder is 10 steps ahead of commanded steps + * -90 -100 +10 encoder is 10 steps ahead of commanded steps + * 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. + for (int i = 0; i < MOTORS; i++) { + // previous segment's position, delayed by 1 segment + ex.commanded_steps[i] = ex.position_steps[i]; + // previous segment's target becomes position + ex.position_steps[i] = ex.target_steps[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]; + } + + // Determine the target steps. + mp_kinematics(ex.ms.target, ex.target_steps); + + // Compute distances in steps to be traveled. + // + // The direct manipulation of steps to compute travel_steps only + // works for Cartesian kinematics. Other kinematics may require + // 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]; + + // 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); +} + + +/// 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]; + } + + 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 new file mode 100644 index 0000000..f726dc1 --- /dev/null +++ b/src/plan/runtime.h @@ -0,0 +1,44 @@ +/******************************************************************************\ + + 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" + +#include + + +bool mp_runtime_is_busy(); +int32_t mp_runtime_get_line(); +float mp_runtime_get_velocity(); +void mp_runtime_set_steps_to_position(); +void mp_runtime_set_position(uint8_t axis, const float position); +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); diff --git a/src/plan/state.c b/src/plan/state.c index 0b5252a..003f298 100644 --- a/src/plan/state.c +++ b/src/plan/state.c @@ -30,6 +30,7 @@ #include "state.h" #include "machine.h" #include "planner.h" +#include "runtime.h" #include "buffer.h" #include "arc.h" @@ -117,6 +118,12 @@ bool mp_is_flushing() {return ps.flush_requested && !ps.resume_requested;} 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(); +} + + void mp_state_holding() {_set_state(STATE_HOLDING);} @@ -168,10 +175,7 @@ void mp_state_callback() { } // Only flush queue when idle or holding. - if (ps.flush_requested && - (mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING) && - !mp_get_runtime_busy()) { - + if (ps.flush_requested && mp_is_quiescent()) { mach_abort_arc(); if (!mp_queue_empty()) { @@ -181,7 +185,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_get_runtime_absolute_position(axis)); + mach_set_position(axis, mp_runtime_get_absolute_position(axis)); } // Resume diff --git a/src/plan/state.h b/src/plan/state.h index f5e8cf6..a8ca4a9 100644 --- a/src/plan/state.h +++ b/src/plan/state.h @@ -62,6 +62,7 @@ PGM_P mp_get_cycle_pgmstr(plannerCycle_t cycle); bool mp_is_flushing(); bool mp_is_resuming(); +bool mp_is_quiescent(); void mp_state_holding(); void mp_state_running(); diff --git a/src/probing.c b/src/probing.c index f372155..3fe2bcd 100644 --- a/src/probing.c +++ b/src/probing.c @@ -32,6 +32,7 @@ #include "util.h" #include "plan/planner.h" +#include "plan/runtime.h" #include "plan/state.h" #include @@ -81,7 +82,7 @@ static struct pbProbingSingleton pb = {0}; * 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_get_runtime_busy() is + * the Gcode model. That's what the call to mp_runtime_is_busy() is * about. */ @@ -116,7 +117,7 @@ static void _probing_finish() { for (int axis = 0; axis < AXES; axis++) { // if we got here because of a feed hold keep the model position correct - mach_set_position(axis, mp_get_runtime_work_position(axis)); + mach_set_position(axis, mp_runtime_get_work_position(axis)); // store the probe results pb.results[axis] = mach_get_absolute_position(axis); @@ -219,7 +220,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_get_runtime_busy()) return; + if (!mach_is_probing() || mp_runtime_is_busy()) return; pb.func(); // execute the current homing move } diff --git a/src/stepper.c b/src/stepper.c index 8518ea0..a773939 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -31,7 +31,7 @@ #include "config.h" #include "machine.h" -#include "plan/exec.h" +#include "plan/runtime.h" #include "plan/command.h" #include "motor.h" #include "hardware.h" @@ -80,13 +80,13 @@ void st_shutdown() { /// Return true if motors or dwell are running -uint8_t st_runtime_isbusy() {return st.busy;} +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_exec_move(); + mp_runtime_exec_move(); ADCB_CH0_INTCTRL = 0; st.requesting = false; } @@ -142,7 +142,7 @@ ISR(STEP_TIMER_ISR) { st.dwell = st.prep_dwell; } else if (st.move_type == MOVE_TYPE_COMMAND) - mp_runtime_command(st.bf); // Execute command + mp_command_callback(st.bf); // Execute command // We are done with this move st.move_type = MOVE_TYPE_NULL; diff --git a/src/stepper.h b/src/stepper.h index a77ca07..ec9c37b 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -37,7 +37,7 @@ void stepper_init(); void st_shutdown(); -uint8_t st_runtime_isbusy(); +uint8_t st_is_busy(); stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time); void st_prep_command(mpBuf_t *bf); diff --git a/src/varcb.c b/src/varcb.c index 4d4a136..8876c90 100644 --- a/src/varcb.c +++ b/src/varcb.c @@ -28,20 +28,21 @@ #include "usart.h" #include "machine.h" +#include "plan/runtime.h" #include "plan/planner.h" #include "plan/state.h" #include "plan/buffer.h" -float get_position(int index) {return mp_get_runtime_absolute_position(index);} -float get_velocity() {return mp_get_runtime_velocity();} +float get_position(int index) {return mp_runtime_get_absolute_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();} uint8_t get_tool() {return mach_get_tool();} bool get_echo() {return usart_is_set(USART_ECHO);} void set_echo(bool value) {return usart_set(USART_ECHO, value);} uint16_t get_queue() {return mp_get_planner_buffer_room();} -int32_t get_line() {return mp_get_line();} +int32_t get_line() {return mp_runtime_get_line();} PGM_P get_state() {return mp_get_state_pgmstr(mp_get_state());} PGM_P get_cycle() {return mp_get_cycle_pgmstr(mp_get_cycle());} PGM_P get_unit() {return mp_get_units_mode_pgmstr(mach_get_units_mode());}