On the first call to ``mp_exec_aline()`` a call is made to
``_exec_aline_init()``. This function may stop processing the move if a
feedhold is in effect. It may also skip a move if it has zero length.
-Otherwise, it initializes the move runtime state (``mr``) by copying the move
-state from the planner buffer and initializing variables. In addition, it
-computes waypoints at the ends of each trapezoid section. Waypoints are used
-later for position correction which adjust position for rounding errors.
+Otherwise, it initializes the move runtime state (``mr``) copying over the
+variables set in the planner buffer. In addition, it computes waypoints at
+the ends of each trapezoid section. Waypoints are used later to correct
+position for rounding errors.
### Move execution
After move initialization ``mp_exec_aline()`` calls ``_exec_aline_head()``,
-``_exec_aline_body()`` and ``exec_aline_tail()`` on successive callbacks. Each
-of these functions are called repeatedly until the section finishes. If any
+``_exec_aline_body()`` and ``exec_aline_tail()`` on successive callbacks.
+These functions are called repeatedly until each section finishes. If any
sections have zero length they are skipped and execution is passed immediately
-to the next section. During each section forward differencing is used to map
+to the next section. During each section, forward differencing is used to map
the trapezoid computed during the planning stage to a fifth-degree Bezier
-polynomial S-curve. The curve is used to find the next target position.
+polynomial S-curve. The curve is used to find the appropriate velocity at the
+next target position.
``_exec_aline_segment()`` is called for each non-zero section to convert the
computed target position to target steps by calling ``mp_kinematics()``. The
returns ``STAT_OK`` indicating the next segment should be loaded. When all
non-zero segments have been executed, the move is complete.
-## Stepper driver
-Calls to ``st_prep_line()`` prepare short (~5ms) moves for execution by the
-stepper driver. The move time in clock ticks is computed from travel in steps
-and the move duration. Then ``motor_prep_move()`` is called for each motor.
-``motor_prep_move()`` may perform step correction, enables the motors if needed.
-It then computes the optimal step clock divisor, clock ticks and sets the move
-direction, taking the motor's configuration in to account.
+## Pulse generation
+Calls to ``st_prep_line()`` prepare short (~5ms) move fragements for pluse
+generation by the stepper driver. Move time in clock ticks is computed from
+travel in steps and move duration. Then ``motor_prep_move()`` is called for
+each motor. ``motor_prep_move()`` may perform step correction and enable the
+motor. It then computes the optimal step clock divisor, clock ticks and sets
+the move direction, taking the motor's configuration in to account.
The stepper timer ISR, after ending the previous move, calls
``motor_load_move()`` on each motor. This sets up and starts the motor clocks,
static char *_cmd = 0;
-static void _estop() {estop_trigger(ESTOP_USER);}
-static void _clear() {estop_clear();}
-static void _pause() {mp_request_hold();}
-static void _run() {mp_request_start();}
-static void _step() {} // TODO
-static void _report() {report_request_full();}
-static void _reboot() {hw_request_hard_reset();}
+static void _estop() {estop_trigger(ESTOP_USER);}
+static void _clear() {estop_clear();}
+static void _pause() {mp_request_hold();}
+static void _opt_pause() {} // TODO
+static void _run() {mp_request_start();}
+static void _step() {} // TODO
+static void _report() {report_request_full();}
+static void _home() {} // TODO
+static void _reboot() {hw_request_hard_reset();}
static void command_i2c_cb(i2c_cmd_t cmd, uint8_t *data, uint8_t length) {
switch (cmd) {
- case I2C_ESTOP: _estop(); break;
- case I2C_CLEAR: _clear(); break;
- case I2C_PAUSE: _pause(); break;
- case I2C_RUN: _run(); break;
- case I2C_STEP: _step(); break;
- case I2C_REPORT: _report(); break;
- case I2C_HOME: break;
- case I2C_REBOOT: _reboot(); break;
+ case I2C_ESTOP: _estop(); break;
+ case I2C_CLEAR: _clear(); break;
+ case I2C_PAUSE: _pause(); break;
+ case I2C_OPTIONAL_PAUSE: _opt_pause(); break;
+ case I2C_RUN: _run(); break;
+ case I2C_STEP: _step(); break;
+ case I2C_REPORT: _report(); break;
+ case I2C_HOME: _home(); break;
+ case I2C_REBOOT: _reboot(); break;
default: break;
}
}
* executes commands - passing the stateless commands to the motion
* planning layer.
*
- * Synchronizing command execution
+ * Synchronizing command execution:
*
- * "Synchronous commands" are commands that affect the runtime need
- * to be synchronized with movement. Examples include G4 dwells,
- * program stops and ends, and most M commands. These are queued
- * into the planner queue and execute from the queue. Synchronous
- * commands work like this:
+ * "Synchronous commands" are commands that affect the runtime need
+ * to be synchronized with movement. Examples include G4 dwells,
+ * program stops and ends, and most M commands. These are queued
+ * into the planner queue and execute from the queue. Synchronous
+ * commands work like this:
*
- * - Call the mach_xxx_xxx() function which will do any input
- * validation and return an error if it detects one.
+ * - Call the mach_xxx_xxx() function which will do any input
+ * validation and return an error if it detects one.
*
- * - The mach_ function calls mp_queue_command(). Arguments are a
- * callback to the _exec_...() function, which is the runtime
- * execution routine, and any arguments that are needed by the
- * runtime. See typedef for *exec in planner.h for details
+ * - The mach_ function calls mp_queue_command(). Arguments are a
+ * callback to the _exec_...() function, which is the runtime
+ * execution routine, and any arguments that are needed by the
+ * runtime. See typedef for *exec in planner.h for details
*
- * - mp_queue_command() stores the callback and the args in a
- planner buffer.
+ * - mp_queue_command() stores the callback and the args in a
+ * planner buffer.
*
- * - When planner execution reaches the buffer it executes the
- * callback w/ the args. Take careful note that the callback
- * executes under an interrupt, so beware of variables that may
- * need to be volatile.
+ * - When planner execution reaches the buffer it executes the
+ * callback w/ the args. Take careful note that the callback
+ * executes under an interrupt, so beware of variables that may
+ * need to be volatile.
*
- * Note: - The synchronous command execution mechanism uses 2
- * vectors in the bf buffer to store and return values for the
- * callback. It's obvious, but impractical to pass the entire bf
- * buffer to the callback as some of these commands are actually
- * executed locally and have no buffer.
+ * Note: The synchronous command execution mechanism uses 2
+ * vectors in the bf buffer to store and return values for the
+ * callback. It's obvious, but impractical to pass the entire bf
+ * buffer to the callback as some of these commands are actually
+ * executed locally and have no buffer.
*/
#include "machine.h"
// Fall through
case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE:
- _energize(motor);
+ _energize(motor); // TODO is ~5ms enough time to enable the motor?
break;
case MOTOR_POWER_MODE_MAX_VALUE: break; // Shouldn't get here
* buffer once it has been queued. Action may start on the buffer immediately,
* invalidating its contents
*/
-void mp_commit_write_buffer(uint32_t line, moveType_t move_type) {
+void mp_commit_write_buffer(uint32_t line, moveType_t type) {
mp_state_running();
mb.q->ms.line = line;
- mb.q->move_type = move_type;
+ mb.q->move_type = type;
mb.q->move_state = MOVE_NEW;
mb.q->buffer_state = MP_BUFFER_QUEUED;
mb.q = mb.q->nx; // advance the queued buffer pointer
#include <stdbool.h>
-typedef enum { // bf->move_type values
+typedef enum {
MOVE_TYPE_NULL, // null move - does a no-op
MOVE_TYPE_ALINE, // acceleration planned line
MOVE_TYPE_DWELL, // delay with no movement
MP_BUFFER_QUEUED, // in queue
MP_BUFFER_PENDING, // marked as the next buffer to run
MP_BUFFER_RUNNING // current running buffer
-} mpBufferState_t;
+} bufferState_t;
// Callbacks
bf_func_t bf_func; // callback to buffer exec function
mach_exec_t mach_func; // callback to machine
- float naive_move_time;
-
- mpBufferState_t buffer_state; // used to manage queuing/dequeuing
+ bufferState_t buffer_state; // used to manage queuing/dequeuing
moveType_t move_type; // used to dispatch to run routine
- uint8_t move_code; // byte used by used exec functions
moveState_t move_state; // move state machine sequence
bool replannable; // true if move can be re-planned
float head_length;
float body_length;
float tail_length;
+
// See notes on these variables, in aline()
float entry_velocity; // entry velocity requested for the move
float cruise_velocity; // cruise velocity requested & achieved
float exit_velocity; // exit velocity requested for the move
+ float braking_velocity; // current value for braking velocity
float entry_vmax; // max junction velocity at entry of this move
float cruise_vmax; // max cruise velocity requested for move
float exit_vmax; // max exit velocity possible (redundant)
float delta_vmax; // max velocity difference for this move
- float braking_velocity; // current value for braking velocity
- uint8_t jerk_axis; // rate limiting axis used to compute jerk
float jerk; // maximum linear jerk term for this move
float recip_jerk; // 1/Jm used for planning (computed & cached)
float cbrt_jerk; // cube root of Jm (computed & cached)
void mp_wait_for_buffer();
void mp_init_buffers();
mpBuf_t *mp_get_write_buffer();
-void mp_commit_write_buffer(uint32_t line, moveType_t move_type);
+void mp_commit_write_buffer(uint32_t line, moveType_t type);
mpBuf_t *mp_get_run_buffer();
void mp_free_run_buffer();
mpBuf_t *mp_get_last_buffer();
return; // Shouldn't happen, buffer availability was checked upstream.
}
- bf->move_type = MOVE_TYPE_COMMAND;
bf->bf_func = _exec_command; // callback to planner queue exec function
bf->mach_func = mach_exec; // callback to machine exec function
#include <string.h>
#include <stdbool.h>
#include <math.h>
+#include <stdio.h>
/*** Segment runner
*
* Note that with our current control points, D and E are actually 0.
*/
-static void _init_forward_diffs(float Vi, float Vt) {
+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;
// E = 0
// F = Vi
- float h = 1 / mr.segments;
+ float h = 1 / segments;
float Ah_5 = A * h * h * h * h * h;
float Bh_4 = B * h * h * h * 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;
- mr.segment_velocity = half_Ah_5 + half_Bh_4 + half_Ch_3 + Vi;
+ return half_Ah_5 + half_Bh_4 + half_Ch_3 + Vi;
}
/// Common code for head and tail sections
-static stat_t _exec_aline_ends(float length, float entry_velocity,
- float exit_velocity) {
+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 / (entry_velocity + exit_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;
- _init_forward_diffs(entry_velocity, exit_velocity);
+ 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
mr.section_new = false;
- } else {
- mr.segment_velocity += mr.forward_diff[4];
+ } else { // Do subsequent segments
+ if (vin != vout) mr.segment_velocity += mr.forward_diff[4];
if (_exec_aline_segment() == STAT_OK) return STAT_OK;
- else {
+
+ 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];
/// Callback for tail section
static stat_t _exec_aline_tail() {
mr.section = SECTION_TAIL;
- return _exec_aline_ends(mr.tail_length, mr.cruise_velocity, mr.exit_velocity);
+ return
+ _exec_aline_section(mr.tail_length, mr.cruise_velocity, mr.exit_velocity);
}
-/// Callback for cruise section
+/// Callback for body section
static stat_t _exec_aline_body() {
- if (mr.section_new) {
- if (fp_ZERO(mr.body_length)) {
- mr.section = SECTION_TAIL;
- return _exec_aline_tail(); // skip to tail
- }
+ mr.section = SECTION_BODY;
- mr.ms.move_time = mr.body_length / mr.cruise_velocity;
- mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC);
- mr.segment_time = mr.ms.move_time / mr.segments;
- mr.segment_velocity = mr.cruise_velocity;
- mr.segment_count = mr.segments;
-
- if (mr.segment_time < MIN_SEGMENT_TIME)
- return STAT_MINIMUM_TIME_MOVE; // exit without advancing position
+ stat_t status =
+ _exec_aline_section(mr.body_length, mr.cruise_velocity, mr.cruise_velocity);
- mr.section = SECTION_BODY;
- mr.section_new = false;
- }
+ if (status == STAT_OK) {
+ if (mr.section_new) return _exec_aline_tail();
- if (_exec_aline_segment() == STAT_OK) { // OK means this section is done
mr.section = SECTION_TAIL;
mr.section_new = true;
+
+ return STAT_EAGAIN;
}
- return STAT_EAGAIN;
+ return status;
}
static stat_t _exec_aline_head() {
mr.section = SECTION_HEAD;
stat_t status =
- _exec_aline_ends(mr.head_length, mr.entry_velocity, mr.cruise_velocity);
+ _exec_aline_section(mr.head_length, mr.entry_velocity, mr.cruise_velocity);
if (status == STAT_OK) {
- mr.section = SECTION_BODY;
if (mr.section_new) return _exec_aline_body();
+
+ mr.section = SECTION_BODY;
mr.section_new = true;
+
+ return STAT_EAGAIN;
}
return status;
mr.section = SECTION_HEAD;
mr.section_new = true;
- mr.jerk = bf->jerk;
+ 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;
-
- copy_vector(mr.unit, bf->unit);
- copy_vector(mr.final_target, bf->ms.target);
+ mr.jerk = bf->jerk;
// Generate waypoints for position correction at section ends
for (int axis = 0; axis < AXES; axis++) {
case SECTION_HEAD: status = _exec_aline_head(); break;
case SECTION_BODY: status = _exec_aline_body(); break;
case SECTION_TAIL: status = _exec_aline_tail(); break;
- default: return CM_ALARM(STAT_INTERNAL_ERROR); // never supposed to get here
}
mp_state_hold_callback(status == STAT_OK);
* before or after motion stops.
*
* Terms used:
- * - mr is the runtime buffer. It was initially loaded from the bf
- * buffer
+ * - mr is the runtime buffer. It was initially loaded from the bf buffer
* - bp + 0 is the "companion" bf buffer to the mr buffer.
* - bp + 1 is the bf buffer following bp+0. This runs through bp + N
- * - bp (by itself) just refers to the current buffer being
- * adjusted / replanned
+ * - bp (by itself) refers to the current buffer being adjusted / replanned
*
* Details:
* Planning re-uses bp + 0 as an "extra" buffer. Normally bp + 0
* is returned to the buffer pool as it is redundant once mr is
- * loaded. Use the extra buffer to split the move in two where the
- * hold decelerates to zero. Use one buffer to go to zero, the other
- * to replan up from zero. All buffers past that point are
- * unaffected other than that they need to be replanned for
- * velocity.
+ * loaded. Use the extra buffer to split the move in two where the
+ * hold decelerates to zero. Use one buffer to go to zero, the other
+ * to replan up from zero. All buffers past that point are
+ * unaffected other than that they need to be replanned for velocity.
*
* Note:
* There are multiple opportunities for more efficient
// 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.
+ // that has not yet been computed. In the mean time, this hack will work.
if (mr_available_length < braking_length && fp_ZERO(bp->exit_velocity))
braking_length = mr_available_length;
braking_velocity = mr.exit_velocity; // adjust braking velocity downward
bp->move_state = MOVE_NEW; // tell _exec to re-use buffer
- // a safety to avoid wraparound
+ // A safety to avoid wraparound
for (int i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) {
mp_copy_buffer(bp, bp->nx); // copy bp+1 into bp+0, and onward
bp->length = braking_length;
bp->exit_vmax = 0;
- bp = mp_get_next_buffer(bp); // point to the acceleration buffer
+ bp = mp_get_next_buffer(bp); // point to the acceleration buffer
bp->entry_vmax = 0;
- bp->length -= braking_length; // identical buffers and hence their lengths
+ bp->length -= braking_length; // identical buffers, hence their lengths
bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp);
bp->exit_vmax = bp->delta_vmax;
- _reset_replannable_list(); // replan all the blocks
+ _reset_replannable_list(); // replan all the blocks
mp_plan_block_list(mp_get_last_buffer(), true);
mp_set_hold_state(FEEDHOLD_DECEL); // set state to decelerate
}
#include <math.h>
/// common variables for planning (move master)
-typedef struct mpMoveMasterSingleton {
+typedef struct {
float position[AXES]; // final move position for planning purposes
float jerk; // jerk values cached from previous block
float recip_jerk;
float cbrt_jerk;
-} mpMoveMasterSingleton_t;
+} mpMoveMaster_t;
-mpMoveMasterSingleton_t mm = {{0}}; // context for line planning
+mpMoveMaster_t mm = {{0}}; // context for line planning
/// Set planner position for a single axis
* approximation term, not the TinyG jerk terms)
*
* If you do the geometry in terms of the known variables, you get:
- * sin(theta/2) = R/(R+delta)
+ *
+ * sin(theta/2) = R / (R + delta)
+ *
* Re-arranging in terms of circle radius (R)
- * R = delta*sin(theta/2)/(1-sin(theta/2).
+ *
+ * R = delta * sin(theta/2) / (1 - sin(theta/2))
*
* Theta is the angle between line segments given by:
- * cos(theta) = dot(a,b)/(norm(a)*norm(b)).
+ *
+ * cos(theta) = dot(a, b) / (norm(a) * norm(b)).
*
* Most of these calculations are already done in the planner.
* To remove the acos() and sin() computations, use the trig half
* angle identity:
- * sin(theta/2) = +/- sqrt((1-cos(theta))/2).
+ *
+ * sin(theta/2) = +/-sqrt((1 - cos(theta)) / 2)
*
* For our applications, this should always be positive. Now just
* plug the equations into the centripetal acceleration equation:
* computations and no sine/cosines."
*
* How to compute the radius using brute-force trig:
+ *
* float theta = acos(costheta);
- * float radius = delta * sin(theta/2)/(1-sin(theta/2));
+ * float radius = delta * sin(theta/2) / (1 - sin(theta/2));
*
* This version extends Chamnit's algorithm by computing a value for
* delta that takes the contributions of the individual axes in the
*
* U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i])
* Usum Length of sums Ux + Uy
- * d Delta of sums (Dx*Ux+DY*UY)/Usum
+ * d Delta of sums (Dx * Ux + DY * UY) / Usum
*/
static float _get_junction_vmax(const float a_unit[], const float b_unit[]) {
- float costheta =
- -a_unit[AXIS_X] * b_unit[AXIS_X] -
- a_unit[AXIS_Y] * b_unit[AXIS_Y] -
- a_unit[AXIS_Z] * b_unit[AXIS_Z] -
- a_unit[AXIS_A] * b_unit[AXIS_A] -
- a_unit[AXIS_B] * b_unit[AXIS_B] -
- a_unit[AXIS_C] * b_unit[AXIS_C];
+ float costheta = 0;
+ for (int axis = 0; axis < AXES; axis++)
+ costheta -= a_unit[axis] * b_unit[axis];
if (costheta < -0.99) return 10000000; // straight line cases
if (costheta > 0.99) return 0; // reversal cases
// Fuse the junction deviations into a vector sum
- float a_delta = square(a_unit[AXIS_X] * mach.a[AXIS_X].junction_dev);
- a_delta += square(a_unit[AXIS_Y] * mach.a[AXIS_Y].junction_dev);
- a_delta += square(a_unit[AXIS_Z] * mach.a[AXIS_Z].junction_dev);
- a_delta += square(a_unit[AXIS_A] * mach.a[AXIS_A].junction_dev);
- a_delta += square(a_unit[AXIS_B] * mach.a[AXIS_B].junction_dev);
- a_delta += square(a_unit[AXIS_C] * mach.a[AXIS_C].junction_dev);
-
- float b_delta = square(b_unit[AXIS_X] * mach.a[AXIS_X].junction_dev);
- b_delta += square(b_unit[AXIS_Y] * mach.a[AXIS_Y].junction_dev);
- b_delta += square(b_unit[AXIS_Z] * mach.a[AXIS_Z].junction_dev);
- b_delta += square(b_unit[AXIS_A] * mach.a[AXIS_A].junction_dev);
- b_delta += square(b_unit[AXIS_B] * mach.a[AXIS_B].junction_dev);
- b_delta += square(b_unit[AXIS_C] * mach.a[AXIS_C].junction_dev);
+ float a_delta = 0;
+ float b_delta = 0;
+
+ for (int axis = 0; axis < AXES; axis++) {
+ a_delta += square(a_unit[axis] * mach.a[axis].junction_dev);
+ b_delta += square(b_unit[axis] * mach.a[axis].junction_dev);
+ }
float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2;
float sintheta_over2 = sqrt((1 - costheta) / 2);
float C; // contribution term. C = T * a
float maxC = 0;
float recip_L2 = 1 / length_square;
+ int jerk_axis = 0;
for (int axis = 0; axis < AXES; axis++)
// You cannot use the fp_XXX comparisons here!
if (C > maxC) {
maxC = C;
- bf->jerk_axis = axis; // also needed for junction vmax calculation
+ jerk_axis = axis; // also needed for junction vmax calculation
}
}
// set up and pre-compute the jerk terms needed for this round of planning
- bf->jerk = mach.a[bf->jerk_axis].jerk_max * JERK_MULTIPLIER /
- fabs(bf->unit[bf->jerk_axis]); // scale jerk
+ bf->jerk = mach.a[jerk_axis].jerk_max * JERK_MULTIPLIER /
+ fabs(bf->unit[jerk_axis]); // scale jerk
// specialized comparison for tolerance of delta
if (fabs(bf->jerk - mm.jerk) > JERK_MATCH_PRECISION) {
// before committing the buffer.
mp_plan_block_list(bf, false); // replan block list
copy_vector(mm.position, bf->ms.target); // set the planner position
+
// commit current block (must follow the position update)
mp_commit_write_buffer(ms->line, MOVE_TYPE_ALINE);
// velocity you can get given the delta_vmax (maximum velocity slew)
// supportable.
- bf->naive_move_time =
+ float naive_move_time =
2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average
- if (bf->naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) {
+ if (naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) {
bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN;
bf->exit_velocity = max(0.0, min(bf->cruise_velocity,
(bf->entry_velocity - bf->delta_vmax)));
}
// B" case: Block is short, but fits into a single body segment
- if (bf->naive_move_time <= NOM_SEGMENT_TIME) {
+ if (naive_move_time <= NOM_SEGMENT_TIME) {
bf->entry_velocity = bf->pv->exit_velocity;
if (fp_NOT_ZERO(bf->entry_velocity)) {
* planned becomes the effective first block.
*
* mp_plan_block_list() plans all blocks between and including the
- * (effective) first block and the bf. It sets entry, exit and
+ * (effective) first block and the bf. It sets entry, exit and
* cruise v's from vmax's then calls trapezoid generation.
*
* Variables that must be provided in the mpBuffers that will be processed:
mpBuf_t *bp = bf;
// Backward planning pass. Find first block and update braking velocities.
- // At the end *bp points to the buffer before the first block.
+ // By the end bp points to the buffer before the first block.
while ((bp = mp_get_prev_buffer(bp)) != bf) {
if (!bp->replannable) break;
bp->braking_velocity =
min(bp->nx->entry_vmax, bp->nx->braking_velocity) + bp->delta_vmax;
}
- // Forward planning pass - recomputes trapezoids in the list from the first
- // block to the bf block.
+ // Forward planning pass. Recompute trapezoids from the first block to bf.
while ((bp = mp_get_next_buffer(bp)) != bf) {
if (bp->pv == bf || mr_flag) {
- bp->entry_velocity = bp->entry_vmax; // first block in the list
+ bp->entry_velocity = bp->entry_vmax; // first block
mr_flag = false;
- } else bp->entry_velocity = bp->pv->exit_velocity; // other blocks in list
+ } else bp->entry_velocity = bp->pv->exit_velocity; // other blocks
bp->cruise_velocity = bp->cruise_vmax;
bp->exit_velocity = min4(bp->exit_vmax, bp->nx->entry_vmax,
mp_calculate_trapezoid(bp);
- // test for optimally planned trapezoids - only need to check various exit
- // conditions
+ // Test for optimally planned trapezoids by checking exit conditions
if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) ||
fp_EQ(bp->exit_velocity, bp->nx->entry_vmax)) ||
(!bp->pv->replannable &&
bp->replannable = false;
}
- // finish up the last block move
+ // Finish last block
bp->entry_velocity = bp->pv->exit_velocity;
bp->cruise_velocity = bp->cruise_vmax;
bp->exit_velocity = 0;
+
mp_calculate_trapezoid(bp);
}
SECTION_HEAD, // acceleration
SECTION_BODY, // cruise
SECTION_TAIL, // deceleration
- SECTIONS // section count
} moveSection_t;
/// current move position
float position[AXES];
/// head/body/tail endpoints for correction
- float waypoint[SECTIONS][AXES];
+ float waypoint[3][AXES];
/// current MR target (absolute target as steps)
float target_steps[MOTORS];
/// current MR position (target from previous segment)
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 jerk; // max linear jerk
float forward_diff[5]; // forward difference levels
MoveState_t ms;
projects / bbctrl-firmware / commitdiff