#include "report.h"
#include "plan/planner.h"
+#include "plan/runtime.h"
#include "plan/state.h"
#include <avr/pgmspace.h>
* 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.
*/
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
/// 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
}
#include "homing.h"
#include "plan/planner.h"
+#include "plan/runtime.h"
#include "plan/dwell.h"
#include "plan/command.h"
#include "plan/arc.h"
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
}
* 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();
}
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();
}
#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 <avr/interrupt.h>
void motor_init() {
// Reset position
- mp_set_steps_to_runtime_position();
+ mp_runtime_set_steps_to_position();
// Enable DMA
DMA.CTRL = DMA_RESET_bm;
* - 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,
}
-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);
#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);
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2012 - 2015 Rob Giseburt
- All rights reserved.
-
- This file ("the software") is free software: you can redistribute it
- and/or modify it under the terms of the GNU General Public License,
- version 2 as published by the Free Software Foundation. You should
- have received a copy of the GNU General Public License, version 2
- along with the software. If not, see <http://www.gnu.org/licenses/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "planner.h"
-#include "buffer.h"
-#include "stepper.h"
-#include "motor.h"
-#include "util.h"
-#include "report.h"
-#include "state.h"
-#include "config.h"
-
-#include <string.h>
-#include <stdbool.h>
-#include <math.h>
-#include <stdio.h>
-
-
-/*** 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 <don't care> 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
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- The software is distributed in the hope that it will be useful, but
- WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- Lesser General Public License for more details.
-
- You should have received a copy of the GNU Lesser General Public
- License along with the software. If not, see
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "buffer.h"
-
-stat_t mp_exec_move();
-stat_t mp_exec_aline(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
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);
mp_commit_write_buffer(-1, MOVE_TYPE_COMMAND);
}
- return 0;
+ return STAT_OK;
}
#include "line.h"
#include "planner.h"
-#include "exec.h"
+#include "runtime.h"
#include "buffer.h"
#include "machine.h"
#include "stepper.h"
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
*
* 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
#include <stdio.h>
-mpMoveRuntimeSingleton_t mr = {0}; // context for line runtime
-
-
void planner_init() {
mp_init_buffers();
}
/*** 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.
}
-/* 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)
*
* 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:
*
*
* 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
} 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;}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ All rights reserved.
+
+ This file ("the software") is free software: you can redistribute it
+ and/or modify it under the terms of the GNU General Public License,
+ version 2 as published by the Free Software Foundation. You should
+ have received a copy of the GNU General Public License, version 2
+ along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "planner.h"
+#include "buffer.h"
+#include "stepper.h"
+#include "motor.h"
+#include "util.h"
+#include "report.h"
+#include "state.h"
+#include "config.h"
+
+#include <string.h>
+#include <stdbool.h>
+#include <math.h>
+#include <stdio.h>
+
+
+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 <don't care> buffer has more segments to run
+ // STAT_OK MOVE_RUN ex and bf buffers are done
+ // STAT_OK MOVE_NEW ex done; bf must be run again
+ // (it's been reused)
+ if (status != STAT_EAGAIN) {
+ ex.active = false; // reset buffer
+ bf->nx->replannable = false; // prevent overplanning (Note 2)
+ if (fp_ZERO(ex.exit_velocity)) ex.segment_velocity = 0;
+ // Note, _plan_hold() may change bf->move_state to reuse this buffer so it
+ // can plan the deceleration.
+ if (bf->move_state == MOVE_RUN) mp_free_run_buffer();
+ }
+
+ if (mp_get_state() == STATE_STOPPING &&
+ (status == STAT_OK || status == STAT_EAGAIN)) {
+ if (!ex.active && !ex.segment_velocity) mp_state_holding();
+ else if (ex.active && !ex.hold_planned) _plan_hold();
+ }
+
+ return status;
+}
+
+
+/// Dequeues buffer and executes move callback
+stat_t mp_runtime_exec_move() {
+ if (mp_get_state() == STATE_ESTOPPED) return STAT_MACHINE_ALARMED;
+ if (mp_get_state() == STATE_HOLDING) return STAT_NOOP;
+
+ mpBuf_t *bf = mp_get_run_buffer();
+ if (!bf) return STAT_NOOP; // nothing running
+ if (!bf->bf_func) return CM_ALARM(STAT_INTERNAL_ERROR);
+
+ return bf->bf_func(bf); // move callback
+}
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "buffer.h"
+
+#include <stdbool.h>
+
+
+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);
#include "state.h"
#include "machine.h"
#include "planner.h"
+#include "runtime.h"
#include "buffer.h"
#include "arc.h"
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);}
}
// 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()) {
// 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
bool mp_is_flushing();
bool mp_is_resuming();
+bool mp_is_quiescent();
void mp_state_holding();
void mp_state_running();
#include "util.h"
#include "plan/planner.h"
+#include "plan/runtime.h"
#include "plan/state.h"
#include <avr/pgmspace.h>
* 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.
*/
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);
/// 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
}
#include "config.h"
#include "machine.h"
-#include "plan/exec.h"
+#include "plan/runtime.h"
#include "plan/command.h"
#include "motor.h"
#include "hardware.h"
/// 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;
}
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;
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);
#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());}
projects / bbctrl-firmware / commitdiff