bf37ab7e7b
- All pins, which include limits, control command, and probe pins, can now all be configured to trigger as active-low or active-high and whether the pin has its internal pull-up resistor enabled. This should allow for just about all types of NO and NC switch configurations. - The probe pin invert setting hasn’t been added to the Grbl settings, like the others, and will have to wait until v1.0. But for now, it’s available as a compile-time option in config.h. - Fixed a variable spindle bug.
461 lines
24 KiB
C
461 lines
24 KiB
C
/*
|
|
planner.c - buffers movement commands and manages the acceleration profile plan
|
|
Part of Grbl v0.9
|
|
|
|
Copyright (c) 2012-2015 Sungeun K. Jeon
|
|
|
|
Grbl is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
Grbl 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 General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with Grbl. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
/*
|
|
This file is based on work from Grbl v0.8, distributed under the
|
|
terms of the MIT-license. See COPYING for more details.
|
|
Copyright (c) 2009-2011 Simen Svale Skogsrud
|
|
Copyright (c) 2011-2012 Sungeun K. Jeon
|
|
Copyright (c) 2011 Jens Geisler
|
|
*/
|
|
|
|
#include "system.h"
|
|
#include "planner.h"
|
|
#include "protocol.h"
|
|
#include "stepper.h"
|
|
#include "settings.h"
|
|
|
|
|
|
#define SOME_LARGE_VALUE 1.0E+38 // Used by rapids and acceleration maximization calculations. Just needs
|
|
// to be larger than any feasible (mm/min)^2 or mm/sec^2 value.
|
|
|
|
static plan_block_t block_buffer[BLOCK_BUFFER_SIZE]; // A ring buffer for motion instructions
|
|
static uint8_t block_buffer_tail; // Index of the block to process now
|
|
static uint8_t block_buffer_head; // Index of the next block to be pushed
|
|
static uint8_t next_buffer_head; // Index of the next buffer head
|
|
static uint8_t block_buffer_planned; // Index of the optimally planned block
|
|
|
|
// Define planner variables
|
|
typedef struct {
|
|
int32_t position[N_AXIS]; // The planner position of the tool in absolute steps. Kept separate
|
|
// from g-code position for movements requiring multiple line motions,
|
|
// i.e. arcs, canned cycles, and backlash compensation.
|
|
float previous_unit_vec[N_AXIS]; // Unit vector of previous path line segment
|
|
float previous_nominal_speed_sqr; // Nominal speed of previous path line segment
|
|
} planner_t;
|
|
static planner_t pl;
|
|
|
|
|
|
// Returns the index of the next block in the ring buffer. Also called by stepper segment buffer.
|
|
uint8_t plan_next_block_index(uint8_t block_index)
|
|
{
|
|
block_index++;
|
|
if (block_index == BLOCK_BUFFER_SIZE) { block_index = 0; }
|
|
return(block_index);
|
|
}
|
|
|
|
|
|
// Returns the index of the previous block in the ring buffer
|
|
static uint8_t plan_prev_block_index(uint8_t block_index)
|
|
{
|
|
if (block_index == 0) { block_index = BLOCK_BUFFER_SIZE; }
|
|
block_index--;
|
|
return(block_index);
|
|
}
|
|
|
|
|
|
/* PLANNER SPEED DEFINITION
|
|
+--------+ <- current->nominal_speed
|
|
/ \
|
|
current->entry_speed -> + \
|
|
| + <- next->entry_speed (aka exit speed)
|
|
+-------------+
|
|
time -->
|
|
|
|
Recalculates the motion plan according to the following basic guidelines:
|
|
|
|
1. Go over every feasible block sequentially in reverse order and calculate the junction speeds
|
|
(i.e. current->entry_speed) such that:
|
|
a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of
|
|
neighboring blocks.
|
|
b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed)
|
|
with a maximum allowable deceleration over the block travel distance.
|
|
c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero).
|
|
2. Go over every block in chronological (forward) order and dial down junction speed values if
|
|
a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable
|
|
acceleration over the block travel distance.
|
|
|
|
When these stages are complete, the planner will have maximized the velocity profiles throughout the all
|
|
of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In
|
|
other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements
|
|
are possible. If a new block is added to the buffer, the plan is recomputed according to the said
|
|
guidelines for a new optimal plan.
|
|
|
|
To increase computational efficiency of these guidelines, a set of planner block pointers have been
|
|
created to indicate stop-compute points for when the planner guidelines cannot logically make any further
|
|
changes or improvements to the plan when in normal operation and new blocks are streamed and added to the
|
|
planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are
|
|
bracketed by junction velocities at their maximums (or by the first planner block as well), no new block
|
|
added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute
|
|
them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute
|
|
point) are all accelerating, they are all optimal and can not be altered by a new block added to the
|
|
planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum
|
|
junction velocity is reached. However, if the operational conditions of the plan changes from infrequently
|
|
used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is
|
|
recomputed as stated in the general guidelines.
|
|
|
|
Planner buffer index mapping:
|
|
- block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.
|
|
- block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether
|
|
the buffer is full or empty. As described for standard ring buffers, this block is always empty.
|
|
- next_buffer_head: Points to next planner buffer block after the buffer head block. When equal to the
|
|
buffer tail, this indicates the buffer is full.
|
|
- block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal
|
|
streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the
|
|
planner buffer that don't change with the addition of a new block, as describe above. In addition,
|
|
this block can never be less than block_buffer_tail and will always be pushed forward and maintain
|
|
this requirement when encountered by the plan_discard_current_block() routine during a cycle.
|
|
|
|
NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short
|
|
line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't
|
|
enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then
|
|
decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and
|
|
becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner
|
|
will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line
|
|
motion(s) distance per block to a desired tolerance. The more combined distance the planner has to use,
|
|
the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance
|
|
for the planner to compute over. It also increases the number of computations the planner has to perform
|
|
to compute an optimal plan, so select carefully. The Arduino 328p memory is already maxed out, but future
|
|
ARM versions should have enough memory and speed for look-ahead blocks numbering up to a hundred or more.
|
|
|
|
*/
|
|
static void planner_recalculate()
|
|
{
|
|
// Initialize block index to the last block in the planner buffer.
|
|
uint8_t block_index = plan_prev_block_index(block_buffer_head);
|
|
|
|
// Bail. Can't do anything with one only one plan-able block.
|
|
if (block_index == block_buffer_planned) { return; }
|
|
|
|
// Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
|
|
// block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
|
|
// NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
|
|
float entry_speed_sqr;
|
|
plan_block_t *next;
|
|
plan_block_t *current = &block_buffer[block_index];
|
|
|
|
// Calculate maximum entry speed for last block in buffer, where the exit speed is always zero.
|
|
current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters);
|
|
|
|
block_index = plan_prev_block_index(block_index);
|
|
if (block_index == block_buffer_planned) { // Only two plannable blocks in buffer. Reverse pass complete.
|
|
// Check if the first block is the tail. If so, notify stepper to update its current parameters.
|
|
if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); }
|
|
} else { // Three or more plan-able blocks
|
|
while (block_index != block_buffer_planned) {
|
|
next = current;
|
|
current = &block_buffer[block_index];
|
|
block_index = plan_prev_block_index(block_index);
|
|
|
|
// Check if next block is the tail block(=planned block). If so, update current stepper parameters.
|
|
if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); }
|
|
|
|
// Compute maximum entry speed decelerating over the current block from its exit speed.
|
|
if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
|
|
entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
|
|
if (entry_speed_sqr < current->max_entry_speed_sqr) {
|
|
current->entry_speed_sqr = entry_speed_sqr;
|
|
} else {
|
|
current->entry_speed_sqr = current->max_entry_speed_sqr;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
|
|
// Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
|
|
next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer
|
|
block_index = plan_next_block_index(block_buffer_planned);
|
|
while (block_index != block_buffer_head) {
|
|
current = next;
|
|
next = &block_buffer[block_index];
|
|
|
|
// Any acceleration detected in the forward pass automatically moves the optimal planned
|
|
// pointer forward, since everything before this is all optimal. In other words, nothing
|
|
// can improve the plan from the buffer tail to the planned pointer by logic.
|
|
if (current->entry_speed_sqr < next->entry_speed_sqr) {
|
|
entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
|
|
// If true, current block is full-acceleration and we can move the planned pointer forward.
|
|
if (entry_speed_sqr < next->entry_speed_sqr) {
|
|
next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
|
|
block_buffer_planned = block_index; // Set optimal plan pointer.
|
|
}
|
|
}
|
|
|
|
// Any block set at its maximum entry speed also creates an optimal plan up to this
|
|
// point in the buffer. When the plan is bracketed by either the beginning of the
|
|
// buffer and a maximum entry speed or two maximum entry speeds, every block in between
|
|
// cannot logically be further improved. Hence, we don't have to recompute them anymore.
|
|
if (next->entry_speed_sqr == next->max_entry_speed_sqr) { block_buffer_planned = block_index; }
|
|
block_index = plan_next_block_index( block_index );
|
|
}
|
|
}
|
|
|
|
|
|
void plan_reset()
|
|
{
|
|
memset(&pl, 0, sizeof(pl)); // Clear planner struct
|
|
block_buffer_tail = 0;
|
|
block_buffer_head = 0; // Empty = tail
|
|
next_buffer_head = 1; // plan_next_block_index(block_buffer_head)
|
|
block_buffer_planned = 0; // = block_buffer_tail;
|
|
}
|
|
|
|
|
|
void plan_discard_current_block()
|
|
{
|
|
if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer.
|
|
uint8_t block_index = plan_next_block_index( block_buffer_tail );
|
|
// Push block_buffer_planned pointer, if encountered.
|
|
if (block_buffer_tail == block_buffer_planned) { block_buffer_planned = block_index; }
|
|
block_buffer_tail = block_index;
|
|
}
|
|
}
|
|
|
|
|
|
plan_block_t *plan_get_current_block()
|
|
{
|
|
if (block_buffer_head == block_buffer_tail) { return(NULL); } // Buffer empty
|
|
return(&block_buffer[block_buffer_tail]);
|
|
}
|
|
|
|
|
|
float plan_get_exec_block_exit_speed()
|
|
{
|
|
uint8_t block_index = plan_next_block_index(block_buffer_tail);
|
|
if (block_index == block_buffer_head) { return( 0.0 ); }
|
|
return( sqrt( block_buffer[block_index].entry_speed_sqr ) );
|
|
}
|
|
|
|
|
|
// Returns the availability status of the block ring buffer. True, if full.
|
|
uint8_t plan_check_full_buffer()
|
|
{
|
|
if (block_buffer_tail == next_buffer_head) { return(true); }
|
|
return(false);
|
|
}
|
|
|
|
|
|
/* Add a new linear movement to the buffer. target[N_AXIS] is the signed, absolute target position
|
|
in millimeters. Feed rate specifies the speed of the motion. If feed rate is inverted, the feed
|
|
rate is taken to mean "frequency" and would complete the operation in 1/feed_rate minutes.
|
|
All position data passed to the planner must be in terms of machine position to keep the planner
|
|
independent of any coordinate system changes and offsets, which are handled by the g-code parser.
|
|
NOTE: Assumes buffer is available. Buffer checks are handled at a higher level by motion_control.
|
|
In other words, the buffer head is never equal to the buffer tail. Also the feed rate input value
|
|
is used in three ways: as a normal feed rate if invert_feed_rate is false, as inverse time if
|
|
invert_feed_rate is true, or as seek/rapids rate if the feed_rate value is negative (and
|
|
invert_feed_rate always false). */
|
|
#ifdef USE_LINE_NUMBERS
|
|
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate, int32_t line_number)
|
|
#else
|
|
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
|
|
#endif
|
|
{
|
|
// Prepare and initialize new block
|
|
plan_block_t *block = &block_buffer[block_buffer_head];
|
|
block->step_event_count = 0;
|
|
block->millimeters = 0;
|
|
block->direction_bits = 0;
|
|
block->acceleration = SOME_LARGE_VALUE; // Scaled down to maximum acceleration later
|
|
#ifdef USE_LINE_NUMBERS
|
|
block->line_number = line_number;
|
|
#endif
|
|
|
|
// Compute and store initial move distance data.
|
|
// TODO: After this for-loop, we don't touch the stepper algorithm data. Might be a good idea
|
|
// to try to keep these types of things completely separate from the planner for portability.
|
|
int32_t target_steps[N_AXIS];
|
|
float unit_vec[N_AXIS], delta_mm;
|
|
uint8_t idx;
|
|
#ifdef COREXY
|
|
target_steps[A_MOTOR] = lround(target[A_MOTOR]*settings.steps_per_mm[A_MOTOR]);
|
|
target_steps[B_MOTOR] = lround(target[B_MOTOR]*settings.steps_per_mm[B_MOTOR]);
|
|
block->steps[A_MOTOR] = labs((target_steps[X_AXIS]-pl.position[X_AXIS]) - (target_steps[Y_AXIS]-pl.position[Y_AXIS]));
|
|
block->steps[B_MOTOR] = labs((target_steps[X_AXIS]-pl.position[X_AXIS]) + (target_steps[Y_AXIS]-pl.position[Y_AXIS]));
|
|
#endif
|
|
|
|
for (idx=0; idx<N_AXIS; idx++) {
|
|
// Calculate target position in absolute steps, number of steps for each axis, and determine max step events.
|
|
// Also, compute individual axes distance for move and prep unit vector calculations.
|
|
// NOTE: Computes true distance from converted step values.
|
|
#ifdef COREXY
|
|
if ( !(idx == A_MOTOR) && !(idx == B_MOTOR) ) {
|
|
target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
|
|
block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
|
|
}
|
|
block->step_event_count = max(block->step_event_count, block->steps[idx]);
|
|
if (idx == A_MOTOR) {
|
|
delta_mm = ((target_steps[X_AXIS]-pl.position[X_AXIS]) - (target_steps[Y_AXIS]-pl.position[Y_AXIS]))/settings.steps_per_mm[idx];
|
|
} else if (idx == B_MOTOR) {
|
|
delta_mm = ((target_steps[X_AXIS]-pl.position[X_AXIS]) + (target_steps[Y_AXIS]-pl.position[Y_AXIS]))/settings.steps_per_mm[idx];
|
|
} else {
|
|
delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];
|
|
}
|
|
#else
|
|
target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
|
|
block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
|
|
block->step_event_count = max(block->step_event_count, block->steps[idx]);
|
|
delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];
|
|
#endif
|
|
unit_vec[idx] = delta_mm; // Store unit vector numerator. Denominator computed later.
|
|
|
|
// Set direction bits. Bit enabled always means direction is negative.
|
|
if (delta_mm < 0 ) { block->direction_bits |= get_direction_pin_mask(idx); }
|
|
|
|
// Incrementally compute total move distance by Euclidean norm. First add square of each term.
|
|
block->millimeters += delta_mm*delta_mm;
|
|
}
|
|
block->millimeters = sqrt(block->millimeters); // Complete millimeters calculation with sqrt()
|
|
|
|
// Bail if this is a zero-length block. Highly unlikely to occur.
|
|
if (block->step_event_count == 0) { return; }
|
|
|
|
// Adjust feed_rate value to mm/min depending on type of rate input (normal, inverse time, or rapids)
|
|
// TODO: Need to distinguish a rapids vs feed move for overrides. Some flag of some sort.
|
|
if (feed_rate < 0) { feed_rate = SOME_LARGE_VALUE; } // Scaled down to absolute max/rapids rate later
|
|
else if (invert_feed_rate) { feed_rate = block->millimeters/feed_rate; }
|
|
if (feed_rate < MINIMUM_FEED_RATE) { feed_rate = MINIMUM_FEED_RATE; } // Prevents step generation round-off condition.
|
|
|
|
// Calculate the unit vector of the line move and the block maximum feed rate and acceleration scaled
|
|
// down such that no individual axes maximum values are exceeded with respect to the line direction.
|
|
// NOTE: This calculation assumes all axes are orthogonal (Cartesian) and works with ABC-axes,
|
|
// if they are also orthogonal/independent. Operates on the absolute value of the unit vector.
|
|
float inverse_unit_vec_value;
|
|
float inverse_millimeters = 1.0/block->millimeters; // Inverse millimeters to remove multiple float divides
|
|
float junction_cos_theta = 0;
|
|
for (idx=0; idx<N_AXIS; idx++) {
|
|
if (unit_vec[idx] != 0) { // Avoid divide by zero.
|
|
unit_vec[idx] *= inverse_millimeters; // Complete unit vector calculation
|
|
inverse_unit_vec_value = fabs(1.0/unit_vec[idx]); // Inverse to remove multiple float divides.
|
|
|
|
// Check and limit feed rate against max individual axis velocities and accelerations
|
|
feed_rate = min(feed_rate,settings.max_rate[idx]*inverse_unit_vec_value);
|
|
block->acceleration = min(block->acceleration,settings.acceleration[idx]*inverse_unit_vec_value);
|
|
|
|
// Incrementally compute cosine of angle between previous and current path. Cos(theta) of the junction
|
|
// between the current move and the previous move is simply the dot product of the two unit vectors,
|
|
// where prev_unit_vec is negative. Used later to compute maximum junction speed.
|
|
junction_cos_theta -= pl.previous_unit_vec[idx] * unit_vec[idx];
|
|
}
|
|
}
|
|
|
|
// TODO: Need to check this method handling zero junction speeds when starting from rest.
|
|
if (block_buffer_head == block_buffer_tail) {
|
|
|
|
// Initialize block entry speed as zero. Assume it will be starting from rest. Planner will correct this later.
|
|
block->entry_speed_sqr = 0.0;
|
|
block->max_junction_speed_sqr = 0.0; // Starting from rest. Enforce start from zero velocity.
|
|
|
|
} else {
|
|
/*
|
|
Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
|
|
Let a circle be tangent to both previous and current path line segments, where the junction
|
|
deviation is defined as the distance from the junction to the closest edge of the circle,
|
|
colinear with the circle center. The circular segment joining the two paths represents the
|
|
path of centripetal acceleration. Solve for max velocity based on max acceleration about the
|
|
radius of the circle, defined indirectly by junction deviation. This may be also viewed as
|
|
path width or max_jerk in the previous Grbl version. This approach does not actually deviate
|
|
from path, but used as a robust way to compute cornering speeds, as it takes into account the
|
|
nonlinearities of both the junction angle and junction velocity.
|
|
|
|
NOTE: If the junction deviation value is finite, Grbl executes the motions in an exact path
|
|
mode (G61). If the junction deviation value is zero, Grbl will execute the motion in an exact
|
|
stop mode (G61.1) manner. In the future, if continuous mode (G64) is desired, the math here
|
|
is exactly the same. Instead of motioning all the way to junction point, the machine will
|
|
just follow the arc circle defined here. The Arduino doesn't have the CPU cycles to perform
|
|
a continuous mode path, but ARM-based microcontrollers most certainly do.
|
|
|
|
NOTE: The max junction speed is a fixed value, since machine acceleration limits cannot be
|
|
changed dynamically during operation nor can the line move geometry. This must be kept in
|
|
memory in the event of a feedrate override changing the nominal speeds of blocks, which can
|
|
change the overall maximum entry speed conditions of all blocks.
|
|
*/
|
|
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
|
|
float sin_theta_d2 = sqrt(0.5*(1.0-junction_cos_theta)); // Trig half angle identity. Always positive.
|
|
|
|
// TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the
|
|
// two junctions. However, this shouldn't be a significant problem except in extreme circumstances.
|
|
block->max_junction_speed_sqr = max( MINIMUM_JUNCTION_SPEED*MINIMUM_JUNCTION_SPEED,
|
|
(block->acceleration * settings.junction_deviation * sin_theta_d2)/(1.0-sin_theta_d2) );
|
|
}
|
|
|
|
// Store block nominal speed
|
|
block->nominal_speed_sqr = feed_rate*feed_rate; // (mm/min). Always > 0
|
|
|
|
// Compute the junction maximum entry based on the minimum of the junction speed and neighboring nominal speeds.
|
|
block->max_entry_speed_sqr = min(block->max_junction_speed_sqr,
|
|
min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr));
|
|
|
|
// Update previous path unit_vector and nominal speed (squared)
|
|
memcpy(pl.previous_unit_vec, unit_vec, sizeof(unit_vec)); // pl.previous_unit_vec[] = unit_vec[]
|
|
pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;
|
|
|
|
// Update planner position
|
|
memcpy(pl.position, target_steps, sizeof(target_steps)); // pl.position[] = target_steps[]
|
|
|
|
// New block is all set. Update buffer head and next buffer head indices.
|
|
block_buffer_head = next_buffer_head;
|
|
next_buffer_head = plan_next_block_index(block_buffer_head);
|
|
|
|
// Finish up by recalculating the plan with the new block.
|
|
planner_recalculate();
|
|
}
|
|
|
|
|
|
// Reset the planner position vectors. Called by the system abort/initialization routine.
|
|
void plan_sync_position()
|
|
{
|
|
// TODO: For motor configurations not in the same coordinate frame as the machine position,
|
|
// this function needs to be updated to accomodate the difference.
|
|
uint8_t idx;
|
|
for (idx=0; idx<N_AXIS; idx++) {
|
|
#ifdef COREXY
|
|
if (idx==A_MOTOR) {
|
|
pl.position[idx] = (sys.position[A_MOTOR] + sys.position[B_MOTOR])/2;
|
|
} else if (idx==B_MOTOR) {
|
|
pl.position[idx] = (sys.position[A_MOTOR] - sys.position[B_MOTOR])/2;
|
|
} else {
|
|
pl.position[idx] = sys.position[idx];
|
|
}
|
|
#else
|
|
pl.position[idx] = sys.position[idx];
|
|
#endif
|
|
}
|
|
}
|
|
|
|
|
|
// Returns the number of active blocks are in the planner buffer.
|
|
uint8_t plan_get_block_buffer_count()
|
|
{
|
|
if (block_buffer_head >= block_buffer_tail) { return(block_buffer_head-block_buffer_tail); }
|
|
return(BLOCK_BUFFER_SIZE - (block_buffer_tail-block_buffer_head));
|
|
}
|
|
|
|
|
|
// Re-initialize buffer plan with a partially completed block, assumed to exist at the buffer tail.
|
|
// Called after a steppers have come to a complete stop for a feed hold and the cycle is stopped.
|
|
void plan_cycle_reinitialize()
|
|
{
|
|
// Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer.
|
|
st_update_plan_block_parameters();
|
|
block_buffer_planned = block_buffer_tail;
|
|
planner_recalculate();
|
|
}
|