ManuelW/grbl-LPC-CoreXY
1
0
Fork 0
Code Issues Pull Requests Releases 1 Wiki Activity

Planner function call fix. More clean up.

Browse Source
This commit is contained in:
Sonny Jeon 2013-10-29 18:55:55 -06:00
parent 27297d444b
commit 0ac6c87196
3 changed files with 115 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

172
planner.c
View File

@ -148,34 +148,18 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr)
recomputed as stated in the general guidelines.
Planner buffer index mapping:
- block_buffer_head: Points to the newest incoming buffer block just added by plan_buffer_line(). The
planner never touches the exit speed of this block, which always defaults to 0.
- block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.
Can dynamically change with the old stepper algorithm, but with the new algorithm, this should be impossible
as long as the segment buffer is not empty.
- next_buffer_head: Points to next planner buffer block after the last block. Should always be empty.
- block_buffer_safe: Points to the first planner block in the buffer for which it is safe to change. Since
the stepper can be executing the first block and if the planner changes its conditions, this will cause
a discontinuity and error in the stepper profile with lost steps likely. With the new stepper algorithm,
the block_buffer_safe is always where the stepper segment buffer ends and can never be overwritten, but
this can change the state of the block profile from a pure trapezoid assumption. Meaning, if that block
is decelerating, the planner conditions can change such that the block can new accelerate mid-block.
!!! I need to make sure that the stepper algorithm can modify the acceleration mid-block. Needed for feedrate overrides too.
!!! planner_recalculate() may not work correctly with re-planning.... may need to artificially set both the
block_buffer_head and next_buffer_head back one index so that this works correctly, or allow the operation
of this function to accept two different conditions to operate on.
- block_buffer_planned: Points to the first buffer block after the last optimally fixed block, which can no longer be
improved. This block and the trailing buffer blocks that can still be altered when new blocks are added. This planned
block points to the transition point between the fixed and non-fixed states and is handled slightly different. The entry
speed is fixed, indicating the reverse pass cannot maximize the speed further, but the velocity profile within it
can still be changed, meaning the forward pass calculations must start from here and influence the following block
entry speed.
!!! Need to check if this is the start of the non-optimal or the end of the optimal block.
- 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_safe: Points to the first sequential planner block for which it is safe to recompute, which
is defined to be where the stepper's step segment buffer ends. This may or may not be the buffer tail,
since the step segment buffer queues steps which may have not finished executing and could span a few
blocks, if the block moves are very short.
- 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.
NOTE: All planner computations are performed in floating point to minimize numerical round-off errors.
When a planner block is executed, the floating point values are converted to fast integers by the stepper
@ -198,7 +182,7 @@ static void planner_recalculate()
{
// Initialize block index to the last block in the planner buffer.
uint8_t block_index = prev_block_index(block_buffer_head);
uint8_t block_index = plan_prev_block_index(block_buffer_head);
// Query stepper module for safe planner block index to recalculate to, which corresponds to the end
// of the step segment buffer.
@ -324,58 +308,6 @@ static void planner_recalculate()
}
/*
uint8_t block_buffer_safe = st_get_prep_block_index();
if (block_buffer_head == block_buffer_safe) { // Also catches head = tail
block_buffer_planned = block_buffer_head;
} else {
uint8_t block_index = block_buffer_head;
plan_block_t *current = &block_buffer[block_index];
current->entry_speed_sqr = min( current->max_entry_speed_sqr, 2*current->acceleration*current->millimeters);
float entry_speed_sqr;
plan_block_t *next;
block_index = plan_prev_block_index(block_index);
if (block_index == block_buffer_safe) { // !! OR plan pointer? Yes I think so.
plan_update_partial_block(block_index, 0.0);
block_buffer_planned = block_index;
} else {
while (block_index != block_buffer_planned) {
next = current;
current = &block_buffer[block_index];
block_index = plan_prev_block_index(block_index);
if (block_index == block_buffer_safe) {
plan_update_partial_block(block_index,current->entry_speed_sqr);
block_buffer_planned = block_index;
}
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;
}
}
}
}
next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer
block_index = plan_next_block_index(block_buffer_planned);
while (block_index != next_buffer_head) {
current = next;
next = &block_buffer[block_index];
if (current->entry_speed_sqr < next->entry_speed_sqr) {
entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
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.
}
}
if (next->entry_speed_sqr == next->max_entry_speed_sqr) {
block_buffer_planned = block_index; // Set optimal plan pointer
}
block_index = plan_next_block_index( block_index );
}
} */
}
@ -439,16 +371,16 @@ void plan_synchronize()
}
// 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).
/* 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). */
void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
{
// Prepare and initialize new block
@ -675,3 +607,63 @@ void plan_cycle_reinitialize(int32_t step_events_remaining)
block_buffer_planned = block_buffer_tail;
planner_recalculate();
}
/*
TODO:
When a feed hold or feedrate override is reduced, the velocity profile must execute a
deceleration over the existing plan. By logic, since the plan already decelerates to zero
at the end of the buffer, any replanned deceleration mid-way will never exceed this. It
will only asymptotically approach this in the worst case scenario.
- For a feed hold, we simply need to plan and compute the stopping point within a block
when velocity decelerates to zero. We then can recompute the plan with the already
existing partial block planning code and set the system to a QUEUED state.
- When a feed hold is initiated, the main program should be able to continue doing what
it has been, i.e. arcs, parsing, but needs to be able to reinitialize the plan after
it has come to a stop.
- For a feed rate override (reduce-only), we need to enforce a deceleration until we
intersect the reduced nominal speed of a block after it's been planned with the new
overrides and the newly planned block is accelerating or cruising only. If the new plan
block is decelerating at the intersection point, we keep decelerating until we find a
valid intersection point. Once we find this point, we can then resume onto the new plan,
but we may need to adjust the deceleration point in the intersection block since the
feedrate override could have intersected at an acceleration ramp. This would change the
acceleration ramp to a cruising, so the deceleration point will have changed, but the
plan will have not. It should still be valid for the rest of the buffer. Coding this
can get complicated, but it should be doable. One issue could be is in how to handle
scenarios when a user issues several feedrate overrides and inundates this code. Does
this method still work and is robust enough to compute all of this on the fly? This is
the critical question. However, we could block user input until the planner has time to
catch to solve this as well.
- When the feed rate override increases, we don't have to do anything special. We just
replan the entire buffer with the new nominal speeds and adjust the maximum junction
speeds accordingly.
void plan_compute_deceleration() {
}
void plan_recompute_max_junction_velocity() {
// Assumes the nominal_speed_sqr values have been updated. May need to just multiply
// override values here.
// PROBLEM: Axes-limiting velocities get screwed up. May need to store an int8 value for the
// max override value possible for each block when the line is added. So the nominal_speed
// is computed with that ceiling, but still retained if the rates change again.
uint8_t block_index = block_buffer_tail;
plan_block_t *block = &block_buffer[block_index];
pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;
block_index = plan_next_block_index(block_index);
while (block_index != block_buffer_head) {
block = &block_buffer[block_index];
block->max_entry_speed_sqr = min(block->max_junction_speed_sqr,
min(block->nominal_speed_sqr,pl.previous_nominal_speed_sqr));
pl.previous_nominal_speed_sqr = block->nominal_speed_sqr;
block_index = plan_next_block_index(block_index);
}
}
*/

2
protocol.c
View File

@ -153,6 +153,8 @@ void protocol_execute_runtime()
// Initiate stepper feed hold
if (rt_exec & EXEC_FEED_HOLD) {
// !!! During a cycle, the segment buffer has just been reloaded and full. So the math involved
// with the feed hold should be fine for most, if not all, operational scenarios.
st_feed_hold(); // Initiate feed hold.
bit_false(sys.execute,EXEC_FEED_HOLD);
}

61
stepper.c
View File

@ -39,8 +39,8 @@
#define SEGMENT_NOOP 0
#define SEGMENT_END_OF_BLOCK bit(0)
#define SEGMENT_ACCEL bit(1)
#define SEGMENT_DECEL bit(2)
#define RAMP_CHANGE_ACCEL bit(1)
#define RAMP_CHANGE_DECEL bit(2)
#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change
@ -76,7 +76,7 @@ static stepper_t st;
// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1).
typedef struct {
int32_t step_events_remaining; // Tracks step event count for the executing planner block
uint32_t dist_next_step; // Scaled distance to next step
uint32_t dist_per_step; // Scaled distance to next step
uint32_t initial_rate; // Initialized step rate at re/start of a planner block
uint32_t nominal_rate; // The nominal step rate for this block in step_events/minute
uint32_t rate_delta; // The steps/minute to add or subtract when changing speed (must be positive)
@ -254,7 +254,7 @@ ISR(TIMER2_COMPA_vect)
st.counter_z = st.counter_x;
// Initialize inverse time, step rate data, and acceleration ramp counters
st.counter_dist = st_current_data->dist_next_step; // dist_next_step always greater than ramp_rate.
st.counter_dist = st_current_data->dist_per_step; // dist_per_step always greater than ramp_rate.
st.ramp_rate = st_current_data->initial_rate;
st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Initialize ramp counter via midpoint rule
st.ramp_type = RAMP_NOOP_CRUISE; // Initialize as no ramp operation. Corrected later if necessary.
@ -265,7 +265,7 @@ ISR(TIMER2_COMPA_vect)
}
// Check if ramp conditions have changed. If so, update ramp counters and control variables.
if ( st_current_segment->flag & (SEGMENT_DECEL | SEGMENT_ACCEL) ) {
if ( st_current_segment->flag & (RAMP_CHANGE_DECEL | RAMP_CHANGE_ACCEL) ) {
/* Compute correct ramp count for a ramp change. Upon a switch from acceleration to deceleration,
or vice-versa, the new ramp count must be set to trigger the next acceleration tick equal to
the number of ramp ISR ticks counted since the last acceleration tick. This is ensures the
@ -274,7 +274,7 @@ ISR(TIMER2_COMPA_vect)
as mandated by the mid-point rule. For the latter conditions, the ramp count have been
initialized such that the following computation is still correct. */
st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK-st.counter_ramp;
if ( st_current_segment->flag & SEGMENT_DECEL ) { st.ramp_type = RAMP_DECEL; }
if ( st_current_segment->flag & RAMP_CHANGE_DECEL ) { st.ramp_type = RAMP_DECEL; }
else { st.ramp_type = RAMP_ACCEL; }
}
@ -319,7 +319,7 @@ ISR(TIMER2_COMPA_vect)
// Execute Bresenham step event, when it's time to do so.
if (st.counter_dist < 0) {
st.counter_dist += st_current_data->dist_next_step; // Reload inverse time counter
st.counter_dist += st_current_data->dist_per_step; // Reload inverse time counter
st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits
st.execute_step = true;
@ -493,21 +493,21 @@ void st_cycle_reinitialize()
Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps.
NOTE: The segment buffer executes a set number of steps over an approximate time period.
If we try to execute over a set time period, it is difficult to guarantee or predict how
many steps will execute over it, especially when the step pulse phasing between the
neighboring segments are kept consistent. Meaning that, if the last segment step pulses
right before its end, the next segment must delay its first pulse so that the step pulses
are consistently spaced apart over time to keep the step pulse train nice and smooth.
Keeping track of phasing and ensuring that the exact number of steps are executed as
defined by the planner block, the related computational overhead gets quickly and
If we try to execute over a fixed time period, it is difficult to guarantee or predict
how many steps will execute over it, especially when the step pulse phasing between the
neighboring segments must also be kept consistent. Meaning that, if the last segment step
pulses right before a segment end, the next segment must delay its first pulse so that the
step pulses are consistently spaced apart over time to keep the step pulse train nice and
smooth. Keeping track of phasing and ensuring that the exact number of steps are executed
as defined by the planner block, the related computational overhead can get quickly and
prohibitively expensive, especially in real-time.
Since the stepper algorithm automatically takes care of the step pulse phasing with
its ramp and inverse time counters, we don't have to explicitly and expensively track the
exact number of steps, time, or phasing of steps. All we need to do is approximate
the number of steps in each segment such that the segment buffer has enough execution time
for the main program to do what it needs to do and refill it when it has time. In other
words, we just need to compute a cheap approximation of the current velocity and the
number of steps over it.
its ramp and inverse time counters by retaining the count remainders, we don't have to
explicitly and expensively track and synchronize the exact number of steps, time, and
phasing of steps. All we need to do is approximate the number of steps in each segment
such that the segment buffer has enough execution time for the main program to do what
it needs to do and refill it when it comes back. In other words, we just need to compute
a cheap approximation of the current velocity and the number of steps over it.
*/
/*
@ -529,7 +529,7 @@ void st_cycle_reinitialize()
*/
void st_prep_buffer()
{
if (sys.state != STATE_QUEUED) { // Block until a motion state is issued
if (sys.state == STATE_QUEUED) { return; } // Block until a motion state is issued
while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer.
// Initialize new segment
@ -555,7 +555,7 @@ void st_prep_buffer()
st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining;
st_prep_data->rate_delta = last_st_prep_data->rate_delta;
st_prep_data->dist_next_step = last_st_prep_data->dist_next_step;
st_prep_data->dist_per_step = last_st_prep_data->dist_per_step;
st_prep_data->nominal_rate = last_st_prep_data->nominal_rate; // TODO: Feedrate overrides recomputes this.
st_prep_data->mm_per_step = last_st_prep_data->mm_per_step;
@ -575,7 +575,7 @@ void st_prep_buffer()
st_prep_data->nominal_rate = ceil(sqrt(pl_prep_block->nominal_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
st_prep_data->rate_delta = ceil(pl_prep_block->acceleration*
((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
st_prep_data->dist_next_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
st_prep_data->dist_per_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
// TODO: Check if we really need to store this.
st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count;
@ -593,8 +593,8 @@ void st_prep_buffer()
// Calculate the planner block velocity profile type, determine deceleration point, and initial ramp.
float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index);
st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step );
if (st_prep_data->decelerate_after > 0) { // If 0, SEGMENT_DECEL flag is set later.
if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = SEGMENT_ACCEL; }
if (st_prep_data->decelerate_after > 0) { // If 0, RAMP_CHANGE_DECEL flag is set later.
if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = RAMP_CHANGE_ACCEL; }
}
}
@ -604,9 +604,9 @@ void st_prep_buffer()
// Approximate the velocity over the new segment using the already computed rate values.
// NOTE: This assumes that each segment will have an execution time roughly equal to every ACCELERATION_TICK.
// We do this to minimize memory and computational requirements. However, this could easily be replaced with
// a more exact approximation or have a unique time per segment, if CPU and memory overhead allows.
// a more exact approximation or have an user-defined time per segment, if CPU and memory overhead allows.
if (st_prep_data->decelerate_after <= 0) {
if (st_prep_data->decelerate_after == 0) { prep_segment->flag = SEGMENT_DECEL; } // Set segment deceleration flag
if (st_prep_data->decelerate_after == 0) { prep_segment->flag = RAMP_CHANGE_DECEL; } // Set segment deceleration flag
else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; }
if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; }
} else {
@ -618,10 +618,12 @@ void st_prep_buffer()
}
}
// TODO: Look into replacing the following dist_per_step divide with multiplying its inverse to save cycles.
// Compute the number of steps in the prepped segment based on the approximate current rate.
// NOTE: The dist_next_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps.
// NOTE: The dist_per_step divide cancels out the INV_TIME_MULTIPLIER and converts the rate value to steps.
prep_segment->n_step = ceil(max(MINIMUM_STEP_RATE,st_prep_data->current_approx_rate)*
(ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_next_step);
(ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_per_step);
// NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps
// a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed.
prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT);
@ -668,7 +670,6 @@ void st_prep_buffer()
// printString(" ");
}
}
}
uint8_t st_get_prep_block_index()