Yet another major stepper algorithm and planner overhaul.

- Overhauled the stepper algorithm and planner again. This time
concentrating on the decoupling of the stepper ISR completely. It is
now dumb, relying on the segment generator to provide the number of
steps to execute and how fast it needs to go. This freed up lots of
memory as well because it made a lot tracked variables obsolete.

- The segment generator now computes the velocity profile of the
executing planner block on the fly in floating point math, instead of
allowing the stepper algorithm to govern accelerations in the previous
code. What this accomplishes is the ability and framework to (somewhat)
easily install a different physics model for generating a velocity
profile, i.e. s-curves.

- Made some more planner enhancements and increased efficiency a bit.

- The changes also did not increase the compiled size of Grbl, but
decreased it slightly as well.

- Cleaned up a lot of the commenting.

- Still much to do, but this push works and still is missing feedholds
(coming next.)

archive/planner_dist.c

@@ -0,0 +1,669 @@
+/*
+  planner.c - buffers movement commands and manages the acceleration profile plan
+  Part of Grbl
+
+  Copyright (c) 2011-2013 Sungeun K. Jeon
+  Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Copyright (c) 2011 Jens Geisler  
+  
+  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/>.
+*/
+
+/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */
+
+#include <inttypes.h>    
+#include <stdlib.h>
+#include "planner.h"
+#include "nuts_bolts.h"
+#include "stepper.h"
+#include "settings.h"
+#include "config.h"
+#include "protocol.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 volatile 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.
+// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
+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);
+}
+                            
+
+// Update the entry speed and millimeters remaining to execute for a partially completed block. Called only
+// when the planner knows it will be changing the conditions of this block.
+// TODO: Set up to be called from planner calculations. Need supporting code framework still, i.e. checking
+//   and executing this only when necessary, combine with the block_buffer_safe pointer.
+// TODO: This is very similar to the planner reinitialize after a feed hold. Could make this do double duty.
+void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr)
+{
+// TODO: Need to make a condition to check if we need make these calculations. We don't if nothing has 
+//   been executed or placed into segment buffer. This happens with the first block upon startup or if 
+//   the segment buffer is exactly in between two blocks. Just check if the step_events_remaining is equal
+//   the total step_event_count in the block. If so, we don't have to do anything.
+
+  // !!! block index is the same as block_buffer_safe.
+  // See if we can reduce this down to just requesting the millimeters remaining..
+  uint8_t is_decelerating;
+  float millimeters_remaining = 0.0;
+  st_fetch_partial_block_parameters(block_index, &millimeters_remaining, &is_decelerating);
+ 
+  if (millimeters_remaining != 0.0) {
+    // Point to current block partially executed by stepper algorithm
+    plan_block_t *partial_block = plan_get_block_by_index(block_index);
+
+    // Compute the midway speed of the partially completely block at the end of the segment buffer.
+    if (is_decelerating) { // Block is decelerating
+      partial_block->entry_speed_sqr = exit_speed_sqr - 2*partial_block->acceleration*millimeters_remaining;
+    } else { // Block is accelerating or cruising
+      partial_block->entry_speed_sqr += 2*partial_block->acceleration*(partial_block->millimeters-millimeters_remaining);
+      partial_block->entry_speed_sqr = min(partial_block->entry_speed_sqr, partial_block->nominal_speed_sqr);
+    }
+
+    // Update only the relevant planner block information so the planner can plan correctly.
+    partial_block->millimeters = millimeters_remaining;
+    partial_block->max_entry_speed_sqr = partial_block->entry_speed_sqr; // Not sure if this needs to be updated.
+  }
+}
+
+
+/*                            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_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
+  algorithm segment buffer. See the stepper module for details. 
+  
+  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 
+  segment(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);
+
+  // Query stepper module for safe planner block index to recalculate to, which corresponds to the end
+  // of the step segment buffer.
+  uint8_t block_buffer_safe = st_get_prep_block_index();
+    
+  // TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module
+  // returns a NULL pointer or something. This could happen when the segment buffer is empty. Although,
+  // this call won't return a NULL, only an index.. I have to make sure that this index is synced with the
+  // planner at all times. 
+    
+  // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one.  
+  // NOTE: block_buffer_safe can be the last planner block if the segment buffer has completely queued up the
+  // remainder of the planner buffer. In this case, a new planner block will be treated as a single block.
+  if (block_index == block_buffer_safe) { // Also catches (head-1) = tail
+    
+    // Just set block_buffer_planned pointer.
+    block_buffer_planned = block_index;
+
+    // TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For
+    //   a single added block and recalculate after a feed hold, we don't need to compute this, since we already
+    //   know that the velocity starts and ends at zero. With an override, we can be traveling at some midblock
+    //   rate, and we have to calculate the new velocity profile from it.
+    // plan_update_partial_block(block_index,0.0);
+           
+  } else {
+    
+    // TODO: If the nominal speeds change during a feedrate override, we need to recompute the max entry speeds for 
+    //       all junctions before proceeding.
+
+    // Initialize planner buffer pointers and indexing.
+    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);
+      
+    // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
+    // block in buffer. Cease planning when: (1) the last optimal planned pointer is reached.
+    // (2) the safe block pointer is reached, whereby the planned pointer is updated.
+    // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
+    // NOTE: If the safe block is encountered before the planned block pointer, we know the safe block
+    // will be recomputed within the plan. So, we need to update it if it is partially completed.
+    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.
+      
+      // Only two plannable blocks in buffer. Compute previous block based on 
+      // !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero.
+      // !!! Need to make the current entry speed calculation after this.
+      plan_update_partial_block(block_index, 0.0);
+      block_buffer_planned = block_index;
+    
+    } else {
+      
+      // Three or more plan-able 
+      while (block_index != block_buffer_planned) { 
+
+        next = current;
+        current = &block_buffer[block_index];
+
+        // Increment block index early to check if the safe block is before the current block. If encountered,
+        // this is an exit condition as we can't go further than this block in the reverse pass.
+        block_index = plan_prev_block_index(block_index);
+        if (block_index == block_buffer_safe) {
+          // Check if the safe block is partially completed. If so, update it before its exit speed 
+          // (=current->entry speed) is over-written.
+          // TODO: The update breaks with feedrate overrides, because the replanning process no longer has
+          // the previous nominal speed to update this block with. There will need to be something along the
+          // lines of a nominal speed change check and send the correct value to this function.
+          plan_update_partial_block(block_index,current->entry_speed_sqr);
+
+          // Set planned pointer at safe block and for loop exit after following computation is done.
+          block_buffer_planned = block_index; 
+        } 
+
+        // 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.
+      // TODO: Need to check if the planned flag logic is correct for all scenarios. It may not
+      // be for certain conditions. However, if the block reaches nominal speed, it can be a valid
+      // breakpoint substitute.
+      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; // Set optimal plan pointer
+      }
+      
+      block_index = plan_next_block_index( block_index );
+    }
+    
+  }  
+  
+}
+
+
+void plan_reset_buffer()
+{
+  block_buffer_planned = block_buffer_tail;
+}
+
+
+void plan_init() 
+{
+  block_buffer_tail = 0;
+  block_buffer_head = 0; // Empty = tail
+  next_buffer_head = 1; // plan_next_block_index(block_buffer_head)
+  plan_reset_buffer();
+  memset(&pl, 0, sizeof(pl)); // Clear planner struct
+}
+
+
+void plan_discard_current_block() 
+{
+  if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer.
+    block_buffer_tail = plan_next_block_index( block_buffer_tail );
+  }
+}
+
+
+plan_block_t *plan_get_current_block() 
+{
+  if (block_buffer_head == block_buffer_tail) { // Buffer empty
+    plan_reset_buffer();
+    return(NULL); 
+  } 
+  return(&block_buffer[block_buffer_tail]);
+}
+
+
+plan_block_t *plan_get_block_by_index(uint8_t block_index) 
+{
+  if (block_buffer_head == block_index) { return(NULL); }
+  return(&block_buffer[block_index]);
+}
+
+
+// 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);
+}
+
+
+// Block until all buffered steps are executed or in a cycle state. Works with feed hold
+// during a synchronize call, if it should happen. Also, waits for clean cycle end.
+void plan_synchronize()
+{
+  while (plan_get_current_block() || sys.state == STATE_CYCLE) { 
+    protocol_execute_runtime();   // Check and execute run-time commands
+    if (sys.abort) { return; } // Check for system abort
+  }    
+}
+
+
+/* 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
+  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
+
+  // 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;
+  for (idx=0; idx<N_AXIS; idx++) {
+    // Calculate target position in absolute steps. This conversion should be consistent throughout.
+    target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
+  
+    // Number of steps for each axis and determine max step events
+    block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
+    block->step_event_count = max(block->step_event_count, block->steps[idx]);
+    
+    // Compute individual axes distance for move and prep unit vector calculations.
+    // NOTE: Computes true distance from converted step values.
+    delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];
+    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_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; }
+
+  // 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 = abs(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_velocity[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: Acceleration used in calculation needs to be limited by the minimum of the two junctions. 
+    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();
+
+// int32_t blength = block_buffer_head - block_buffer_tail;
+// if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } 
+// printInteger(blength);
+
+
+}
+
+
+// Reset the planner position vectors. Called by the system abort/initialization routine.
+void plan_sync_position()
+{
+  uint8_t idx;
+  for (idx=0; idx<N_AXIS; idx++) {
+    pl.position[idx] = sys.position[idx];
+  }
+}
+
+
+/*       STEPPER VELOCITY PROFILE DEFINITION 
+                                                 less than nominal speed->  + 
+                    +--------+ <- nominal_speed                            /|\                                         
+                   /          \                                           / | \                      
+   entry_speed -> +            \                                         /  |  + <- next->entry_speed
+                  |             + <- next->entry_speed                  /   |  |                       
+                  +-------------+                       entry_speed -> +----+--+                   
+                   time -->  ^  ^                                           ^  ^                       
+                             |  |                                           |  |                       
+                     decelerate distance                           decelerate distance  
+                                                        
+  Calculates the type of velocity profile for a given planner block and provides the deceleration
+  distance for the stepper algorithm to use to accurately trace the profile exactly. The planner
+  computes the entry and exit speeds of each block, but does not bother to determine the details of
+  the velocity profiles within them, as they aren't needed for computing an optimal plan. When the
+  stepper algorithm begins to execute a block, the block velocity profiles are computed ad hoc. 
+  
+  Each block velocity profiles can be described as either a trapezoidal or a triangular shape. The
+  trapezoid occurs when the block reaches the nominal speed of the block and cruises for a period of
+  time. A triangle occurs when the nominal speed is not reached within the block. Both of these 
+  velocity profiles may also be truncated on either end with no acceleration or deceleration ramps, 
+  as they can be influenced by the conditions of neighboring blocks, where the acceleration ramps
+  are defined by constant acceleration equal to the maximum allowable acceleration of a block.
+  
+  Since the stepper algorithm already assumes to begin executing a planner block by accelerating
+  from the planner entry speed and cruise if the nominal speed is reached, we only need to know 
+  when to begin deceleration to the end of the block. Hence, only the distance from the end of the
+  block to begin a deceleration ramp is computed for the stepper algorithm when requested.
+*/
+float plan_calculate_velocity_profile(uint8_t block_index) 
+{  
+  plan_block_t *current_block = &block_buffer[block_index];
+  
+  // Determine current block exit speed
+  float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed.
+  plan_block_t *next_block = plan_get_block_by_index(plan_next_block_index(block_index));
+  if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block
+  
+  // First determine intersection distance (in steps) from the exit point for a triangular profile.
+  // Computes: d_intersect = distance/2 + (v_entry^2-v_exit^2)/(4*acceleration)
+  float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) );  
+  
+  // Check if this is a pure acceleration block by a intersection distance less than zero. Also
+  // prevents signed and unsigned integer conversion errors.
+  if (intersect_distance > 0 ) {
+    float decelerate_distance;
+    // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile.
+    // Value is never negative. Nominal speed is always greater than or equal to the exit speed.
+    // Computes: d_decelerate = (v_nominal^2 - v_exit^2)/(2*acceleration)
+    decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration);
+
+    // The lesser of the two triangle and trapezoid distances always defines the velocity profile.
+    if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; }
+    
+    // Finally, check if this is a pure deceleration block.
+    if (decelerate_distance > current_block->millimeters) { return(0.0); }
+    else { return( (current_block->millimeters-decelerate_distance) ); }
+  }
+  return( current_block->millimeters ); // No deceleration in velocity profile.
+}        
+
+
+// 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(int32_t step_events_remaining) 
+{
+  plan_block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block
+  
+  // Only remaining millimeters and step_event_count need to be updated for planner recalculate. 
+  // Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to
+  // ensure the original planned motion is resumed exactly.
+  block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count;
+  block->step_event_count = step_events_remaining;
+  
+  // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer.
+  block->entry_speed_sqr = 0.0;
+  block->max_entry_speed_sqr = 0.0;
+  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);
+  }
+}
+
+*/

archive/planner_time_archive.c

@@ -0,0 +1,459 @@
+/*
+  planner.c - buffers movement commands and manages the acceleration profile plan
+  Part of Grbl
+
+  Copyright (c) 2011-2013 Sungeun K. Jeon
+  Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Copyright (c) 2011 Jens Geisler  
+  
+  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/>.
+*/
+
+/* The ring buffer implementation gleaned from the wiring_serial library by David A. Mellis. */
+
+#include <inttypes.h>    
+#include <stdlib.h>
+#include "planner.h"
+#include "nuts_bolts.h"
+#include "stepper.h"
+#include "settings.h"
+#include "config.h"
+#include "protocol.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.
+  
+  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 
+  segment(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);
+        
+  // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one.  
+  if (block_index == block_buffer_tail) {
+    // Just set block_buffer_planned pointer.
+    block_buffer_planned = block_buffer_tail;
+    return;
+  }
+  
+  // Initialize planner buffer pointers and indexing.
+  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);
+    
+  // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
+  // block in buffer. Cease planning when: (1) the last optimal planned pointer is reached.
+  // (2) the safe block pointer is reached, whereby the planned pointer is updated.
+  // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
+  // NOTE: If the safe block is encountered before the planned block pointer, we know the safe block
+  // will be recomputed within the plan. So, we need to update it if it is partially completed.
+  float entry_speed_sqr;
+  plan_block_t *next;
+  block_index = plan_prev_block_index(block_index);
+  if (block_index == block_buffer_tail) { // !! OR plan pointer? Yes I think so.  
+    // Only two plannable blocks in buffer. Compute previous block based on 
+    // !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero.
+    // !!! Need to make the current entry speed calculation after this.
+    st_update_plan_block_parameters();
+    block_buffer_planned = block_buffer_tail;
+  } else {
+    // Three or more plan-able blocks
+    while (block_index != block_buffer_planned) { 
+      next = current;
+      current = &block_buffer[block_index];
+
+      // Increment block index early to check if the safe block is before the current block. If encountered,
+      // this is an exit condition as we can't go further than this block in the reverse pass.
+      block_index = plan_prev_block_index(block_index);
+      if (block_index == block_buffer_tail) {
+        // Check if the safe block is partially completed. If so, update it before its exit speed 
+        // (=current->entry speed) is over-written.
+        // TODO: The update breaks with feedrate overrides, because the replanning process no longer has
+        // the previous nominal speed to update this block with. There will need to be something along the
+        // lines of a nominal speed change check and send the correct value to this function.
+        st_update_plan_block_parameters();
+        
+        // Set planned pointer at safe block and for loop exit after following computation is done.
+        block_buffer_planned = block_buffer_tail; 
+      } 
+
+      // 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.
+    // TODO: Need to check if the planned flag logic is correct for all scenarios. It may not
+    // be for certain conditions. However, if the block reaches nominal speed, it can be a valid
+    // breakpoint substitute.
+    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; // Set optimal plan pointer
+    }
+    block_index = plan_next_block_index( block_index );
+  } 
+}
+
+
+void plan_init() 
+{
+  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.
+    block_buffer_tail = plan_next_block_index( block_buffer_tail );
+  }
+}
+
+
+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);
+}
+
+
+// Block until all buffered steps are executed or in a cycle state. Works with feed hold
+// during a synchronize call, if it should happen. Also, waits for clean cycle end.
+void plan_synchronize()
+{
+  while (plan_get_current_block() || sys.state == STATE_CYCLE) { 
+    protocol_execute_runtime();   // Check and execute run-time commands
+    if (sys.abort) { return; } // Check for system abort
+  }    
+}
+
+
+/* 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
+  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
+
+  // 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;
+  for (idx=0; idx<N_AXIS; idx++) {
+    // Calculate target position in absolute steps. This conversion should be consistent throughout.
+    target_steps[idx] = lround(target[idx]*settings.steps_per_mm[idx]);
+  
+    // Number of steps for each axis and determine max step events
+    block->steps[idx] = labs(target_steps[idx]-pl.position[idx]);
+    block->step_event_count = max(block->step_event_count, block->steps[idx]);
+    
+    // Compute individual axes distance for move and prep unit vector calculations.
+    // NOTE: Computes true distance from converted step values.
+    delta_mm = (target_steps[idx] - pl.position[idx])/settings.steps_per_mm[idx];
+    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_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; }
+
+  // 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 = abs(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_velocity[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: Acceleration used in calculation needs to be limited by the minimum of the two junctions. 
+    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();
+  
+// int32_t blength = block_buffer_head - block_buffer_tail;
+// if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } 
+// printInteger(blength);
+}
+
+
+// Reset the planner position vectors. Called by the system abort/initialization routine.
+void plan_sync_position()
+{
+  uint8_t idx;
+  for (idx=0; idx<N_AXIS; idx++) {
+    pl.position[idx] = sys.position[idx];
+  }
+}
+
+
+// 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(int32_t step_events_remaining) 
+{
+  plan_block_t *block = &block_buffer[block_buffer_tail]; // Point to partially completed block
+  
+  // Only remaining millimeters and step_event_count need to be updated for planner recalculate. 
+  // Other variables (step_x, step_y, step_z, rate_delta, etc.) all need to remain the same to
+  // ensure the original planned motion is resumed exactly.
+  block->millimeters = (block->millimeters*step_events_remaining)/block->step_event_count;
+  block->step_event_count = step_events_remaining;
+  
+  // Re-plan from a complete stop. Reset planner entry speeds and buffer planned pointer.
+  block->entry_speed_sqr = 0.0;
+  block->max_entry_speed_sqr = 0.0;
+  block_buffer_planned = block_buffer_tail;
+  planner_recalculate();  
+}

archive/stepper_dist.c

@@ -0,0 +1,706 @@
+/*
+  stepper.c - stepper motor driver: executes motion plans using stepper motors
+  Part of Grbl
+
+  Copyright (c) 2011-2013 Sungeun K. Jeon
+  Copyright (c) 2009-2011 Simen Svale Skogsrud
+  
+  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/>.
+*/
+
+#include <avr/interrupt.h>
+#include "stepper.h"
+#include "config.h"
+#include "settings.h"
+#include "planner.h"
+#include "nuts_bolts.h"
+
+// Some useful constants
+#define TICKS_PER_MICROSECOND (F_CPU/1000000)
+
+#define RAMP_NOOP_CRUISE 0
+#define RAMP_ACCEL 1
+#define RAMP_DECEL 2
+
+#define LOAD_NOOP 0
+#define LOAD_SEGMENT 1
+#define LOAD_BLOCK 2
+
+#define SEGMENT_NOOP 0
+#define SEGMENT_END_OF_BLOCK bit(0)
+#define RAMP_CHANGE_ACCEL bit(1)
+#define RAMP_CHANGE_DECEL bit(2)
+
+#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change
+
+#define SEGMENT_BUFFER_SIZE 6
+
+// Stepper state variable. Contains running data and trapezoid variables.
+typedef struct {
+  // Used by the bresenham line algorithm
+  int32_t counter_x,        // Counter variables for the bresenham line tracer
+          counter_y, 
+          counter_z;
+  uint8_t segment_steps_remaining;  // Steps remaining in line segment motion
+
+  // Used by inverse time algorithm to track step rate
+  int32_t counter_dist;       // Inverse time distance traveled since last step event
+  uint32_t ramp_rate;        // Inverse time distance traveled per interrupt tick
+  uint32_t dist_per_tick;
+  
+  // Used by the stepper driver interrupt
+  uint8_t execute_step;    // Flags step execution for each interrupt.
+  uint8_t step_pulse_time; // Step pulse reset time after step rise
+  uint8_t out_bits;        // The next stepping-bits to be output
+  uint8_t load_flag;
+  
+  uint8_t counter_ramp;
+  uint8_t ramp_type;
+} stepper_t;
+static stepper_t st;
+
+// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the
+// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs.
+// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed
+// 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_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)
+  uint32_t current_approx_rate;  // Tracks the approximate segment rate to predict steps per segment to execute
+  int32_t decelerate_after;      // Tracks when to initiate deceleration according to the planner block
+  float mm_per_step;
+} st_data_t;
+static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1];
+
+// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute,
+// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps
+// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can.
+typedef struct {
+  uint8_t n_step;         // Number of step events to be executed for this segment
+  uint8_t st_data_index;  // Stepper buffer common data index. Uses this information to execute this segment.
+  uint8_t flag;           // Stepper algorithm bit-flag for special execution conditions.
+} st_segment_t;
+static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
+
+// Step segment ring buffer indices
+static volatile uint8_t segment_buffer_tail;
+static volatile uint8_t segment_buffer_head;
+static uint8_t segment_next_head;
+
+static volatile uint8_t busy;   // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
+static plan_block_t *pl_current_block;  // A pointer to the planner block currently being traced
+static st_segment_t *st_current_segment;
+static st_data_t *st_current_data;
+
+// Pointers for the step segment being prepped from the planner buffer. Accessed only by the
+// main program. Pointers may be planning segments or planner blocks ahead of what being executed.
+static plan_block_t *pl_prep_block;   // Pointer to the planner block being prepped
+static st_data_t *st_prep_data;       // Pointer to the stepper common data being prepped
+static uint8_t pl_prep_index;         // Index of planner block being prepped
+static uint8_t st_data_prep_index;    // Index of stepper common data block being prepped
+static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block
+
+
+/*        __________________________
+         /|                        |\     _________________         ^
+        / |                        | \   /|               |\        |
+       /  |                        |  \ / |               | \       s
+      /   |                        |   |  |               |  \      p
+     /    |                        |   |  |               |   \     e
+    +-----+------------------------+---+--+---------------+----+    e
+    |               BLOCK 1            |      BLOCK 2          |    d
+
+                            time ----->
+  
+   The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
+   until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
+   after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
+   +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
+*/
+
+// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
+// enabled. Startup init and limits call this function but shouldn't start the cycle.
+void st_wake_up() 
+{
+  // Enable steppers by resetting the stepper disable port
+  if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { 
+    STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); 
+  } else { 
+    STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
+  }
+  if (sys.state == STATE_CYCLE) {
+    // Initialize stepper output bits
+    st.out_bits = settings.invert_mask; 
+    // Initialize step pulse timing from settings.
+    st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
+    // Enable stepper driver interrupt
+    st.execute_step = false;
+    st.load_flag = LOAD_BLOCK;
+    
+    TCNT2 = 0; // Clear Timer2
+    TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
+    TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
+  }
+}
+
+
+// Stepper shutdown
+void st_go_idle() 
+{
+  // Disable stepper driver interrupt. Allow Timer0 to finish. It will disable itself.
+  TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt
+  TCCR2B = 0; // Disable Timer2
+  busy = false;
+
+  // Disable steppers only upon system alarm activated or by user setting to not be kept enabled.
+  if ((settings.stepper_idle_lock_time != 0xff) || bit_istrue(sys.execute,EXEC_ALARM)) {
+    // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
+    // stop and not drift from residual inertial forces at the end of the last movement.
+    delay_ms(settings.stepper_idle_lock_time);
+    if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { 
+      STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT); 
+    } else { 
+      STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); 
+    }   
+  }
+}
+
+
+/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
+   on an inverse time stepper algorithm, where a timer ticks at a constant frequency and uses 
+   time-distance counters to track when its the approximate time for a step event. For reference,
+   a similar inverse-time algorithm by Pramod Ranade is susceptible to numerical round-off, as
+   described, meaning that some axes steps may not execute correctly for a given multi-axis motion.
+     Grbl's algorithm differs by using a single inverse time-distance counter to manage a
+   Bresenham line algorithm for multi-axis step events, which ensures the number of steps for
+   each axis are executed exactly. In other words, Grbl uses a Bresenham within a Bresenham 
+   algorithm, where one tracks time for step events and the other steps for multi-axis moves.
+   Grbl specifically uses the Bresenham algorithm due to its innate mathematical exactness and
+   low computational overhead, requiring simple integer +,- counters only.
+     This interrupt pops blocks from the step segment buffer and executes them by pulsing the 
+   stepper pins appropriately. It is supported by The Stepper Port Reset Interrupt which it uses 
+   to reset the stepper port after each pulse. The bresenham line tracer algorithm controls all 
+   three stepper outputs simultaneously with these two interrupts.
+*/
+/* TODO: 
+   - Measure time in ISR. Typical and worst-case. Should be virtually identical to last algorithm.
+     There are no major changes to the base operations of this ISR with the new segment buffer.
+   - Write how the acceleration counters work and why they are set at half via mid-point rule.
+   - Determine if placing the position counters elsewhere (or change them to 8-bit variables that
+     are added to the system position counters at the end of a segment) frees up cycles.
+   - Write a blurb about how the acceleration should be handled within the ISR. All of the 
+     time/step/ramp counters accurately keep track of the remainders and phasing of the variables
+     with time. This means we do not have to compute them via expensive floating point beforehand.
+   - Need to do an analysis to determine if these counters are really that much cheaper. At least
+     find out when it isn't anymore. Particularly when the ISR is at a very high frequency.
+   - Create NOTE: to describe that the total time in this ISR must be less than the ISR frequency
+     in its worst case scenario.
+*/
+ISR(TIMER2_COMPA_vect)
+{
+// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
+  if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
+  
+  // Pulse stepper port pins, if flagged. New block dir will always be set one timer tick 
+  // before any step pulse due to algorithm design.
+  if (st.execute_step) {
+    st.execute_step = false;
+    STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | st.out_bits;
+    TCNT0 = st.step_pulse_time; // Reload Timer0 counter.
+    TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
+  }
+  
+  busy = true;
+  sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time. 
+         // NOTE: The remaining code in this ISR will finish before returning to main program.
+    
+  // If there is no step segment, attempt to pop one from the stepper buffer
+  if (st.load_flag != LOAD_NOOP) {
+    
+    // Anything in the buffer? If so, load and initialize next step segment.
+    if (segment_buffer_head != segment_buffer_tail) {
+      
+      // Initialize new step segment and load number of steps to execute
+      st_current_segment = &segment_buffer[segment_buffer_tail];
+      st.segment_steps_remaining = st_current_segment->n_step;
+    
+      // If the new segment starts a new planner block, initialize stepper variables and counters.
+      // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous.
+      if (st.load_flag == LOAD_BLOCK) {
+        pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this.
+        st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index];
+        
+        // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick.
+        st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask;
+        st.execute_step = true;
+        
+        // Initialize Bresenham line counters
+        st.counter_x = (pl_current_block->step_event_count >> 1);
+        st.counter_y = st.counter_x;
+        st.counter_z = st.counter_x;
+        
+        // Initialize inverse time, step rate data, and acceleration ramp counters
+        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. 
+
+        // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE.
+        if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; }
+        else { st.dist_per_tick = st.ramp_rate; }
+      }
+
+      // Check if ramp conditions have changed. If so, update ramp counters and control variables.
+      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 
+           ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known
+           rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2
+           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 & RAMP_CHANGE_DECEL ) { st.ramp_type = RAMP_DECEL; }
+        else { st.ramp_type = RAMP_ACCEL; }
+      }
+      
+      st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution.
+
+    } else {
+      // Can't discard planner block here if a feed hold stops in middle of block.
+      st_go_idle();
+      bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
+      return; // Nothing to do but exit.
+    }  
+    
+  } 
+  
+  // Adjust inverse time counter for ac/de-celerations
+  if (st.ramp_type) { // Ignored when ramp type is RAMP_NOOP_CRUISE
+    st.counter_ramp--; // Tick acceleration ramp counter
+    if (st.counter_ramp == 0) { // Adjust step rate when its time
+      st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
+      if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration
+        st.ramp_rate += st_current_data->rate_delta;
+        if (st.ramp_rate >= st_current_data->nominal_rate) { // Reached nominal rate.
+          st.ramp_rate = st_current_data->nominal_rate; // Set cruising velocity
+          st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising
+          st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp change.
+        }
+      } else { // Adjust velocity for deceleration.
+        if (st.ramp_rate > st_current_data->rate_delta) {
+          st.ramp_rate -= st_current_data->rate_delta;
+        } else { // Moving near zero feed rate. Gracefully slow down.
+          st.ramp_rate >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
+        }
+      }
+      // Adjust for minimum step rate, but retain operating ramp rate for accurate velocity tracing.
+      if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; }
+      else { st.dist_per_tick = st.ramp_rate; }
+    }
+  }
+  
+  // Iterate inverse time counter. Triggers each Bresenham step event.
+  st.counter_dist -= st.dist_per_tick; 
+  
+  // Execute Bresenham step event, when it's time to do so.
+  if (st.counter_dist < 0) {  
+    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;
+    
+    // Execute step displacement profile by Bresenham line algorithm
+    st.counter_x -= pl_current_block->steps[X_AXIS];
+    if (st.counter_x < 0) {
+      st.out_bits |= (1<<X_STEP_BIT);
+      st.counter_x += pl_current_block->step_event_count;
+      if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
+      else { sys.position[X_AXIS]++; }
+    }
+    st.counter_y -= pl_current_block->steps[Y_AXIS];
+    if (st.counter_y < 0) {
+      st.out_bits |= (1<<Y_STEP_BIT);
+      st.counter_y += pl_current_block->step_event_count;
+      if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
+      else { sys.position[Y_AXIS]++; }
+    }
+    st.counter_z -= pl_current_block->steps[Z_AXIS];
+    if (st.counter_z < 0) {
+      st.out_bits |= (1<<Z_STEP_BIT);
+      st.counter_z += pl_current_block->step_event_count;
+      if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
+      else { sys.position[Z_AXIS]++; }
+    }
+
+    // Check step events for trapezoid change or end of block.
+    st.segment_steps_remaining--; // Decrement step events count    
+    if (st.segment_steps_remaining == 0) {
+      // Line move is complete, set load line flag to check for new move.
+      // Check if last line move in planner block. Discard if so.
+      if (st_current_segment->flag & SEGMENT_END_OF_BLOCK) {
+        plan_discard_current_block();
+        st.load_flag = LOAD_BLOCK;
+      } else {
+        st.load_flag = LOAD_SEGMENT;
+      }
+      
+      // Discard current segment by advancing buffer tail index
+      if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; }
+    }
+
+    st.out_bits ^= settings.invert_mask;  // Apply step port invert mask    
+  }
+  busy = false;
+// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
+}
+
+
+// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the step
+// pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
+// finish, if Timer2 is disabled after completing a move.
+ISR(TIMER0_OVF_vect)
+{
+  STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); 
+  TCCR0B = 0; // Disable timer until needed.
+}
+
+
+// Reset and clear stepper subsystem variables
+void st_reset()
+{
+  memset(&st, 0, sizeof(st));
+  
+  st.load_flag = LOAD_BLOCK;
+  busy = false;
+
+  pl_current_block = NULL; // Planner block pointer used by stepper algorithm
+  pl_prep_block = NULL;  // Planner block pointer used by segment buffer
+  pl_prep_index =   0; // Planner buffer indices are also reset to zero.
+  st_data_prep_index = 0;
+  
+  segment_buffer_tail = 0;
+  segment_buffer_head = 0; // empty = tail
+  segment_next_head = 1;
+    
+  pl_partial_block_flag = false;
+}
+
+
+// Initialize and start the stepper motor subsystem
+void st_init()
+{
+  // Configure directions of interface pins
+  STEPPING_DDR |= STEPPING_MASK;
+  STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
+  STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
+	
+  // Configure Timer 2
+  TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt while configuring it
+  TCCR2B = 0; // Disable Timer2 until needed
+  TCNT2 = 0; // Clear Timer2 counter
+  TCCR2A = (1<<WGM21);  // Set CTC mode
+  OCR2A = (F_CPU/ISR_TICKS_PER_SECOND)/8 - 1; // Set Timer2 CTC rate
+
+  // Configure Timer 0
+  TIMSK0 &= ~(1<<TOIE0);
+  TCCR0A = 0; // Normal operation
+  TCCR0B = 0; // Disable Timer0 until needed
+  TIMSK0 |= (1<<TOIE0); // Enable overflow interrupt
+  
+  // Start in the idle state, but first wake up to check for keep steppers enabled option.
+  st_wake_up();
+  st_go_idle();
+}
+
+
+// Planner external interface to start stepper interrupt and execute the blocks in queue. Called
+// by the main program functions: planner auto-start and run-time command execution.
+void st_cycle_start() 
+{
+  if (sys.state == STATE_QUEUED) {
+    sys.state = STATE_CYCLE;
+    st_prep_buffer(); // Initialize step segment buffer before beginning cycle.
+    st_wake_up();
+  }
+}
+
+
+// Execute a feed hold with deceleration, only during cycle. Called by main program.
+void st_feed_hold() 
+{
+  if (sys.state == STATE_CYCLE) {
+    sys.state = STATE_HOLD;
+    sys.auto_start = false; // Disable planner auto start upon feed hold.
+  }
+}
+
+
+// Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by 
+// runtime command execution in the main program, ensuring that the planner re-plans safely.
+// NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
+// cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
+// Only the planner de/ac-celerations profiles and stepper rates have been updated.
+void st_cycle_reinitialize()
+{
+//   if (pl_current_block != NULL) {
+    // Replan buffer from the feed hold stop location.
+    
+    // TODO: Need to add up all of the step events in the current planner block to give 
+    // back to the planner. Should only need it for the current block. 
+    // BUT! The planner block millimeters is all changed and may be changed into the next
+    // planner block. The block millimeters would need to be recalculated via step counts
+    // and the mm/step variable.
+    // OR. Do we plan the feed hold itself down with the planner.
+    
+//     plan_cycle_reinitialize(st_current_data->step_events_remaining);
+//     st.ramp_type = RAMP_ACCEL;
+//     st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; 
+//     st.ramp_rate = 0;
+//     sys.state = STATE_QUEUED;
+//   } else {
+//     sys.state = STATE_IDLE;
+//   }
+    sys.state = STATE_IDLE;
+
+}
+
+
+/* Prepares step segment buffer. Continuously called from main program. 
+
+   The segment buffer is an intermediary buffer interface between the execution of steps
+   by the stepper algorithm and the velocity profiles generated by the planner. The stepper
+   algorithm only executes steps within the segment buffer and is filled by the main program
+   when steps are "checked-out" from the first block in the planner buffer. This keeps the
+   step execution and planning optimization processes atomic and protected from each other.
+   The number of steps "checked-out" from the planner buffer and the number of segments in
+   the segment buffer is sized and computed such that no operation in the main program takes
+   longer than the time it takes the stepper algorithm to empty it before refilling it. 
+   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 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 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. 
+*/
+
+/* 
+   TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. 
+     The problem is that when an override is reduced, the planner may not plan back to 
+     the current rate. Meaning that the velocity profiles for certain conditions no longer
+     are trapezoidal or triangular. For example, if the current block is cruising at a
+     nominal rate and the feedrate override is reduced, the new nominal rate will now be
+     lower. The velocity profile must first decelerate to the new nominal rate and then 
+     follow on the new plan. So the remaining velocity profile will have a decelerate, 
+     cruise, and another decelerate.
+        Another issue is whether or not a feedrate override reduction causes a deceleration
+     that acts over several planner blocks. For example, say that the plan is already 
+     heavily decelerating throughout it, reducing the feedrate will not do much to it. So,
+     how do we determine when to resume the new plan? How many blocks do we have to wait 
+     until the new plan intersects with the deceleration curve? One plus though, the 
+     deceleration will never be more than the number of blocks in the entire planner buffer,
+     but it theoretically can be equal to it when all planner blocks are decelerating already.
+*/
+void st_prep_buffer()
+{
+  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
+    st_segment_t *prep_segment = &segment_buffer[segment_buffer_head];
+    prep_segment->flag = SEGMENT_NOOP;
+    
+    // Determine if we need to load a new planner block.
+    if (pl_prep_block == NULL) {
+      pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block
+      if (pl_prep_block == NULL) { return; } // No planner blocks. Exit.
+        
+        
+SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; 
+        
+      // Increment stepper common data index 
+      if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; }
+        
+      // Check if the planner has re-computed this block mid-execution. If so, push the previous segment
+      // data. Otherwise, prepare a new segment data for the new planner block.
+      if (pl_partial_block_flag) {
+        
+        // Prepare new shared segment block data and copy the relevant last segment block data.
+        st_data_t *last_st_prep_data;
+        last_st_prep_data = st_prep_data;
+        st_prep_data = &segment_data[st_data_prep_index];
+            
+        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_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;
+
+        pl_partial_block_flag = false; // Reset flag
+        
+      } else {
+      
+        // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since 
+        // the planner buffer can dynamically change the velocity profile data as blocks are added.
+        st_prep_data = &segment_data[st_data_prep_index];
+      
+        // Initialize Bresenham variables
+        st_prep_data->step_events_remaining = pl_prep_block->step_event_count;
+
+        // Convert planner block velocity profile data to stepper rate and step distance data.
+        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_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;        
+    
+      }
+      
+      // Convert planner entry speed to stepper initial rate. 
+      st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
+
+      // TODO: Nominal rate changes with feedrate override.
+      // 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->current_approx_rate = st_prep_data->initial_rate;
+
+      // 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, RAMP_CHANGE_DECEL flag is set later.
+        if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = RAMP_CHANGE_ACCEL; }
+      }
+    }
+
+    // Set new segment to point to the current segment data block.
+    prep_segment->st_data_index = st_data_prep_index;
+
+    // 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 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 = 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 {    
+      if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) {
+        st_prep_data->current_approx_rate += st_prep_data->rate_delta;
+        if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) {
+          st_prep_data->current_approx_rate = st_prep_data->nominal_rate;
+        }
+      }
+    }
+    
+    // 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_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_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);
+    // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow.
+    // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow.
+    
+    // Check if n_step exceeds steps remaining in planner block. If so, truncate.
+    if (prep_segment->n_step > st_prep_data->step_events_remaining) { 
+      prep_segment->n_step = st_prep_data->step_events_remaining; 
+    }
+
+    // Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration
+    // ramp counters are set correctly during execution.
+    if (st_prep_data->decelerate_after > 0) {
+      if (prep_segment->n_step > st_prep_data->decelerate_after) { 
+        prep_segment->n_step = st_prep_data->decelerate_after; 
+      }
+    }
+ 
+    // Update stepper common data variables.
+    st_prep_data->decelerate_after -= prep_segment->n_step;
+    st_prep_data->step_events_remaining -= prep_segment->n_step;
+    
+    // Check for end of planner block
+    if ( st_prep_data->step_events_remaining == 0 ) { 
+    
+      // TODO: When a feed hold ends, the step_events_remaining will also be zero, even though a block
+      // have partially been completed. We need to flag the stepper algorithm to indicate a stepper shutdown
+      // when complete, but not remove the planner block unless it truly is the end of the block (rare).
+    
+      // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes.
+      prep_segment->flag |= SEGMENT_END_OF_BLOCK;
+      // Move planner pointer to next block and flag to load a new block for the next segment.
+      pl_prep_index = plan_next_block_index(pl_prep_index);
+      pl_prep_block = NULL;
+    }    
+
+    // New step segment completed. Increment segment buffer indices.
+    segment_buffer_head = segment_next_head;
+    if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; }
+    
+// long a = prep_segment->n_step;    
+// printInteger(a);
+// printString(" ");
+SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; 
+  } 
+}      
+
+uint8_t st_get_prep_block_index() 
+{
+// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this?
+  return(pl_prep_index);
+}
+
+void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating)
+{
+ // if called, can we assume that this always changes and needs to be updated? if so, then
+ // we can perform all of the segment buffer setup tasks here to make sure the next time
+ // the segments are loaded, the st_data buffer is updated correctly.
+ // !!! Make sure that this is always pointing to the correct st_prep_data block. 
+ 
+ // When a mid-block acceleration occurs, we have to make sure the ramp counters are updated
+ // correctly, much in the same fashion as the deceleration counters. Need to think about this 
+ // make sure this is right, but i'm pretty sure it is.
+  
+  // TODO: NULL means that the segment buffer has just completed a planner block. Clean up!
+  if (pl_prep_block != NULL) {
+    *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step;
+    if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } 
+    else { *is_decelerating = true; }
+
+    // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes.
+    pl_partial_block_flag = true;
+    pl_prep_block = NULL;
+  }
+  return;
+}

archive/stepper_time.c

@@ -0,0 +1,775 @@
+/*
+  stepper.c - stepper motor driver: executes motion plans using stepper motors
+  Part of Grbl
+
+  Copyright (c) 2011-2013 Sungeun K. Jeon
+  Copyright (c) 2009-2011 Simen Svale Skogsrud
+  
+  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/>.
+*/
+
+#include <avr/interrupt.h>
+#include "stepper.h"
+#include "config.h"
+#include "settings.h"
+#include "planner.h"
+#include "nuts_bolts.h"
+
+// Some useful constants
+#define TICKS_PER_MICROSECOND (F_CPU/1000000)
+
+#define RAMP_NOOP_CRUISE 0
+#define RAMP_ACCEL 1
+#define RAMP_DECEL 2
+
+#define LOAD_NOOP 0
+#define LOAD_SEGMENT 1
+#define LOAD_BLOCK 2
+
+#define SEGMENT_NOOP 0
+#define SEGMENT_END_OF_BLOCK bit(0)
+#define RAMP_CHANGE_ACCEL bit(1)
+#define RAMP_CHANGE_DECEL bit(2)
+
+#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change
+
+#define SEGMENT_BUFFER_SIZE 6
+
+#define DT_SEGMENT ACCELERATION_TICKS_PER_SECOND/ISR_TICKS_PER_SECOND
+
+// Stepper state variable. Contains running data and trapezoid variables.
+typedef struct {
+  // Used by the bresenham line algorithm
+  int32_t counter_x,        // Counter variables for the bresenham line tracer
+          counter_y, 
+          counter_z;
+
+  // Used by inverse time algorithm to track step rate
+  int32_t counter_dist;    // Inverse time distance traveled since last step event
+  
+  uint8_t step_count;  // Steps remaining in line segment motion
+  uint8_t phase_count; // Phase ticks remaining after line segment steps complete
+  
+  // Used by the stepper driver interrupt
+  uint8_t execute_step;    // Flags step execution for each interrupt.
+  uint8_t step_pulse_time; // Step pulse reset time after step rise
+  uint8_t out_bits;        // The next stepping-bits to be output
+  uint8_t load_flag;
+} stepper_t;
+static stepper_t st;
+
+// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the
+// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs.
+// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed
+// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1).
+typedef struct {  
+  uint32_t dist_per_step;
+  float step_events_remaining; // Tracks step event count for the executing planner block
+  float accelerate_until;
+  float decelerate_after;
+  float current_rate;
+  float maximum_rate;
+  float exit_rate;
+  
+  float acceleration;
+  float step_per_mm;
+} st_data_t;
+static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1];
+
+// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute,
+// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps
+// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can.
+typedef struct {
+  uint8_t n_step;         // Number of step events to be executed for this segment
+  uint8_t n_phase_tick;
+  uint32_t dist_per_tick;
+  uint8_t st_data_index;  // Stepper buffer common data index. Uses this information to execute this segment.
+  uint8_t flag;           // Stepper algorithm bit-flag for special execution conditions.
+} st_segment_t;
+static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
+
+// Step segment ring buffer indices
+static volatile uint8_t segment_buffer_tail;
+static volatile uint8_t segment_buffer_head;
+static uint8_t segment_next_head;
+
+static volatile uint8_t busy;   // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
+static plan_block_t *pl_current_block;  // A pointer to the planner block currently being traced
+static st_segment_t *st_current_segment;
+static st_data_t *st_current_data;
+
+// Pointers for the step segment being prepped from the planner buffer. Accessed only by the
+// main program. Pointers may be planning segments or planner blocks ahead of what being executed.
+static plan_block_t *pl_prep_block;   // Pointer to the planner block being prepped
+static st_data_t *st_prep_data;       // Pointer to the stepper common data being prepped
+static uint8_t pl_prep_index;         // Index of planner block being prepped
+static uint8_t st_data_prep_index;    // Index of stepper common data block being prepped
+static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block
+
+
+/*        __________________________
+         /|                        |\     _________________         ^
+        / |                        | \   /|               |\        |
+       /  |                        |  \ / |               | \       s
+      /   |                        |   |  |               |  \      p
+     /    |                        |   |  |               |   \     e
+    +-----+------------------------+---+--+---------------+----+    e
+    |               BLOCK 1            |      BLOCK 2          |    d
+
+                            time ----->
+  
+   The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
+   until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
+   after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
+   +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
+*/
+
+// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
+// enabled. Startup init and limits call this function but shouldn't start the cycle.
+void st_wake_up() 
+{
+  // Enable steppers by resetting the stepper disable port
+  if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { 
+    STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); 
+  } else { 
+    STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
+  }
+  if (sys.state == STATE_CYCLE) {
+    // Initialize stepper output bits
+    st.out_bits = settings.invert_mask; 
+    // Initialize step pulse timing from settings.
+    st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
+    // Enable stepper driver interrupt
+    st.execute_step = false;
+    st.load_flag = LOAD_BLOCK;
+    
+    TCNT2 = 0; // Clear Timer2
+    TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
+    TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
+  }
+}
+
+
+// Stepper shutdown
+void st_go_idle() 
+{
+  // Disable stepper driver interrupt. Allow Timer0 to finish. It will disable itself.
+  TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt
+  TCCR2B = 0; // Disable Timer2
+  busy = false;
+
+  // Disable steppers only upon system alarm activated or by user setting to not be kept enabled.
+  if ((settings.stepper_idle_lock_time != 0xff) || bit_istrue(sys.execute,EXEC_ALARM)) {
+    // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
+    // stop and not drift from residual inertial forces at the end of the last movement.
+    delay_ms(settings.stepper_idle_lock_time);
+    if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { 
+      STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT); 
+    } else { 
+      STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); 
+    }   
+  }
+}
+
+
+/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
+   on an inverse time stepper algorithm, where a timer ticks at a constant frequency and uses 
+   time-distance counters to track when its the approximate time for a step event. For reference,
+   a similar inverse-time algorithm by Pramod Ranade is susceptible to numerical round-off, as
+   described, meaning that some axes steps may not execute correctly for a given multi-axis motion.
+     Grbl's algorithm differs by using a single inverse time-distance counter to manage a
+   Bresenham line algorithm for multi-axis step events, which ensures the number of steps for
+   each axis are executed exactly. In other words, Grbl uses a Bresenham within a Bresenham 
+   algorithm, where one tracks time for step events and the other steps for multi-axis moves.
+   Grbl specifically uses the Bresenham algorithm due to its innate mathematical exactness and
+   low computational overhead, requiring simple integer +,- counters only.
+     This interrupt pops blocks from the step segment buffer and executes them by pulsing the 
+   stepper pins appropriately. It is supported by The Stepper Port Reset Interrupt which it uses 
+   to reset the stepper port after each pulse. The bresenham line tracer algorithm controls all 
+   three stepper outputs simultaneously with these two interrupts.
+*/
+/* TODO: 
+   - Measure time in ISR. Typical and worst-case. Should be virtually identical to last algorithm.
+     There are no major changes to the base operations of this ISR with the new segment buffer.
+   - Write how the acceleration counters work and why they are set at half via mid-point rule.
+   - Determine if placing the position counters elsewhere (or change them to 8-bit variables that
+     are added to the system position counters at the end of a segment) frees up cycles.
+   - Write a blurb about how the acceleration should be handled within the ISR. All of the 
+     time/step/ramp counters accurately keep track of the remainders and phasing of the variables
+     with time. This means we do not have to compute them via expensive floating point beforehand.
+   - Need to do an analysis to determine if these counters are really that much cheaper. At least
+     find out when it isn't anymore. Particularly when the ISR is at a very high frequency.
+   - Create NOTE: to describe that the total time in this ISR must be less than the ISR frequency
+     in its worst case scenario.
+*/
+ISR(TIMER2_COMPA_vect)
+{
+// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
+  if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
+  
+  // Pulse stepper port pins, if flagged. New block dir will always be set one timer tick 
+  // before any step pulse due to algorithm design.
+  if (st.execute_step) {
+    st.execute_step = false;
+    STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | st.out_bits;
+    TCNT0 = st.step_pulse_time; // Reload Timer0 counter.
+    TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
+  }
+  
+  busy = true;
+  sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time. 
+         // NOTE: The remaining code in this ISR will finish before returning to main program.
+    
+  // If there is no step segment, attempt to pop one from the stepper buffer
+  if (st.load_flag != LOAD_NOOP) {
+    
+    // Anything in the buffer? If so, load and initialize next step segment.
+    if (segment_buffer_head != segment_buffer_tail) {
+      
+      // Initialize new step segment and load number of steps to execute
+      st_current_segment = &segment_buffer[segment_buffer_tail];
+      st.step_count = st_current_segment->n_step;
+    
+      // If the new segment starts a new planner block, initialize stepper variables and counters.
+      // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous.
+      if (st.load_flag == LOAD_BLOCK) {
+        pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this.
+        st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index];
+        
+        // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick.
+        st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask;
+        st.execute_step = true;
+        
+        // Initialize Bresenham line counters
+        st.counter_x = (pl_current_block->step_event_count >> 1);
+        st.counter_y = st.counter_x;
+        st.counter_z = st.counter_x;
+        
+        // Initialize inverse time, step rate data, and acceleration ramp counters
+        st.counter_dist = st_current_data->dist_per_step;  // dist_per_step always greater than dist_per_tick.
+      }
+      
+      st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution.
+
+    } else {
+      // Can't discard planner block here if a feed hold stops in middle of block.
+      st_go_idle();
+      bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
+      return; // Nothing to do but exit.
+    }  
+    
+  } 
+    
+  // Iterate inverse time counter. Triggers each Bresenham step event.
+  st.counter_dist -= st_current_segment->dist_per_tick; 
+  
+  // Execute Bresenham step event, when it's time to do so.
+  if (st.counter_dist < 0) {
+    if (st.step_count > 0) {  // Block phase correction from executing step. 
+      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;
+      
+      // Execute step displacement profile by Bresenham line algorithm
+      st.counter_x -= pl_current_block->steps[X_AXIS];
+      if (st.counter_x < 0) {
+        st.out_bits |= (1<<X_STEP_BIT);
+        st.counter_x += pl_current_block->step_event_count;
+        if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
+        else { sys.position[X_AXIS]++; }
+      }
+      st.counter_y -= pl_current_block->steps[Y_AXIS];
+      if (st.counter_y < 0) {
+        st.out_bits |= (1<<Y_STEP_BIT);
+        st.counter_y += pl_current_block->step_event_count;
+        if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
+        else { sys.position[Y_AXIS]++; }
+      }
+      st.counter_z -= pl_current_block->steps[Z_AXIS];
+      if (st.counter_z < 0) {
+        st.out_bits |= (1<<Z_STEP_BIT);
+        st.counter_z += pl_current_block->step_event_count;
+        if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
+        else { sys.position[Z_AXIS]++; }
+      }
+  
+      // Check step events for trapezoid change or end of block.
+      st.step_count--; // Decrement step events count    
+  
+      st.out_bits ^= settings.invert_mask;  // Apply step port invert mask    
+    }
+  }
+  
+  if (st.step_count == 0) {
+    if (st.phase_count == 0) {
+      // Line move is complete, set load line flag to check for new move.
+      // Check if last line move in planner block. Discard if so.
+      if (st_current_segment->flag & SEGMENT_END_OF_BLOCK) {
+        plan_discard_current_block();
+        st.load_flag = LOAD_BLOCK;
+      } else {
+        st.load_flag = LOAD_SEGMENT;
+      }
+      
+      // Discard current segment by advancing buffer tail index
+      if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; }
+    }
+    st.phase_count--;
+  }
+  
+  
+  busy = false;
+// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
+}
+
+
+// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the step
+// pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
+// finish, if Timer2 is disabled after completing a move.
+ISR(TIMER0_OVF_vect)
+{
+  STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); 
+  TCCR0B = 0; // Disable timer until needed.
+}
+
+
+// Reset and clear stepper subsystem variables
+void st_reset()
+{
+  memset(&st, 0, sizeof(st));
+  
+  st.load_flag = LOAD_BLOCK;
+  busy = false;
+
+  pl_current_block = NULL; // Planner block pointer used by stepper algorithm
+  pl_prep_block = NULL;  // Planner block pointer used by segment buffer
+  pl_prep_index =   0; // Planner buffer indices are also reset to zero.
+  st_data_prep_index = 0;
+  
+  segment_buffer_tail = 0;
+  segment_buffer_head = 0; // empty = tail
+  segment_next_head = 1;
+    
+  pl_partial_block_flag = false;
+}
+
+
+// Initialize and start the stepper motor subsystem
+void st_init()
+{
+  // Configure directions of interface pins
+  STEPPING_DDR |= STEPPING_MASK;
+  STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
+  STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
+	
+  // Configure Timer 2
+  TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt while configuring it
+  TCCR2B = 0; // Disable Timer2 until needed
+  TCNT2 = 0; // Clear Timer2 counter
+  TCCR2A = (1<<WGM21);  // Set CTC mode
+  OCR2A = (F_CPU/ISR_TICKS_PER_SECOND)/8 - 1; // Set Timer2 CTC rate
+
+  // Configure Timer 0
+  TIMSK0 &= ~(1<<TOIE0);
+  TCCR0A = 0; // Normal operation
+  TCCR0B = 0; // Disable Timer0 until needed
+  TIMSK0 |= (1<<TOIE0); // Enable overflow interrupt
+  
+  // Start in the idle state, but first wake up to check for keep steppers enabled option.
+  st_wake_up();
+  st_go_idle();
+}
+
+
+// Planner external interface to start stepper interrupt and execute the blocks in queue. Called
+// by the main program functions: planner auto-start and run-time command execution.
+void st_cycle_start() 
+{
+  if (sys.state == STATE_QUEUED) {
+    sys.state = STATE_CYCLE;
+    st_prep_buffer(); // Initialize step segment buffer before beginning cycle.
+    st_wake_up();
+  }
+}
+
+
+// Execute a feed hold with deceleration, only during cycle. Called by main program.
+void st_feed_hold() 
+{
+  if (sys.state == STATE_CYCLE) {
+    sys.state = STATE_HOLD;
+    sys.auto_start = false; // Disable planner auto start upon feed hold.
+  }
+}
+
+
+// Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by 
+// runtime command execution in the main program, ensuring that the planner re-plans safely.
+// NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
+// cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
+// Only the planner de/ac-celerations profiles and stepper rates have been updated.
+void st_cycle_reinitialize()
+{
+//   if (pl_current_block != NULL) {
+    // Replan buffer from the feed hold stop location.
+    
+    // TODO: Need to add up all of the step events in the current planner block to give 
+    // back to the planner. Should only need it for the current block. 
+    // BUT! The planner block millimeters is all changed and may be changed into the next
+    // planner block. The block millimeters would need to be recalculated via step counts
+    // and the mm/step variable.
+    // OR. Do we plan the feed hold itself down with the planner.
+    
+//     plan_cycle_reinitialize(st_current_data->step_events_remaining);
+//     st.ramp_type = RAMP_ACCEL;
+//     st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; 
+//     st.ramp_rate = 0;
+//     sys.state = STATE_QUEUED;
+//   } else {
+//     sys.state = STATE_IDLE;
+//   }
+    sys.state = STATE_IDLE;
+
+}
+
+
+/* Prepares step segment buffer. Continuously called from main program. 
+
+   The segment buffer is an intermediary buffer interface between the execution of steps
+   by the stepper algorithm and the velocity profiles generated by the planner. The stepper
+   algorithm only executes steps within the segment buffer and is filled by the main program
+   when steps are "checked-out" from the first block in the planner buffer. This keeps the
+   step execution and planning optimization processes atomic and protected from each other.
+   The number of steps "checked-out" from the planner buffer and the number of segments in
+   the segment buffer is sized and computed such that no operation in the main program takes
+   longer than the time it takes the stepper algorithm to empty it before refilling it. 
+   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 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 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. 
+*/
+
+/* 
+   TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. 
+     The problem is that when an override is reduced, the planner may not plan back to 
+     the current rate. Meaning that the velocity profiles for certain conditions no longer
+     are trapezoidal or triangular. For example, if the current block is cruising at a
+     nominal rate and the feedrate override is reduced, the new nominal rate will now be
+     lower. The velocity profile must first decelerate to the new nominal rate and then 
+     follow on the new plan. So the remaining velocity profile will have a decelerate, 
+     cruise, and another decelerate.
+        Another issue is whether or not a feedrate override reduction causes a deceleration
+     that acts over several planner blocks. For example, say that the plan is already 
+     heavily decelerating throughout it, reducing the feedrate will not do much to it. So,
+     how do we determine when to resume the new plan? How many blocks do we have to wait 
+     until the new plan intersects with the deceleration curve? One plus though, the 
+     deceleration will never be more than the number of blocks in the entire planner buffer,
+     but it theoretically can be equal to it when all planner blocks are decelerating already.
+*/
+void st_prep_buffer()
+{
+  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
+    st_segment_t *prep_segment = &segment_buffer[segment_buffer_head];
+    prep_segment->flag = SEGMENT_NOOP;
+    
+    // -----------------------------------------------------------------------------------
+    // Determine if we need to load a new planner block. If so, prepare step data.
+    if (pl_prep_block == NULL) {
+      pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block
+      if (pl_prep_block == NULL) { return; } // No planner blocks. Exit.
+  SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
+        
+      // Increment stepper common data index 
+      if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; }
+        
+      // Check if the planner has re-computed this block mid-execution. If so, push the previous 
+      // segment data. Otherwise, prepare a new segment data for the new planner block.
+      if (pl_partial_block_flag) {
+        
+        // Prepare new shared segment block data and copy the relevant last segment block data.
+        st_data_t *last_st_prep_data;
+        last_st_prep_data = st_prep_data;
+        st_prep_data = &segment_data[st_data_prep_index];
+            
+        st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining;
+        st_prep_data->dist_per_step = last_st_prep_data->dist_per_step;
+        st_prep_data->step_per_mm = last_st_prep_data->step_per_mm;
+        st_prep_data->acceleration = last_st_prep_data->acceleration;
+
+        pl_partial_block_flag = false; // Reset flag
+        
+      } else {
+      
+        // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since 
+        // the planner buffer can dynamically change the velocity profile data as blocks are added.
+        st_prep_data = &segment_data[st_data_prep_index];
+      
+        // Initialize planner block step data
+        st_prep_data->step_events_remaining = pl_prep_block->step_event_count;
+        st_prep_data->step_per_mm = pl_prep_block->step_event_count/pl_prep_block->millimeters;        
+        st_prep_data->dist_per_step = ceil(INV_TIME_MULTIPLIER/st_prep_data->step_per_mm); // (mult*mm/step)
+        st_prep_data->acceleration = st_prep_data->step_per_mm*pl_prep_block->acceleration;
+        
+      }
+      
+      // Convert planner entry speed to stepper initial rate. 
+      st_prep_data->current_rate = st_prep_data->step_per_mm*sqrt(pl_prep_block->entry_speed_sqr);
+  
+      // Determine current block exit speed
+      plan_block_t *pl_next_block = plan_get_block_by_index(plan_next_block_index(pl_prep_index));
+      float exit_speed_sqr;  
+      if (pl_next_block != NULL) { 
+        exit_speed_sqr = pl_next_block->entry_speed_sqr; 
+        st_prep_data->exit_rate = st_prep_data->step_per_mm*sqrt(exit_speed_sqr);
+      } else {
+        exit_speed_sqr = 0.0; // End of planner buffer. Zero speed.
+        st_prep_data->exit_rate = 0.0;
+      }
+  
+      // Determine velocity profile based on the 7 possible types: Cruise-only, cruise-deceleration,
+      // acceleration-cruise, acceleration-only, deceleration-only, trapezoid, and triangle.
+      st_prep_data->accelerate_until = pl_prep_block->millimeters;      
+      if (pl_prep_block->entry_speed_sqr == pl_prep_block->nominal_speed_sqr) {
+        st_prep_data->maximum_rate = sqrt(pl_prep_block->nominal_speed_sqr);
+        st_prep_data->accelerate_until = pl_prep_block->millimeters;
+        if (exit_speed_sqr == pl_prep_block->nominal_speed_sqr) { // Cruise-only type
+          st_prep_data->decelerate_after = 0.0;
+        } else { // Cruise-deceleration type
+          st_prep_data->decelerate_after = (pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(2*pl_prep_block->acceleration);
+        }
+      } else if (exit_speed_sqr == pl_prep_block->nominal_speed_sqr) { 
+        // Acceleration-cruise type
+        st_prep_data->maximum_rate = sqrt(pl_prep_block->nominal_speed_sqr);
+        st_prep_data->decelerate_after = 0.0;
+        st_prep_data->accelerate_until -= (pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(2*pl_prep_block->acceleration);
+      } else {
+        float intersection_dist = 0.5*( pl_prep_block->millimeters + (pl_prep_block->entry_speed_sqr
+                                  - exit_speed_sqr)/(2*pl_prep_block->acceleration) );
+        if (intersection_dist > 0.0) {
+          if (intersection_dist < pl_prep_block->millimeters) { // Either trapezoid or triangle types
+            st_prep_data->decelerate_after = (pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(2*pl_prep_block->acceleration);
+            if (st_prep_data->decelerate_after < intersection_dist) { // Trapezoid type
+              st_prep_data->maximum_rate = sqrt(pl_prep_block->nominal_speed_sqr);
+              st_prep_data->accelerate_until -= (pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(2*pl_prep_block->acceleration);
+            } else { // Triangle type
+              st_prep_data->decelerate_after = intersection_dist;
+              st_prep_data->maximum_rate = sqrt(2*pl_prep_block->acceleration*st_prep_data->decelerate_after+exit_speed_sqr);
+              st_prep_data->accelerate_until -= st_prep_data->decelerate_after;
+            }          
+          } else { // Deceleration-only type
+            st_prep_data->maximum_rate = sqrt(pl_prep_block->entry_speed_sqr);
+            st_prep_data->decelerate_after = pl_prep_block->millimeters;
+          }
+        } else { // Acceleration-only type
+          st_prep_data->maximum_rate = sqrt(exit_speed_sqr);
+          st_prep_data->decelerate_after = 0.0;
+          st_prep_data->accelerate_until = 0.0;
+        }
+      }
+      
+      // Determine block velocity profile parameters
+//       st_prep_data->accelerate_until = (pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(2*pl_prep_block->acceleration);
+//       st_prep_data->decelerate_after = (pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(2*pl_prep_block->acceleration);
+//          
+//       // Determine if velocity profile is a triangle or trapezoid.
+//       if (pl_prep_block->millimeters < st_prep_data->accelerate_until+st_prep_data->decelerate_after) {  
+//         st_prep_data->decelerate_after = 0.5*( pl_prep_block->millimeters + (pl_prep_block->entry_speed_sqr
+//                                           - exit_speed_sqr)/(2*pl_prep_block->acceleration) );
+//         st_prep_data->accelerate_until = pl_prep_block->millimeters-st_prep_data->decelerate_after;
+//         st_prep_data->maximum_speed = sqrt(2*pl_prep_block->acceleration*st_prep_data->decelerate_after+exit_speed_sqr);
+//       } else {
+//         st_prep_data->accelerate_until = pl_prep_block->millimeters-st_prep_data->accelerate_until;
+//         st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr);
+//       }
+
+      // Convert velocity profile parameters in terms of steps.
+      st_prep_data->maximum_rate *= st_prep_data->step_per_mm;
+      st_prep_data->accelerate_until *= st_prep_data->step_per_mm;
+      st_prep_data->decelerate_after *= st_prep_data->step_per_mm;
+
+    }
+
+    // Set new segment to point to the current segment data block.
+    prep_segment->st_data_index = st_data_prep_index;
+
+    // -----------------------------------------------------------------------------------
+    // Initialize segment execute distance. Attempt to create a full segment over DT_SEGMENT.
+    // NOTE: Computed in terms of steps and seconds to prevent numerical round-off issues.
+
+    float steps_remaining = st_prep_data->step_events_remaining;
+    float dt = DT_SEGMENT;           
+    if (steps_remaining > st_prep_data->accelerate_until) { // Acceleration ramp
+      steps_remaining -= st_prep_data->current_rate*DT_SEGMENT
+                         + st_prep_data->acceleration*(0.5*DT_SEGMENT*DT_SEGMENT);
+      if (steps_remaining < st_prep_data->accelerate_until) { // **Incomplete** Acceleration ramp end.
+        // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block
+        steps_remaining = st_prep_data->accelerate_until;
+        dt = 2*(st_prep_data->step_events_remaining-steps_remaining)/
+             (st_prep_data->current_rate+st_prep_data->maximum_rate);
+        st_prep_data->current_rate = st_prep_data->maximum_rate;
+      } else { // **Complete** Acceleration only. 
+        st_prep_data->current_rate += st_prep_data->acceleration*DT_SEGMENT;
+      }
+    } else if (steps_remaining <= st_prep_data->decelerate_after) { // Deceleration ramp
+      steps_remaining -= st_prep_data->current_rate*DT_SEGMENT
+                         - st_prep_data->acceleration*(0.5*DT_SEGMENT*DT_SEGMENT);
+      if (steps_remaining > 0) { // **Complete** Deceleration only.
+        st_prep_data->current_rate -= st_prep_data->acceleration*DT_SEGMENT;
+      } else { // **Complete* End of block.
+        dt = 2*st_prep_data->step_events_remaining/(st_prep_data->current_rate+st_prep_data->exit_rate);
+        steps_remaining = 0;
+        // st_prep_data->current_speed = st_prep_data->exit_speed;
+      }
+    } else { // Cruising profile
+      steps_remaining -= st_prep_data->maximum_rate*DT_SEGMENT;
+      if (steps_remaining < st_prep_data->decelerate_after) { // **Incomplete** End of cruise. 
+        steps_remaining = st_prep_data->decelerate_after;
+        dt = (st_prep_data->step_events_remaining-steps_remaining)/st_prep_data->maximum_rate;
+      } // Otherwise **Complete** Cruising only.
+    }
+    
+    // -----------------------------------------------------------------------------------
+    // If segment is incomplete, attempt to fill the remainder.
+    // NOTE: Segment remainder always spans a cruise and/or a deceleration ramp.
+    
+    if (dt < DT_SEGMENT) {
+      if (steps_remaining > 0) { // Skip if end of block.
+        float last_steps_remaining;
+        
+        // Fill incomplete segment with an acceleration junction. 
+        if (steps_remaining > st_prep_data->decelerate_after) { // Cruising profile
+          last_steps_remaining = steps_remaining;
+          steps_remaining -= st_prep_data->current_rate*(DT_SEGMENT-dt);
+          if (steps_remaining < st_prep_data->decelerate_after) { // **Incomplete**
+            steps_remaining = st_prep_data->decelerate_after;
+            dt += (last_steps_remaining-steps_remaining)/st_prep_data->maximum_rate;
+            // current_speed = maximum_speed;
+          } else { // **Complete** Segment filled. 
+            dt = DT_SEGMENT;
+          }
+        }
+        
+        // Fill incomplete segment with a deceleration junction.
+        if (steps_remaining > 0) {
+          if (steps_remaining <= st_prep_data->decelerate_after) { // Deceleration ramp
+            last_steps_remaining = steps_remaining;
+            float dt_remainder = DT_SEGMENT-dt;
+            steps_remaining -= dt_remainder*(st_prep_data->current_rate 
+                               - 0.5*st_prep_data->acceleration*dt_remainder);
+            if (steps_remaining > 0) { // **Complete** Segment filled.
+              st_prep_data->current_rate -= st_prep_data->acceleration*dt_remainder;
+              dt = DT_SEGMENT;
+            } else { // **Complete** End of block.
+              steps_remaining = 0;
+              dt += (2*last_steps_remaining/(st_prep_data->current_rate+st_prep_data->exit_rate));
+              // st_prep_data->current_speed = st_prep_data->exit_speed;
+            }
+          }
+        }
+        
+      }
+    }
+ 
+    // -----------------------------------------------------------------------------------
+    // Compute segment step rate, steps to execute, and step phase correction parameters.
+    // NOTE: 
+    
+    // !!! PROBLEM. Step events remaining in floating point can limit the number of steps
+    // we can accurately track, since floats have 8 significant digits. However, this only
+    // becomes a problem if there are more than 10,000,000, which translates to a CNC machine
+    // with 800 step/mm and 10 meters of axis travel.
+    
+    prep_segment->dist_per_tick = ceil((st_prep_data->step_events_remaining-steps_remaining)
+                                  /dt*(INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND)); // (mult*mm/isr_tic)
+      
+    if (steps_remaining > 0) { 
+
+      // Compute number of steps to execute and segment step phase correction. 
+      prep_segment->n_step = ceil(st_prep_data->step_events_remaining)-ceil(steps_remaining);
+      prep_segment->n_phase_tick = ceil((ceil(steps_remaining)-steps_remaining)*st_prep_data->dist_per_step);
+          
+    } else { // End of block. Finish it out.
+      
+      // Set to execute the remaining steps and no phase correction upon finishing the block. 
+      prep_segment->n_step = ceil(st_prep_data->step_events_remaining);
+      prep_segment->n_phase_tick = 0;
+ 
+      // Move planner pointer to next block and flag to load a new block for the next segment.
+      pl_prep_index = plan_next_block_index(pl_prep_index);
+      pl_prep_block = NULL;
+      prep_segment->flag |= SEGMENT_END_OF_BLOCK;
+    }
+  
+    // Update step execution variables
+    st_prep_data->step_events_remaining = steps_remaining;
+
+   // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE.
+   // TODO: Use config.h error checking to do this. Otherwise, counters get screwy.
+
+    // New step segment initialization completed. Increment segment buffer indices.
+    segment_buffer_head = segment_next_head;
+    if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; }
+  SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
+  } 
+}      
+
+
+uint8_t st_get_prep_block_index() 
+{
+// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this?
+  return(pl_prep_index);
+}
+
+
+void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating)
+{
+ // if called, can we assume that this always changes and needs to be updated? if so, then
+ // we can perform all of the segment buffer setup tasks here to make sure the next time
+ // the segments are loaded, the st_data buffer is updated correctly.
+ // !!! Make sure that this is always pointing to the correct st_prep_data block. 
+ 
+ // When a mid-block acceleration occurs, we have to make sure the ramp counters are updated
+ // correctly, much in the same fashion as the deceleration counters. Need to think about this 
+ // make sure this is right, but i'm pretty sure it is.
+  
+  // TODO: NULL means that the segment buffer has just completed a planner block. Clean up!
+  if (pl_prep_block != NULL) {
+    *millimeters_remaining = st_prep_data->step_events_remaining/st_prep_data->step_per_mm;
+    if (st_prep_data->step_events_remaining < st_prep_data->decelerate_after) { *is_decelerating = true; } 
+    else { *is_decelerating = false; }
+
+    // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes.
+    pl_partial_block_flag = true;
+    pl_prep_block = NULL;
+  }
+  return;
+}

archive/stepper_time_archive.c

@@ -0,0 +1,823 @@
+/*
+  stepper.c - stepper motor driver: executes motion plans using stepper motors
+  Part of Grbl
+
+  Copyright (c) 2011-2013 Sungeun K. Jeon
+  Copyright (c) 2009-2011 Simen Svale Skogsrud
+  
+  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/>.
+*/
+
+#include <avr/interrupt.h>
+#include "stepper.h"
+#include "config.h"
+#include "settings.h"
+#include "planner.h"
+#include "nuts_bolts.h"
+
+// Some useful constants
+#define TICKS_PER_MICROSECOND (F_CPU/1000000)
+
+#define RAMP_ACCEL 0
+#define RAMP_CRUISE 1
+#define RAMP_DECEL 2
+
+#define LOAD_NOOP 0
+#define LOAD_SEGMENT 1
+#define LOAD_BLOCK 2
+
+#define SEGMENT_NOOP 0
+#define SEGMENT_END_OF_BLOCK bit(0)
+#define RAMP_CHANGE_ACCEL bit(1)
+#define RAMP_CHANGE_DECEL bit(2)
+
+#define SEGMENT_BUFFER_SIZE 6
+
+#define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0))
+
+// Stores the planner block Bresenham algorithm execution data for the segments in the segment 
+// buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will
+// never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1).
+// NOTE: This data is copied from the prepped planner blocks so that the planner blocks may be
+// discarded when entirely consumed and completed by the segment buffer.
+typedef struct {  
+  uint8_t direction_bits;
+  int32_t steps[N_AXIS];
+  int32_t step_event_count;
+} st_block_t;
+static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE-1];
+// TODO: Directly adjust this parameters to stop motion of individual axes for the homing cycle.
+// But this may require this to be volatile if it is controlled by an interrupt.
+
+// Primary stepper segment ring buffer. Contains small, short line segments for the stepper 
+// algorithm to execute, which are "checked-out" incrementally from the first block in the
+// planner buffer. Once "checked-out", the steps in the segments buffer cannot be modified by 
+// the planner, where the remaining planner block steps still can.
+typedef struct {
+  uint8_t n_step;         // Number of step events to be executed for this segment
+  uint8_t st_block_index;  // Stepper block data index. Uses this information to execute this segment.
+  int32_t phase_dist;
+  int32_t dist_per_tick;
+} segment_t;
+static segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
+
+// Stepper state variable. Contains running data and trapezoid variables.
+typedef struct {
+  // Used by the bresenham line algorithm
+  int32_t counter_x,        // Counter variables for the bresenham line tracer
+          counter_y, 
+          counter_z;
+
+  // Used by inverse time algorithm to track step rate
+  int32_t counter_dist;    // Inverse time distance traveled since last step event
+  
+  // Used by the stepper driver interrupt
+  uint8_t execute_step;     // Flags step execution for each interrupt.
+  uint8_t step_pulse_time;  // Step pulse reset time after step rise
+  uint8_t out_bits;         // The next stepping-bits to be output
+
+  uint8_t step_count;       // Steps remaining in line segment motion  
+  uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block.
+  st_block_t *exec_block;  // Pointer to the block data for the segment being executed
+  segment_t *exec_segment; // Pointer to the segment being executed
+} stepper_t;
+static stepper_t st;
+
+// Step segment ring buffer indices
+static volatile uint8_t segment_buffer_tail;
+static volatile uint8_t segment_buffer_head;
+static uint8_t segment_next_head;
+
+static volatile uint8_t busy;   // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
+
+// Pointers for the step segment being prepped from the planner buffer. Accessed only by the
+// main program. Pointers may be planning segments or planner blocks ahead of what being executed.
+static plan_block_t *pl_block;     // Pointer to the planner block being prepped
+static st_block_t *st_prep_block;  // Pointer to the stepper block data being prepped 
+
+typedef struct {
+  uint8_t st_block_index;      // Index of stepper common data block being prepped
+  uint8_t partial_block_flag;  // Flag indicating the planner has modified the prepped planner block
+
+  float step_per_mm;
+  float step_events_remaining; // Tracks step event count for the executing planner block
+//   int32_t step_events_remaining;
+  float step_remainder;
+  
+  uint8_t ramp_type;
+  float current_speed;
+  float maximum_speed;
+  float exit_speed;
+  float accelerate_until;
+  float decelerate_after;  
+} st_prep_t;
+static st_prep_t prep;
+
+
+/*        __________________________
+         /|                        |\     _________________         ^
+        / |                        | \   /|               |\        |
+       /  |                        |  \ / |               | \       s
+      /   |                        |   |  |               |  \      p
+     /    |                        |   |  |               |   \     e
+    +-----+------------------------+---+--+---------------+----+    e
+    |               BLOCK 1            |      BLOCK 2          |    d
+
+                            time ----->
+  
+   The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
+   until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
+   after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
+   +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
+*/
+
+// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
+// enabled. Startup init and limits call this function but shouldn't start the cycle.
+void st_wake_up() 
+{
+  // Enable steppers by resetting the stepper disable port
+  if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { 
+    STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); 
+  } else { 
+    STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
+  }
+  if (sys.state == STATE_CYCLE) {
+    // Initialize stepper output bits
+    st.out_bits = settings.invert_mask; 
+    // Initialize step pulse timing from settings.
+    st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
+    
+    // Enable stepper driver interrupt
+    TCNT2 = 0; // Clear Timer2
+    TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
+    TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
+  }
+}
+
+
+// Stepper shutdown
+void st_go_idle() 
+{
+  // Disable stepper driver interrupt. Allow Timer0 to finish. It will disable itself.
+  TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt
+  TCCR2B = 0; // Disable Timer2
+  busy = false;
+
+  // Disable steppers only upon system alarm activated or by user setting to not be kept enabled.
+  if ((settings.stepper_idle_lock_time != 0xff) || bit_istrue(sys.execute,EXEC_ALARM)) {
+    // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
+    // stop and not drift from residual inertial forces at the end of the last movement.
+    delay_ms(settings.stepper_idle_lock_time);
+    if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { 
+      STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT); 
+    } else { 
+      STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); 
+    }   
+  }
+}
+
+
+/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
+   on an inverse time stepper algorithm, where a timer ticks at a constant frequency and uses 
+   time-distance counters to track when its the approximate time for a step event. For reference,
+   a similar inverse-time algorithm by Pramod Ranade is susceptible to numerical round-off, as
+   described, meaning that some axes steps may not execute correctly for a given multi-axis motion.
+     Grbl's algorithm differs by using a single inverse time-distance counter to manage a
+   Bresenham line algorithm for multi-axis step events, which ensures the number of steps for
+   each axis are executed exactly. In other words, Grbl uses a Bresenham within a Bresenham 
+   algorithm, where one tracks time for step events and the other steps for multi-axis moves.
+   Grbl specifically uses the Bresenham algorithm due to its innate mathematical exactness and
+   low computational overhead, requiring simple integer +,- counters only.
+     This interrupt pops blocks from the step segment buffer and executes them by pulsing the 
+   stepper pins appropriately. It is supported by The Stepper Port Reset Interrupt which it uses 
+   to reset the stepper port after each pulse. The bresenham line tracer algorithm controls all 
+   three stepper outputs simultaneously with these two interrupts.
+*/
+/* TODO: 
+   - Measure time in ISR. Typical and worst-case. Should be virtually identical to last algorithm.
+     There are no major changes to the base operations of this ISR with the new segment buffer.
+   - Determine if placing the position counters elsewhere (or change them to 8-bit variables that
+     are added to the system position counters at the end of a segment) frees up cycles.
+   - Create NOTE: to describe that the total time in this ISR must be less than the ISR frequency
+     in its worst case scenario.
+*/
+ISR(TIMER2_COMPA_vect)
+{
+// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
+  if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
+  
+  // Pulse stepper port pins, if flagged. New block dir will always be set one timer tick 
+  // before any step pulse due to algorithm design.
+  if (st.execute_step) {
+    st.execute_step = false;
+    STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | st.out_bits;
+    TCNT0 = st.step_pulse_time; // Reload Timer0 counter.
+    TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
+  }
+  
+  busy = true;
+  sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time. 
+         // NOTE: The remaining code in this ISR will finish before returning to main program.
+    
+  // If there is no step segment, attempt to pop one from the stepper buffer
+  if (st.exec_segment == NULL) {
+    
+    // Anything in the buffer? If so, load and initialize next step segment.
+    if (segment_buffer_head != segment_buffer_tail) {
+      
+      // Initialize new step segment and load number of steps to execute
+      st.exec_segment = &segment_buffer[segment_buffer_tail];
+      st.step_count = st.exec_segment->n_step;
+    
+      // If the new segment starts a new planner block, initialize stepper variables and counters.
+      // NOTE: When the segment data index changes, this indicates a new planner block.
+      if ( st.exec_block_index != st.exec_segment->st_block_index ) {
+        st.exec_block_index = st.exec_segment->st_block_index;
+        st.exec_block = &st_block_buffer[st.exec_block_index];
+        
+        // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick.
+        st.out_bits = st.exec_block->direction_bits ^ settings.invert_mask;
+        st.execute_step = true;
+        
+        // Initialize Bresenham line counters
+        st.counter_x = (st.exec_block->step_event_count >> 1);
+        st.counter_y = st.counter_x;
+        st.counter_z = st.counter_x;
+        
+        // Initialize inverse time, step rate data, and acceleration ramp counters
+        st.counter_dist = INV_TIME_MULTIPLIER;  // dist_per_step always greater than dist_per_tick.
+      }
+      
+    } else {
+      // Segment buffer empty. Shutdown.
+      st_go_idle();
+      bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
+      return; // Nothing to do but exit.
+    }  
+    
+  } 
+    
+  // Iterate inverse time counter. Triggers each Bresenham step event.
+  st.counter_dist -= st.exec_segment->dist_per_tick; 
+  
+  // Execute Bresenham step event, when it's time to do so.
+  if (st.counter_dist < 0) {
+    if (st.step_count != 0) {  // Block phase correction from executing step. 
+      st.counter_dist += INV_TIME_MULTIPLIER; // Reload inverse time counter
+      st.step_count--; // Decrement step events count    
+
+      // Execute step displacement profile by Bresenham line algorithm
+      st.execute_step = true;
+      st.out_bits = st.exec_block->direction_bits; // Reset out_bits and reload direction bits      
+      st.counter_x -= st.exec_block->steps[X_AXIS];
+      if (st.counter_x < 0) {
+        st.out_bits |= (1<<X_STEP_BIT);
+        st.counter_x += st.exec_block->step_event_count;
+        if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
+        else { sys.position[X_AXIS]++; }
+      }
+      st.counter_y -= st.exec_block->steps[Y_AXIS];
+      if (st.counter_y < 0) {
+        st.out_bits |= (1<<Y_STEP_BIT);
+        st.counter_y += st.exec_block->step_event_count;
+        if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
+        else { sys.position[Y_AXIS]++; }
+      }
+      st.counter_z -= st.exec_block->steps[Z_AXIS];
+      if (st.counter_z < 0) {
+        st.out_bits |= (1<<Z_STEP_BIT);
+        st.counter_z += st.exec_block->step_event_count;
+        if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
+        else { sys.position[Z_AXIS]++; }
+      }  
+      st.out_bits ^= settings.invert_mask;  // Apply step port invert mask    
+    }
+  }
+  
+  if (st.step_count == 0) {
+    if (st.exec_segment->phase_dist > st.counter_dist) { 
+      // Segment is complete. Discard current segment and advance segment indexing.
+      st.exec_segment = NULL;
+      if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; }
+    }
+  }
+  
+  busy = false;
+// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
+}
+
+
+/* The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the step
+   pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
+   finish, if Timer2 is disabled after completing a move. */
+ISR(TIMER0_OVF_vect)
+{
+  STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); 
+  TCCR0B = 0; // Disable timer until needed.
+}
+
+
+// Reset and clear stepper subsystem variables
+void st_reset()
+{
+  memset(&prep, 0, sizeof(prep));
+  memset(&st, 0, sizeof(st));
+  st.exec_segment = NULL;
+  pl_block = NULL;  // Planner block pointer used by segment buffer
+  
+  segment_buffer_tail = 0;
+  segment_buffer_head = 0; // empty = tail
+  segment_next_head = 1;
+  busy = false;
+}
+
+
+// Initialize and start the stepper motor subsystem
+void st_init()
+{
+  // Configure directions of interface pins
+  STEPPING_DDR |= STEPPING_MASK;
+  STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
+  STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
+	
+  // Configure Timer 2
+  TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt while configuring it
+  TCCR2B = 0; // Disable Timer2 until needed
+  TCNT2 = 0; // Clear Timer2 counter
+  TCCR2A = (1<<WGM21);  // Set CTC mode
+  OCR2A = (F_CPU/ISR_TICKS_PER_SECOND)/8 - 1; // Set Timer2 CTC rate
+
+  // Configure Timer 0
+  TIMSK0 &= ~(1<<TOIE0);
+  TCCR0A = 0; // Normal operation
+  TCCR0B = 0; // Disable Timer0 until needed
+  TIMSK0 |= (1<<TOIE0); // Enable overflow interrupt
+  
+  // Start in the idle state, but first wake up to check for keep steppers enabled option.
+  st_wake_up();
+  st_go_idle();
+}
+
+
+// Planner external interface to start stepper interrupt and execute the blocks in queue. Called
+// by the main program functions: planner auto-start and run-time command execution.
+void st_cycle_start() 
+{
+  if (sys.state == STATE_QUEUED) {
+    sys.state = STATE_CYCLE;
+    st_prep_buffer(); // Initialize step segment buffer before beginning cycle.
+    st_wake_up();
+  }
+}
+
+
+// Execute a feed hold with deceleration, only during cycle. Called by main program.
+void st_feed_hold() 
+{
+  if (sys.state == STATE_CYCLE) {
+    sys.state = STATE_HOLD;
+    sys.auto_start = false; // Disable planner auto start upon feed hold.
+  }
+}
+
+
+// Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by 
+// runtime command execution in the main program, ensuring that the planner re-plans safely.
+// NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
+// cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
+// Only the planner de/ac-celerations profiles and stepper rates have been updated.
+void st_cycle_reinitialize()
+{
+//   if (pl_current_block != NULL) {    
+//     plan_cycle_reinitialize(st_exec_block->step_events_remaining);
+//     st.ramp_type = RAMP_ACCEL;
+//     st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; 
+//     st.ramp_rate = 0;
+//     sys.state = STATE_QUEUED;
+//   } else {
+//     sys.state = STATE_IDLE;
+//   }
+    sys.state = STATE_IDLE;
+
+}
+
+
+/* Prepares step segment buffer. Continuously called from main program. 
+
+   The segment buffer is an intermediary buffer interface between the execution of steps
+   by the stepper algorithm and the velocity profiles generated by the planner. The stepper
+   algorithm only executes steps within the segment buffer and is filled by the main program
+   when steps are "checked-out" from the first block in the planner buffer. This keeps the
+   step execution and planning optimization processes atomic and protected from each other.
+   The number of steps "checked-out" from the planner buffer and the number of segments in
+   the segment buffer is sized and computed such that no operation in the main program takes
+   longer than the time it takes the stepper algorithm to empty it before refilling it. 
+   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 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 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. 
+*/
+
+/* 
+   TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. 
+     The problem is that when an override is reduced, the planner may not plan back to 
+     the current rate. Meaning that the velocity profiles for certain conditions no longer
+     are trapezoidal or triangular. For example, if the current block is cruising at a
+     nominal rate and the feedrate override is reduced, the new nominal rate will now be
+     lower. The velocity profile must first decelerate to the new nominal rate and then 
+     follow on the new plan. So the remaining velocity profile will have a decelerate, 
+     cruise, and another decelerate.
+        Another issue is whether or not a feedrate override reduction causes a deceleration
+     that acts over several planner blocks. For example, say that the plan is already 
+     heavily decelerating throughout it, reducing the feedrate will not do much to it. So,
+     how do we determine when to resume the new plan? How many blocks do we have to wait 
+     until the new plan intersects with the deceleration curve? One plus though, the 
+     deceleration will never be more than the number of blocks in the entire planner buffer,
+     but it theoretically can be equal to it when all planner blocks are decelerating already.
+*/
+void st_prep_buffer()
+{
+  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.
+
+    
+    // -----------------------------------------------------------------------------------
+    // Determine if we need to load a new planner block. If so, prepare step data.
+    if (pl_block == NULL) {
+      pl_block = plan_get_current_block(); // Query planner for a queued block
+      if (pl_block == NULL) { return; } // No planner blocks. Exit.
+      
+// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; 
+                
+      // Check if the planner has re-computed this block mid-execution.
+      if (prep.partial_block_flag) {
+        // Retain last Bresenham data, but recompute the velocity profile.      
+        prep.partial_block_flag = false; // Reset flag
+      } else {
+        // Increment stepper common data index 
+        if ( ++prep.st_block_index == (SEGMENT_BUFFER_SIZE-1) ) { prep.st_block_index = 0; }
+
+        // Prepare and copy Bresenham algorithm segment data from the new planner block, so that
+        // when the segment buffer completes the planner block, it may be discarded immediately.
+        st_prep_block = &st_block_buffer[prep.st_block_index];
+        st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS];
+        st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS];
+        st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS];
+        st_prep_block->direction_bits = pl_block->direction_bits;
+        st_prep_block->step_event_count = pl_block->step_event_count;
+
+        // Initialize planner block step count, unit distance data, and remainder tracker.
+        prep.step_per_mm = ((float)st_prep_block->step_event_count)/pl_block->millimeters;        
+        prep.step_events_remaining = st_prep_block->step_event_count;
+        prep.step_remainder = 0.0;   
+      }
+      
+      // Compute the prepped planner block velocity profile to be traced by stepper algorithm. 
+      prep.current_speed = sqrt(pl_block->entry_speed_sqr);
+      prep.exit_speed = plan_get_exec_block_exit_speed(); 
+
+      // Determine velocity profile based on the 7 possible types: Cruise-only, cruise-deceleration,
+      // acceleration-cruise, acceleration-only, deceleration-only, full-trapezoid, and triangle.
+      prep.ramp_type = RAMP_ACCEL;
+      float exit_speed_sqr = prep.exit_speed*prep.exit_speed;
+      float inv_2_accel = 0.5/pl_block->acceleration;
+      float intersection_dist =
+              0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr));
+      if (intersection_dist > 0.0) {
+        if (intersection_dist < pl_block->millimeters) { // Either trapezoid or triangle types
+          // NOTE: For acceleration-cruise trapezoid, following calculation will be 0.0.
+          prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr);
+          if (prep.decelerate_after < intersection_dist) { // Trapezoid type
+            prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr);
+            if (pl_block->entry_speed_sqr == pl_block->nominal_speed_sqr) { 
+              // Cruise-deceleration or cruise-only type.
+              prep.ramp_type = RAMP_CRUISE;
+              prep.accelerate_until = pl_block->millimeters; 
+            } else {
+              // Full-trapezoid or acceleration-cruise types
+              prep.accelerate_until = 
+                pl_block->millimeters-inv_2_accel*(pl_block->nominal_speed_sqr-pl_block->entry_speed_sqr); 
+            }
+          } else { // Triangle type
+            prep.accelerate_until = intersection_dist;
+            prep.decelerate_after = intersection_dist;
+            prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersection_dist+exit_speed_sqr);
+          }          
+        } else { // Deceleration-only type
+          prep.ramp_type = RAMP_DECEL;
+          prep.maximum_speed = prep.current_speed;
+          prep.accelerate_until = pl_block->millimeters;
+          prep.decelerate_after = pl_block->millimeters;
+        }
+      } else { // Acceleration-only type
+        prep.maximum_speed = prep.exit_speed;
+        prep.accelerate_until = 0.0;
+        prep.decelerate_after = 0.0;
+      }
+        
+    }
+
+    // Initialize new segment
+    segment_t *prep_segment = &segment_buffer[segment_buffer_head];
+
+    // Set new segment to point to the current segment data block.
+    prep_segment->st_block_index = prep.st_block_index;
+
+    /* -----------------------------------------------------------------------------------
+       Compute the average velocity of this new segment by determining the total distance
+       traveled over the segment time DT_SEGMENT. This section attempts to create a full
+       segment based on the current ramp conditions. If the segment is incomplete and 
+       terminates upon a ramp change, the next section will attempt to fill the remaining 
+       segment execution time. However, if an incomplete segment terminates at the end of
+       the planner block, the segment execution time is less than DT_SEGMENT and the new
+       segment will execute over this truncated execution time.
+    */   
+    float dt = 0.0;
+    float mm_remaining = pl_block->millimeters;
+    float dt_var = DT_SEGMENT;
+    float mm_var;
+    do {
+      switch (prep.ramp_type) {
+        case RAMP_ACCEL: 
+          // NOTE: Acceleration ramp always computes during first loop only.
+          mm_remaining -= DT_SEGMENT*(prep.current_speed + pl_block->acceleration*(0.5*DT_SEGMENT));
+          if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp.
+            // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block.
+            mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB
+            dt_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed);
+            if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; }
+            else { prep.ramp_type = RAMP_CRUISE; }
+            prep.current_speed = prep.maximum_speed;
+          } else { // Acceleration only. 
+            prep.current_speed += pl_block->acceleration*dt_var;
+          }
+          break;
+        case RAMP_CRUISE: 
+          // NOTE: mm_var used to retain the last mm_remaining for incomplete segment dt_var calculations.
+          mm_var = mm_remaining - prep.maximum_speed*dt_var;
+          if (mm_var < prep.decelerate_after) { // End of cruise. 
+            // Cruise-deceleration junction or end of block.
+            dt_var = (mm_remaining - prep.decelerate_after)/prep.maximum_speed;
+            mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB
+            prep.ramp_type = RAMP_DECEL;
+          } else { // Cruising only.         
+            mm_remaining = mm_var; 
+          } 
+          break;
+        default: // case RAMP_DECEL:
+          // NOTE: mm_var used to catch negative decelerate distance values near zero speed.
+          mm_var = dt_var*(prep.current_speed - 0.5*pl_block->acceleration*dt_var);
+          if ((mm_var > 0.0) && (mm_var < pl_block->millimeters)) { // Deceleration only.
+            prep.current_speed -= pl_block->acceleration*dt_var;
+            // Check for near-zero speed and prevent divide by zero in rare scenarios.
+            if (prep.current_speed <= prep.exit_speed) { mm_remaining = 0.0; }
+            else { mm_remaining -= mm_var; }
+          } else { // End of block.
+            dt_var = 2.0*mm_remaining/(prep.current_speed+prep.exit_speed);
+            mm_remaining = 0.0;
+            // prep.current_speed = prep.exit_speed;
+          }
+      }
+      dt += dt_var;
+      if (dt < DT_SEGMENT) { dt_var = DT_SEGMENT - dt; } // **Incomplete** At ramp junction.
+      else { break; } // **Complete** Exit loop. Segment execution time maxed.      
+    } while ( mm_remaining > 0.0 ); // **Complete** Exit loop. End of planner block.
+        
+    /*
+    float mm_remaining;
+    float dt = DT_SEGMENT;    
+    if (pl_block->millimeters > prep.accelerate_until) { // [Acceleration Ramp]
+      mm_remaining = pl_block->millimeters - DT_SEGMENT*(prep.current_speed + pl_block->acceleration*(0.5*DT_SEGMENT));
+      if (mm_remaining < prep.accelerate_until) { // **Incomplete** Acceleration ramp end.
+        // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block.
+        mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB
+        dt = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed);
+        prep.current_speed = prep.maximum_speed;
+      } else { // **Complete** Acceleration only. 
+        prep.current_speed += pl_block->acceleration*DT_SEGMENT;
+        prep.current_speed = min(prep.maximum_speed,prep.current_speed);
+      }
+    } else if (pl_block->millimeters > prep.decelerate_after) { // [No Ramp. Cruising]
+        mm_remaining = pl_block->millimeters - prep.maximum_speed*DT_SEGMENT;
+        if (mm_remaining < prep.decelerate_after) { // **Incomplete** End of cruise. 
+          // Cruise-deceleration junction or end of block.
+          mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB
+          dt = (pl_block->millimeters-mm_remaining)/prep.maximum_speed;
+        } // Otherwise **Complete** Cruising only. 
+    } else {  // [Deceleration Ramp]
+      mm_remaining = DT_SEGMENT*(prep.current_speed - 0.5*pl_block->acceleration*DT_SEGMENT);
+      if ((mm_remaining > 0.0) && (mm_remaining < pl_block->millimeters)) { // **Complete** Deceleration only.
+        prep.current_speed -= pl_block->acceleration*DT_SEGMENT;
+        if (prep.current_speed <= prep.exit_speed) { // Round off error fix. Prevents divide by zero.
+          mm_remaining = 0.0;
+        } else { 
+          mm_remaining = pl_block->millimeters - mm_remaining; 
+        }
+      } else { // **Complete** End of block.
+        mm_remaining = 0.0;
+        dt = 2.0*pl_block->millimeters/(prep.current_speed+prep.exit_speed);
+        // prep.current_speed = prep.exit_speed;
+      }
+    }
+    
+    
+    /* -----------------------------------------------------------------------------------
+       If segment is incomplete, attempt to fill the remaining segment execution time.
+       NOTE: Segment remainder always spans a cruise and/or a deceleration ramp.        
+    
+    float partial_mm, dt_remainder;
+    if ((dt < DT_SEGMENT) && (mm_remaining > 0.0)) {      
+      dt_remainder = DT_SEGMENT-dt;
+      
+      // Attempt to fill incomplete segment with cruising profile. 
+      if (mm_remaining > prep.decelerate_after) { // Cruising profile
+        partial_mm = mm_remaining - prep.current_speed*dt_remainder;
+        if (partial_mm < prep.decelerate_after) { // **Incomplete**
+          dt += (mm_remaining-prep.decelerate_after)/prep.maximum_speed;
+          mm_remaining = prep.decelerate_after;
+          // current_speed = maximum_speed;
+        } else { // **Complete** Segment filled. 
+          mm_remaining = partial_mm; 
+          dt = DT_SEGMENT;
+        }
+      }
+      
+      // Attempt to fill incomplete segment with deceleration ramp.
+      if ((dt < DT_SEGMENT) && (mm_remaining > 0.0)) {  
+      if (mm_remaining <= prep.decelerate_after) { // Deceleration ramp
+        dt_remainder = DT_SEGMENT-dt;    
+        partial_mm = dt_remainder*(prep.current_speed-0.5*pl_block->acceleration*dt_remainder);
+        if ((partial_mm > 0.0) && (mm_remaining > partial_mm)) { // **Complete** Segment filled.
+          prep.current_speed -= pl_block->acceleration*dt_remainder;
+          if (prep.current_speed <= prep.exit_speed) {
+            mm_remaining = 0.0;
+
+          } else {
+            mm_remaining -= partial_mm;
+            dt = DT_SEGMENT;
+          }          
+        } else { // **Complete** End of block.
+          dt += (2.0*mm_remaining/(prep.current_speed+prep.exit_speed));
+          mm_remaining = 0.0;
+          // prep.current_speed = prep.exit_speed;
+        }
+      }  
+      }
+    }
+    */
+//  printString(" Z");
+//  printFloat(dt*(60.0*1000.0));
+//  printString(" ");
+//  printFloat(mm_remaining);
+//  printString(" ");
+//  printFloat(prep.current_speed);
+//   printString("Z ");   
+   
+    /* -----------------------------------------------------------------------------------
+       Compute segment step rate, steps to execute, and step phase correction parameters.
+    */
+//     float step_events;
+//     if (mm_remaining > 0.0) { 
+//       step_events = prep.step_per_mm*(pl_block->millimeters - mm_remaining); // Convert mm to steps
+//       prep_segment->n_step = floor(step_events + prep.step_remainder);
+//       if (prep_segment->n_step > prep.step_events_remaining) { // Prevent round-off overshoot
+//         prep_segment->n_step = prep.step_events_remaining; 
+//       }
+//     } else { // Ensure all remaining steps are executed
+//       step_events = prep.step_per_mm*pl_block->millimeters;
+//       prep_segment->n_step = prep.step_events_remaining;
+//     }
+//     prep.step_events_remaining -= prep_segment->n_step;
+//     
+//     // Compute segment rate.
+//     prep_segment->dist_per_tick =
+//       ceil( (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND)) * (step_events/dt) ); // (mult*step/isr_tic)
+// 
+//     if (prep.step_events_remaining > 0) {
+//       // Compute step phase distance and update segment continuation parameters.
+//       prep.step_remainder += step_events - prep_segment->n_step;
+//       prep_segment->phase_dist = ceil(INV_TIME_MULTIPLIER-INV_TIME_MULTIPLIER*prep.step_remainder);
+//       pl_block->millimeters = mm_remaining;
+//       pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed;
+// 
+//     } else { // End of block. Finish it out.
+//       // The planner block is complete. All steps are set to be executed in the segment buffer.
+//       // Move planner pointer to next block and flag to load a new block for the next segment.
+//       prep_segment->phase_dist = INV_TIME_MULTIPLIER;   
+//       pl_block = NULL;
+//       plan_discard_current_block();
+//     }
+
+    if (mm_remaining > 0.0) { 
+
+      float steps_remaining = prep.step_per_mm*mm_remaining; 
+      prep_segment->dist_per_tick = ceil( (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))*
+                                    ((prep.step_events_remaining-steps_remaining)/dt) ); // (mult*step/isr_tic)
+
+      // Compute number of steps to execute and segment step phase correction. 
+      prep_segment->n_step = ceil(prep.step_events_remaining)-ceil(steps_remaining);
+      prep_segment->phase_dist = ceil(INV_TIME_MULTIPLIER*(1.0-ceil(steps_remaining)+steps_remaining));
+
+      // Update step execution variables
+      prep.step_events_remaining = steps_remaining;                                    
+      pl_block->millimeters = mm_remaining;
+      pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed;
+
+    } else { // End of block. Finish it out.
+
+      prep_segment->dist_per_tick = ceil( (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))*
+                                    prep.step_events_remaining/dt ); // (mult*step/isr_tic)
+      prep_segment->phase_dist = INV_TIME_MULTIPLIER;
+
+      // Set to execute the remaining steps and no phase correction upon finishing the block. 
+      prep_segment->n_step = ceil(prep.step_events_remaining);
+
+      
+      // NOTE: Not required. Planner will ignore this block as it is now complete.
+      // prep.step_events_remaining = 0.0;
+      // pl_block->millimeters = 0.0;
+      
+      // The planner block is complete. All steps are set to be executed in the segment buffer.
+      // Move planner pointer to next block and flag to load a new block for the next segment.
+      pl_block = NULL;
+      plan_discard_current_block();
+    }
+    
+// long a = prep_segment->n_step;
+//  printInteger(a);
+//  printString(" ");
+//  a = prep_segment->phase_dist;
+//  printInteger(prep_segment->dist_per_tick);
+//  printString(" ");
+//  printFloat(prep.step_events_remaining);
+//  printString(" "); 
+//  printFloat(pl_block->millimeters);
+//  printString(" ");
+
+
+    // !!! PROBLEM. Step events remaining in floating point can limit the number of steps
+    // we can accurately track, since floats have ~7.2 significant digits. However, this only
+    // becomes a problem if there are more than 1,000,000, which translates to a CNC machine
+    // with 200 step/mm and 5 meters of axis travel. Possible but unlikely. Could have more 
+    // issues with user setting up their machine with too high of steps.
+    
+    // TODO: dist_per_tick must be less than INV_TIME_MULTIPLIER. A check can be made to 
+    // make this a hard limit. Need to make sure this doesn't affect the velocity profiles..
+    // it shouldn't. The same could said for the minimum allowable step rate too. This should
+    // not affect the tracing of the profiles either.
+
+   // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE.
+   // TODO: Use config.h error checking to do this. Otherwise, counters get screwy.
+
+    // New step segment initialization completed. Increment segment buffer indices.
+    segment_buffer_head = segment_next_head;
+    if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; }
+
+int32_t blength = segment_buffer_head - segment_buffer_tail;
+if (blength < 0) { blength += SEGMENT_BUFFER_SIZE; } 
+printInteger(blength);
+//     SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
+  } 
+}      
+
+
+// uint8_t st_get_prep_block_index() 
+// {
+// // Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this?
+//   return(prep.pl_block_index);
+// }
+
+
+void st_update_plan_block_parameters()
+{ 
+  if (pl_block != NULL) { // Ignore if at start of a new block.
+    // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes.
+    prep.partial_block_flag = true;
+    pl_block = NULL;
+  }
+}

config.h

@@ -80,14 +80,14 @@
 // CNC applications, the following multiplier value will work more than well enough. If you do have
 // happened to weird stepper motion issues, try modifying this value by adding or subtracting a 
 // zero and report it to the Grbl administrators. 
-#define INV_TIME_MULTIPLIER 10000000.0
+#define INV_TIME_MULTIPLIER 100000
 
 // Minimum stepper rate for the "Stepper Driver Interrupt". Sets the absolute minimum stepper rate 
 // in the stepper program and never runs slower than this value. If the INVE_TIME_MULTIPLIER value
 // changes, it will affect how this value works. So, if a zero is add/subtracted from the
 // INV_TIME_MULTIPLIER value, do the same to this value if you want to same response. 
 // NOTE: Compute by (desired_step_rate/60) * INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND. (mm/min)
-#define MINIMUM_STEP_RATE 1000L // Integer (mult*mm/isr_tic)
+// #define MINIMUM_STEP_RATE 1000L // Integer (mult*mm/isr_tic)
 
 // Minimum stepper rate. Only used by homing at this point. May be removed in later releases.
 #define MINIMUM_STEPS_PER_MINUTE 800 // (steps/min) - Integer value only
@@ -161,6 +161,14 @@
 // up with planning new incoming motions as they are executed. 
 // #define BLOCK_BUFFER_SIZE 18  // Uncomment to override default in planner.h.
 
+// Governs the size of the intermediary step segment buffer between the step execution algorithm
+// and the planner blocks. Each segment is set of steps executed at a constant velocity over a
+// fixed time defined by ACCELERATION_TICKS_PER_SECOND. They are computed such that the planner
+// block velocity profile is traced exactly. The size of this buffer governs how much step 
+// execution lead time there is for other Grbl processes have to compute and do their thing 
+// before having to come back and refill this buffer, currently at ~50msec of step moves.
+// #define SEGMENT_BUFFER_SIZE 7 // Uncomment to override default in stepper.h.
+
 // Line buffer size from the serial input stream to be executed. Also, governs the size of 
 // each of the startup blocks, as they are each stored as a string of this size. Make sure
 // to account for the available EEPROM at the defined memory address in settings.h and for

planner.c

@@ -35,7 +35,7 @@
                                  // 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 volatile uint8_t block_buffer_tail;       // Index of the block to process now
+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
@@ -52,7 +52,6 @@ static planner_t pl;
 
 
 // Returns the index of the next block in the ring buffer. Also called by stepper segment buffer.
-// NOTE: Removed modulo (%) operator, which uses an expensive divide and multiplication.
 uint8_t plan_next_block_index(uint8_t block_index) 
 {
   block_index++;
@@ -68,43 +67,6 @@ static uint8_t plan_prev_block_index(uint8_t block_index)
   block_index--;
   return(block_index);
 }
-                            
-
-// Update the entry speed and millimeters remaining to execute for a partially completed block. Called only
-// when the planner knows it will be changing the conditions of this block.
-// TODO: Set up to be called from planner calculations. Need supporting code framework still, i.e. checking
-//   and executing this only when necessary, combine with the block_buffer_safe pointer.
-// TODO: This is very similar to the planner reinitialize after a feed hold. Could make this do double duty.
-void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr)
-{
-// TODO: Need to make a condition to check if we need make these calculations. We don't if nothing has 
-//   been executed or placed into segment buffer. This happens with the first block upon startup or if 
-//   the segment buffer is exactly in between two blocks. Just check if the step_events_remaining is equal
-//   the total step_event_count in the block. If so, we don't have to do anything.
-
-  // !!! block index is the same as block_buffer_safe.
-  // See if we can reduce this down to just requesting the millimeters remaining..
-  uint8_t is_decelerating;
-  float millimeters_remaining = 0.0;
-  st_fetch_partial_block_parameters(block_index, &millimeters_remaining, &is_decelerating);
- 
-  if (millimeters_remaining != 0.0) {
-    // Point to current block partially executed by stepper algorithm
-    plan_block_t *partial_block = plan_get_block_by_index(block_index);
-
-    // Compute the midway speed of the partially completely block at the end of the segment buffer.
-    if (is_decelerating) { // Block is decelerating
-      partial_block->entry_speed_sqr = exit_speed_sqr - 2*partial_block->acceleration*millimeters_remaining;
-    } else { // Block is accelerating or cruising
-      partial_block->entry_speed_sqr += 2*partial_block->acceleration*(partial_block->millimeters-millimeters_remaining);
-      partial_block->entry_speed_sqr = min(partial_block->entry_speed_sqr, partial_block->nominal_speed_sqr);
-    }
-
-    // Update only the relevant planner block information so the planner can plan correctly.
-    partial_block->millimeters = millimeters_remaining;
-    partial_block->max_entry_speed_sqr = partial_block->entry_speed_sqr; // Not sure if this needs to be updated.
-  }
-}
 
 
 /*                            PLANNER SPEED DEFINITION                                              
@@ -153,17 +115,11 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr)
       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.
+      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 
-  NOTE: All planner computations are performed in floating point to minimize numerical round-off errors.
+      this requirement when encountered by the plan_discard_current_block() routine during a cycle.
-  When a planner block is executed, the floating point values are converted to fast integers by the stepper
-  algorithm segment buffer. See the stepper module for details. 
   
   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
@@ -180,175 +136,110 @@ void plan_update_partial_block(uint8_t block_index, float exit_speed_sqr)
 */
 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);
-
+        
-  // Query stepper module for safe planner block index to recalculate to, which corresponds to the end
+  // Bail. Can't do anything with one only one plan-able block.
-  // of the step segment buffer.
+  if (block_index == block_buffer_planned) { return; }
-  uint8_t block_buffer_safe = st_get_prep_block_index();
-    
-  // TODO: Make sure that we don't have to check for the block_buffer_tail condition, if the stepper module
-  // returns a NULL pointer or something. This could happen when the segment buffer is empty. Although,
-  // this call won't return a NULL, only an index.. I have to make sure that this index is synced with the
-  // planner at all times. 
-    
-  // Recompute plan only when there is more than one planner block in the buffer. Can't do anything with one.  
-  // NOTE: block_buffer_safe can be the last planner block if the segment buffer has completely queued up the
-  // remainder of the planner buffer. In this case, a new planner block will be treated as a single block.
-  if (block_index == block_buffer_safe) { // Also catches (head-1) = tail
-    
-    // Just set block_buffer_planned pointer.
-    block_buffer_planned = block_index;
-
-    // TODO: Feedrate override of one block needs to update the partial block with an exit speed of zero. For
-    //   a single added block and recalculate after a feed hold, we don't need to compute this, since we already
-    //   know that the velocity starts and ends at zero. With an override, we can be traveling at some midblock
-    //   rate, and we have to calculate the new velocity profile from it.
-    // plan_update_partial_block(block_index,0.0);
-           
-  } else {
-    
-    // TODO: If the nominal speeds change during a feedrate override, we need to recompute the max entry speeds for 
-    //       all junctions before proceeding.
-
-    // Initialize planner buffer pointers and indexing.
-    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);
       
-    // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
+  // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
-    // block in buffer. Cease planning when: (1) the last optimal planned pointer is reached.
+  // block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
-    // (2) the safe block pointer is reached, whereby the planned pointer is updated.
+  // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
-    // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
+  float entry_speed_sqr;
-    // NOTE: If the safe block is encountered before the planned block pointer, we know the safe block
+  plan_block_t *next;
-    // will be recomputed within the plan. So, we need to update it if it is partially completed.
+  plan_block_t *current = &block_buffer[block_index];
-    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.
+  // 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);
-      // Only two plannable blocks in buffer. Compute previous block based on 
+  
-      // !!! May only work if a new block is being added. Not for an override. The exit speed isn't zero.
+  block_index = plan_prev_block_index(block_index);
-      // !!! Need to make the current entry speed calculation after this.
+  if (block_index == block_buffer_planned) { // Only two plannable blocks in buffer. Reverse pass complete.
-      plan_update_partial_block(block_index, 0.0);
+    // Check if the first block is the tail. If so, notify stepper to update its current parameters.
-      block_buffer_planned = block_index;
+    if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); }
-    
+  } else { // Three or more plan-able blocks
-    } else {
+    while (block_index != block_buffer_planned) { 
-      
+      next = current;
-      // Three or more plan-able 
+      current = &block_buffer[block_index];
-      while (block_index != block_buffer_planned) { 
+      block_index = plan_prev_block_index(block_index);
 
-        next = current;
+      // Check if next block is the tail block(=planned block). If so, update current stepper parameters.
-        current = &block_buffer[block_index];
+      if (block_index == block_buffer_tail) { st_update_plan_block_parameters(); } 
 
-        // Increment block index early to check if the safe block is before the current block. If encountered,
+      // Compute maximum entry speed decelerating over the current block from its exit speed.
-        // this is an exit condition as we can't go further than this block in the reverse pass.
+      if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
-        block_index = plan_prev_block_index(block_index);
+        entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
-        if (block_index == block_buffer_safe) {
+        if (entry_speed_sqr < current->max_entry_speed_sqr) {
-          // Check if the safe block is partially completed. If so, update it before its exit speed 
+          current->entry_speed_sqr = entry_speed_sqr;
-          // (=current->entry speed) is over-written.
+        } else {
-          // TODO: The update breaks with feedrate overrides, because the replanning process no longer has
+          current->entry_speed_sqr = current->max_entry_speed_sqr;
-          // the previous nominal speed to update this block with. There will need to be something along the
-          // lines of a nominal speed change check and send the correct value to this function.
-          plan_update_partial_block(block_index,current->entry_speed_sqr);
-
-          // Set planned pointer at safe block and for loop exit after following computation is done.
-          block_buffer_planned = block_index; 
-        } 
-
-        // 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.
+  // Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
-    // Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
+  // Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
-    next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer
+  next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer
-    block_index = plan_next_block_index(block_buffer_planned); 
+  block_index = plan_next_block_index(block_buffer_planned); 
-    while (block_index != block_buffer_head) {
+  while (block_index != block_buffer_head) {
-      current = next;
+    current = next;
-      next = &block_buffer[block_index];
+    next = &block_buffer[block_index];
-      
+    
-      // Any acceleration detected in the forward pass automatically moves the optimal planned
+    // 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
+    // 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.
+    // can improve the plan from the buffer tail to the planned pointer by logic.
-      // TODO: Need to check if the planned flag logic is correct for all scenarios. It may not
+    if (current->entry_speed_sqr < next->entry_speed_sqr) {
-      // be for certain conditions. However, if the block reaches nominal speed, it can be a valid
+      entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
-      // breakpoint substitute.
+      // If true, current block is full-acceleration and we can move the planned pointer forward.
-      if (current->entry_speed_sqr < next->entry_speed_sqr) {
+      if (entry_speed_sqr < next->entry_speed_sqr) {
-        entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
+        next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
-        // If true, current block is full-acceleration and we can move the planned pointer forward.
+        block_buffer_planned = block_index; // Set optimal plan pointer.
-        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; // Set optimal plan pointer
-      }
-      
-      block_index = plan_next_block_index( block_index );
     }
     
-  }  
+    // 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; }
-void plan_reset_buffer()
+    block_index = plan_next_block_index( block_index );
-{
+  } 
-  block_buffer_planned = block_buffer_tail;
 }
 
 
 void plan_init() 
 {
+  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)
-  plan_reset_buffer();
+  block_buffer_planned = 0; // = block_buffer_tail;
-  memset(&pl, 0, sizeof(pl)); // Clear planner struct
 }
 
 
 void plan_discard_current_block() 
 {
   if (block_buffer_head != block_buffer_tail) { // Discard non-empty buffer.
-    block_buffer_tail = plan_next_block_index( block_buffer_tail );
+    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) { // Buffer empty
+  if (block_buffer_head == block_buffer_tail) { return(NULL); } // Buffer empty  
-    plan_reset_buffer();
-    return(NULL); 
-  } 
   return(&block_buffer[block_buffer_tail]);
 }
 
 
-plan_block_t *plan_get_block_by_index(uint8_t block_index) 
+float plan_get_exec_block_exit_speed()
 {
-  if (block_buffer_head == block_index) { return(NULL); }
+  uint8_t block_index = plan_next_block_index(block_buffer_tail);
-  return(&block_buffer[block_index]);
+  if (block_index == block_buffer_head) { return( 0.0 ); }
+  return( sqrt( block_buffer[block_index].entry_speed_sqr ) ); 
 }
 
 
@@ -508,12 +399,10 @@ void plan_buffer_line(float *target, float feed_rate, uint8_t invert_feed_rate)
   
   // Finish up by recalculating the plan with the new block.
   planner_recalculate();
-
+  
 // int32_t blength = block_buffer_head - block_buffer_tail;
 // if (blength < 0) { blength += BLOCK_BUFFER_SIZE; } 
 // printInteger(blength);
-
-
 }
 
 
@@ -527,68 +416,6 @@ void plan_sync_position()
 }
 
 
-/*       STEPPER VELOCITY PROFILE DEFINITION 
-                                                 less than nominal speed->  + 
-                    +--------+ <- nominal_speed                            /|\                                         
-                   /          \                                           / | \                      
-   entry_speed -> +            \                                         /  |  + <- next->entry_speed
-                  |             + <- next->entry_speed                  /   |  |                       
-                  +-------------+                       entry_speed -> +----+--+                   
-                   time -->  ^  ^                                           ^  ^                       
-                             |  |                                           |  |                       
-                     decelerate distance                           decelerate distance  
-                                                        
-  Calculates the type of velocity profile for a given planner block and provides the deceleration
-  distance for the stepper algorithm to use to accurately trace the profile exactly. The planner
-  computes the entry and exit speeds of each block, but does not bother to determine the details of
-  the velocity profiles within them, as they aren't needed for computing an optimal plan. When the
-  stepper algorithm begins to execute a block, the block velocity profiles are computed ad hoc. 
-  
-  Each block velocity profiles can be described as either a trapezoidal or a triangular shape. The
-  trapezoid occurs when the block reaches the nominal speed of the block and cruises for a period of
-  time. A triangle occurs when the nominal speed is not reached within the block. Both of these 
-  velocity profiles may also be truncated on either end with no acceleration or deceleration ramps, 
-  as they can be influenced by the conditions of neighboring blocks, where the acceleration ramps
-  are defined by constant acceleration equal to the maximum allowable acceleration of a block.
-  
-  Since the stepper algorithm already assumes to begin executing a planner block by accelerating
-  from the planner entry speed and cruise if the nominal speed is reached, we only need to know 
-  when to begin deceleration to the end of the block. Hence, only the distance from the end of the
-  block to begin a deceleration ramp is computed for the stepper algorithm when requested.
-*/
-float plan_calculate_velocity_profile(uint8_t block_index) 
-{  
-  plan_block_t *current_block = &block_buffer[block_index];
-  
-  // Determine current block exit speed
-  float exit_speed_sqr = 0.0; // Initialize for end of planner buffer. Zero speed.
-  plan_block_t *next_block = plan_get_block_by_index(plan_next_block_index(block_index));
-  if (next_block != NULL) { exit_speed_sqr = next_block->entry_speed_sqr; } // Exit speed is the entry speed of next buffer block
-  
-  // First determine intersection distance (in steps) from the exit point for a triangular profile.
-  // Computes: d_intersect = distance/2 + (v_entry^2-v_exit^2)/(4*acceleration)
-  float intersect_distance = 0.5*( current_block->millimeters + (current_block->entry_speed_sqr-exit_speed_sqr)/(2*current_block->acceleration) );  
-  
-  // Check if this is a pure acceleration block by a intersection distance less than zero. Also
-  // prevents signed and unsigned integer conversion errors.
-  if (intersect_distance > 0 ) {
-    float decelerate_distance;
-    // Determine deceleration distance (in steps) from nominal speed to exit speed for a trapezoidal profile.
-    // Value is never negative. Nominal speed is always greater than or equal to the exit speed.
-    // Computes: d_decelerate = (v_nominal^2 - v_exit^2)/(2*acceleration)
-    decelerate_distance = (current_block->nominal_speed_sqr - exit_speed_sqr)/(2*current_block->acceleration);
-
-    // The lesser of the two triangle and trapezoid distances always defines the velocity profile.
-    if (decelerate_distance > intersect_distance) { decelerate_distance = intersect_distance; }
-    
-    // Finally, check if this is a pure deceleration block.
-    if (decelerate_distance > current_block->millimeters) { return(0.0); }
-    else { return( (current_block->millimeters-decelerate_distance) ); }
-  }
-  return( current_block->millimeters ); // No deceleration in velocity profile.
-}        
-
-
 // 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(int32_t step_events_remaining) 
@@ -607,63 +434,3 @@ 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);
-  }
-}
-
-*/

planner.h

@@ -35,8 +35,8 @@ typedef struct {
   // Fields used by the bresenham algorithm for tracing the line
   // NOTE: Do not change any of these values once set. The stepper algorithm uses them to execute the block correctly.
   uint8_t direction_bits;            // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
-  int32_t steps[N_AXIS];             // Step count along each axis
+  int32_t steps[N_AXIS];            // Step count along each axis
-  int32_t step_event_count;          // The maximum step axis count and number of steps required to complete this block. 
+  int32_t step_event_count;         // The maximum step axis count and number of steps required to complete this block. 
 
   // Fields used by the motion planner to manage acceleration
   float entry_speed_sqr;             // The current planned entry speed at block junction in (mm/min)^2
@@ -44,10 +44,11 @@ typedef struct {
                                      //   neighboring nominal speeds with overrides in (mm/min)^2
   float max_junction_speed_sqr;      // Junction entry speed limit based on direction vectors in (mm/min)^2
   float nominal_speed_sqr;           // Axis-limit adjusted nominal speed for this block in (mm/min)^2
-  float acceleration;                // Axis-limit adjusted line acceleration in mm/min^2
+  float acceleration;                // Axis-limit adjusted line acceleration in (mm/min^2)
-  float millimeters;                 // The remaining distance for this block to be executed in mm
+  float millimeters;                 // The remaining distance for this block to be executed in (mm)
   
 } plan_block_t;
+
       
 // Initialize the motion plan subsystem
 void plan_init();
@@ -64,13 +65,11 @@ void plan_discard_current_block();
 // Gets the current block. Returns NULL if buffer empty
 plan_block_t *plan_get_current_block();
 
+// Called periodically by step segment buffer. Mostly used internally by planner.
 uint8_t plan_next_block_index(uint8_t block_index);
 
-plan_block_t *plan_get_block_by_index(uint8_t block_index);
+// Called by step segment buffer when computing executing block velocity profile.
-
+float plan_get_exec_block_exit_speed();
-float plan_calculate_velocity_profile(uint8_t block_index);
-
-// void plan_update_partial_block(uint8_t block_index, float millimeters_remaining, uint8_t is_decelerating);
 
 // Reset the planner position vector (in steps)
 void plan_sync_position();

stepper.c

@@ -26,25 +26,41 @@
 #include "planner.h"
 #include "nuts_bolts.h"
 
+
 // Some useful constants
 #define TICKS_PER_MICROSECOND (F_CPU/1000000)
+#define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0)) // min/segment
 
-#define RAMP_NOOP_CRUISE 0
+#define RAMP_ACCEL 0
-#define RAMP_ACCEL 1
+#define RAMP_CRUISE 1
 #define RAMP_DECEL 2
 
-#define LOAD_NOOP 0
-#define LOAD_SEGMENT 1
-#define LOAD_BLOCK 2
 
-#define SEGMENT_NOOP 0
+// Stores the planner block Bresenham algorithm execution data for the segments in the segment 
-#define SEGMENT_END_OF_BLOCK bit(0)
+// buffer. Normally, this buffer is partially in-use, but, for the worst case scenario, it will
-#define RAMP_CHANGE_ACCEL bit(1)
+// never exceed the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1).
-#define RAMP_CHANGE_DECEL bit(2)
+// NOTE: This data is copied from the prepped planner blocks so that the planner blocks may be
+// discarded when entirely consumed and completed by the segment buffer.
+typedef struct {  
+  uint8_t direction_bits;
+  int32_t steps[N_AXIS];
+  int32_t step_event_count;
+} st_block_t;
+static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE-1];
+// TODO: Directly adjust this parameters to stop motion of individual axes for the homing cycle.
+// But this may require this to be volatile if it is controlled by an interrupt.
 
-#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change
+// Primary stepper segment ring buffer. Contains small, short line segments for the stepper 
-
+// algorithm to execute, which are "checked-out" incrementally from the first block in the
-#define SEGMENT_BUFFER_SIZE 6
+// planner buffer. Once "checked-out", the steps in the segments buffer cannot be modified by 
+// the planner, where the remaining planner block steps still can.
+typedef struct {
+  uint8_t n_step;         // Number of step events to be executed for this segment
+  uint8_t st_block_index; // Stepper block data index. Uses this information to execute this segment.
+  int32_t phase_dist;     // Remaining step fraction to tick before completing segment.
+  int32_t dist_per_tick;  // Step distance traveled per ISR tick, aka step rate.
+} segment_t;
+static segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
 
 // Stepper state variable. Contains running data and trapezoid variables.
 typedef struct {
@@ -52,84 +68,90 @@ typedef struct {
   int32_t counter_x,        // Counter variables for the bresenham line tracer
           counter_y, 
           counter_z;
-  uint8_t segment_steps_remaining;  // Steps remaining in line segment motion
 
   // Used by inverse time algorithm to track step rate
-  int32_t counter_dist;       // Inverse time distance traveled since last step event
+  int32_t counter_dist;     // Inverse time distance traveled since last step event
-  uint32_t ramp_rate;        // Inverse time distance traveled per interrupt tick
-  uint32_t dist_per_tick;
   
   // Used by the stepper driver interrupt
-  uint8_t execute_step;    // Flags step execution for each interrupt.
+  uint8_t execute_step;     // Flags step execution for each interrupt.
-  uint8_t step_pulse_time; // Step pulse reset time after step rise
+  uint8_t step_pulse_time;  // Step pulse reset time after step rise
-  uint8_t out_bits;        // The next stepping-bits to be output
+  uint8_t out_bits;         // The next stepping-bits to be output
-  uint8_t load_flag;
+
-  
+  uint8_t step_count;       // Steps remaining in line segment motion  
-  uint8_t counter_ramp;
+  uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block.
-  uint8_t ramp_type;
+  st_block_t *exec_block;   // Pointer to the block data for the segment being executed
+  segment_t *exec_segment;  // Pointer to the segment being executed
 } stepper_t;
 static stepper_t st;
 
-// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the
-// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs.
-// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed
-// 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_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)
-  uint32_t current_approx_rate;  // Tracks the approximate segment rate to predict steps per segment to execute
-  int32_t decelerate_after;      // Tracks when to initiate deceleration according to the planner block
-  float mm_per_step;
-} st_data_t;
-static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1];
-
-// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute,
-// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps
-// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can.
-typedef struct {
-  uint8_t n_step;         // Number of step events to be executed for this segment
-  uint8_t st_data_index;  // Stepper buffer common data index. Uses this information to execute this segment.
-  uint8_t flag;           // Stepper algorithm bit-flag for special execution conditions.
-} st_segment_t;
-static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
-
 // Step segment ring buffer indices
 static volatile uint8_t segment_buffer_tail;
-static volatile uint8_t segment_buffer_head;
+static uint8_t segment_buffer_head;
 static uint8_t segment_next_head;
 
-static volatile uint8_t busy;   // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
+// Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
-static plan_block_t *pl_current_block;  // A pointer to the planner block currently being traced
+static volatile uint8_t busy;   
-static st_segment_t *st_current_segment;
-static st_data_t *st_current_data;
 
 // Pointers for the step segment being prepped from the planner buffer. Accessed only by the
 // main program. Pointers may be planning segments or planner blocks ahead of what being executed.
-static plan_block_t *pl_prep_block;   // Pointer to the planner block being prepped
+static plan_block_t *pl_block;     // Pointer to the planner block being prepped
-static st_data_t *st_prep_data;       // Pointer to the stepper common data being prepped
+static st_block_t *st_prep_block;  // Pointer to the stepper block data being prepped 
-static uint8_t pl_prep_index;         // Index of planner block being prepped
+
-static uint8_t st_data_prep_index;    // Index of stepper common data block being prepped
+// Segment preparation data struct. Contains all the necessary information to compute new segments
-static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block
+// based on the current executing planner block.
+typedef struct {
+  uint8_t st_block_index;      // Index of stepper common data block being prepped
+  uint8_t partial_block_flag;  // Flag indicating the planner has modified the prepped planner block
+
+  float step_per_mm;           // Current planner block step/millimeter conversion scalar
+  float step_events_remaining; // Tracks step event count for the executing planner block
+  
+  uint8_t ramp_type;      // Current segment ramp state
+  float current_speed;    // Current speed at the end of the segment buffer (mm/min)
+  float maximum_speed;    // Maximum speed of executing block. Not always nominal speed. (mm/min)
+  float exit_speed;       // Exit speed of executing block (mm/min)
+  float accelerate_until; // Acceleration ramp end measured from end of block (mm)
+  float decelerate_after; // Deceleration ramp start measured from end of block (mm)
+} st_prep_t;
+static st_prep_t prep;
 
 
-/*        __________________________
+/*    BLOCK VELOCITY PROFILE DEFINITION 
+          __________________________
          /|                        |\     _________________         ^
         / |                        | \   /|               |\        |
        /  |                        |  \ / |               | \       s
       /   |                        |   |  |               |  \      p
      /    |                        |   |  |               |   \     e
     +-----+------------------------+---+--+---------------+----+    e
-    |               BLOCK 1            |      BLOCK 2          |    d
+    |               BLOCK 1            ^      BLOCK 2          |    d
-
+                                       |
-                            time ----->
+                  time ----->      EXAMPLE: Block 2 entry speed is at max junction velocity
   
-   The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
+  The planner block buffer is planned assuming constant acceleration velocity profiles and are
-   until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
+  continuously joined at block junctions as shown above. However, the planner only actively computes
-   after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
+  the block entry speeds for an optimal velocity plan, but does not compute the block internal
-   +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
+  velocity profiles. These velocity profiles are computed ad-hoc as they are executed by the 
+  stepper algorithm and consists of only 7 possible types of profiles: cruise-only, cruise-
+  deceleration, acceleration-cruise, acceleration-only, deceleration-only, full-trapezoid, and 
+  triangle(no cruise).
+
+                                        maximum_speed (< nominal_speed) ->  + 
+                    +--------+ <- maximum_speed (= nominal_speed)          /|\                                         
+                   /          \                                           / | \                      
+ current_speed -> +            \                                         /  |  + <- exit_speed
+                  |             + <- exit_speed                         /   |  |                       
+                  +-------------+                     current_speed -> +----+--+                   
+                   time -->  ^  ^                                           ^  ^                       
+                             |  |                                           |  |                       
+                decelerate_after(in mm)                             decelerate_after(in mm)
+                    ^           ^                                           ^  ^
+                    |           |                                           |  |
+                accelerate_until(in mm)                             accelerate_until(in mm)
+                    
+  The step segment buffer computes the executing block velocity profile and tracks the critical
+  parameters for the stepper algorithm to accurately trace the profile. These critical parameters 
+  are shown and defined in the above illustration.
 */
 
 // Stepper state initialization. Cycle should only start if the st.cycle_start flag is
@@ -147,10 +169,8 @@ void st_wake_up()
     st.out_bits = settings.invert_mask; 
     // Initialize step pulse timing from settings.
     st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
-    // Enable stepper driver interrupt
-    st.execute_step = false;
-    st.load_flag = LOAD_BLOCK;
     
+    // Enable stepper driver interrupt
     TCNT2 = 0; // Clear Timer2
     TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
     TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
@@ -199,14 +219,8 @@ void st_go_idle()
 /* TODO: 
    - Measure time in ISR. Typical and worst-case. Should be virtually identical to last algorithm.
      There are no major changes to the base operations of this ISR with the new segment buffer.
-   - Write how the acceleration counters work and why they are set at half via mid-point rule.
    - Determine if placing the position counters elsewhere (or change them to 8-bit variables that
      are added to the system position counters at the end of a segment) frees up cycles.
-   - Write a blurb about how the acceleration should be handled within the ISR. All of the 
-     time/step/ramp counters accurately keep track of the remainders and phasing of the variables
-     with time. This means we do not have to compute them via expensive floating point beforehand.
-   - Need to do an analysis to determine if these counters are really that much cheaper. At least
-     find out when it isn't anymore. Particularly when the ISR is at a very high frequency.
    - Create NOTE: to describe that the total time in this ISR must be less than the ISR frequency
      in its worst case scenario.
 */
@@ -229,150 +243,94 @@ ISR(TIMER2_COMPA_vect)
          // NOTE: The remaining code in this ISR will finish before returning to main program.
     
   // If there is no step segment, attempt to pop one from the stepper buffer
-  if (st.load_flag != LOAD_NOOP) {
+  if (st.exec_segment == NULL) {
     
     // Anything in the buffer? If so, load and initialize next step segment.
     if (segment_buffer_head != segment_buffer_tail) {
       
       // Initialize new step segment and load number of steps to execute
-      st_current_segment = &segment_buffer[segment_buffer_tail];
+      st.exec_segment = &segment_buffer[segment_buffer_tail];
-      st.segment_steps_remaining = st_current_segment->n_step;
+      st.step_count = st.exec_segment->n_step;
     
       // If the new segment starts a new planner block, initialize stepper variables and counters.
-      // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous.
+      // NOTE: When the segment data index changes, this indicates a new planner block.
-      if (st.load_flag == LOAD_BLOCK) {
+      if ( st.exec_block_index != st.exec_segment->st_block_index ) {
-        pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this.
+        st.exec_block_index = st.exec_segment->st_block_index;
-        st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index];
+        st.exec_block = &st_block_buffer[st.exec_block_index];
         
         // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick.
-        st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask;
+        st.out_bits = st.exec_block->direction_bits ^ settings.invert_mask;
         st.execute_step = true;
         
-        // Initialize Bresenham line counters
+        // Initialize Bresenham line and distance counters
-        st.counter_x = (pl_current_block->step_event_count >> 1);
+        st.counter_x = (st.exec_block->step_event_count >> 1);
         st.counter_y = st.counter_x;
         st.counter_z = st.counter_x;
-        
+        st.counter_dist = 0;         
-        // Initialize inverse time, step rate data, and acceleration ramp counters
-        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. 
-
-        // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE.
-        if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; }
-        else { st.dist_per_tick = st.ramp_rate; }
-      }
-
-      // Check if ramp conditions have changed. If so, update ramp counters and control variables.
-      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 
-           ramp is executed exactly as the plan dictates. Otherwise, when a ramp begins from a known
-           rate (nominal/cruise or initial), the ramp count must be set to ISR_TICKS_PER_ACCELERATION_TICK/2
-           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 & RAMP_CHANGE_DECEL ) { st.ramp_type = RAMP_DECEL; }
-        else { st.ramp_type = RAMP_ACCEL; }
       }
       
-      st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution.
-
     } else {
-      // Can't discard planner block here if a feed hold stops in middle of block.
+      // Segment buffer empty. Shutdown.
       st_go_idle();
       bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
       return; // Nothing to do but exit.
     }  
     
   } 
-  
+    
-  // Adjust inverse time counter for ac/de-celerations
-  if (st.ramp_type) { // Ignored when ramp type is RAMP_NOOP_CRUISE
-    st.counter_ramp--; // Tick acceleration ramp counter
-    if (st.counter_ramp == 0) { // Adjust step rate when its time
-      st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK; // Reload ramp counter
-      if (st.ramp_type == RAMP_ACCEL) { // Adjust velocity for acceleration
-        st.ramp_rate += st_current_data->rate_delta;
-        if (st.ramp_rate >= st_current_data->nominal_rate) { // Reached nominal rate.
-          st.ramp_rate = st_current_data->nominal_rate; // Set cruising velocity
-          st.ramp_type = RAMP_NOOP_CRUISE; // Set ramp flag to cruising
-          st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; // Re-initialize counter for next ramp change.
-        }
-      } else { // Adjust velocity for deceleration.
-        if (st.ramp_rate > st_current_data->rate_delta) {
-          st.ramp_rate -= st_current_data->rate_delta;
-        } else { // Moving near zero feed rate. Gracefully slow down.
-          st.ramp_rate >>= 1; // Integer divide by 2 until complete. Also prevents overflow.
-        }
-      }
-      // Adjust for minimum step rate, but retain operating ramp rate for accurate velocity tracing.
-      if (st.ramp_rate < MINIMUM_STEP_RATE) { st.dist_per_tick = MINIMUM_STEP_RATE; }
-      else { st.dist_per_tick = st.ramp_rate; }
-    }
-  }
-  
   // Iterate inverse time counter. Triggers each Bresenham step event.
-  st.counter_dist -= st.dist_per_tick; 
+  st.counter_dist += st.exec_segment->dist_per_tick; 
   
   // Execute Bresenham step event, when it's time to do so.
-  if (st.counter_dist < 0) {  
+  if (st.counter_dist > INV_TIME_MULTIPLIER) {
-    st.counter_dist += st_current_data->dist_per_step; // Reload inverse time counter
+    if (st.step_count > 0) {  // Block phase correction from executing step. 
+      st.counter_dist -= INV_TIME_MULTIPLIER; // Reload inverse time counter
+      st.step_count--; // Decrement step events count    
 
-    st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits
+      // Execute step displacement profile by Bresenham line algorithm
-    st.execute_step = true;
+      st.execute_step = true;
-    
+      st.out_bits = st.exec_block->direction_bits; // Reset out_bits and reload direction bits      
-    // Execute step displacement profile by Bresenham line algorithm
+      st.counter_x -= st.exec_block->steps[X_AXIS];
-    st.counter_x -= pl_current_block->steps[X_AXIS];
+      if (st.counter_x < 0) {
-    if (st.counter_x < 0) {
+        st.out_bits |= (1<<X_STEP_BIT);
-      st.out_bits |= (1<<X_STEP_BIT);
+        st.counter_x += st.exec_block->step_event_count;
-      st.counter_x += pl_current_block->step_event_count;
+        if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
-      if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
+        else { sys.position[X_AXIS]++; }
-      else { sys.position[X_AXIS]++; }
-    }
-    st.counter_y -= pl_current_block->steps[Y_AXIS];
-    if (st.counter_y < 0) {
-      st.out_bits |= (1<<Y_STEP_BIT);
-      st.counter_y += pl_current_block->step_event_count;
-      if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
-      else { sys.position[Y_AXIS]++; }
-    }
-    st.counter_z -= pl_current_block->steps[Z_AXIS];
-    if (st.counter_z < 0) {
-      st.out_bits |= (1<<Z_STEP_BIT);
-      st.counter_z += pl_current_block->step_event_count;
-      if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
-      else { sys.position[Z_AXIS]++; }
-    }
-
-    // Check step events for trapezoid change or end of block.
-    st.segment_steps_remaining--; // Decrement step events count    
-    if (st.segment_steps_remaining == 0) {
-      // Line move is complete, set load line flag to check for new move.
-      // Check if last line move in planner block. Discard if so.
-      if (st_current_segment->flag & SEGMENT_END_OF_BLOCK) {
-        plan_discard_current_block();
-        st.load_flag = LOAD_BLOCK;
-      } else {
-        st.load_flag = LOAD_SEGMENT;
       }
-      
+      st.counter_y -= st.exec_block->steps[Y_AXIS];
-      // Discard current segment by advancing buffer tail index
+      if (st.counter_y < 0) {
+        st.out_bits |= (1<<Y_STEP_BIT);
+        st.counter_y += st.exec_block->step_event_count;
+        if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
+        else { sys.position[Y_AXIS]++; }
+      }
+      st.counter_z -= st.exec_block->steps[Z_AXIS];
+      if (st.counter_z < 0) {
+        st.out_bits |= (1<<Z_STEP_BIT);
+        st.counter_z += st.exec_block->step_event_count;
+        if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
+        else { sys.position[Z_AXIS]++; }
+      }  
+      st.out_bits ^= settings.invert_mask;  // Apply step port invert mask    
+    }
+  }
+  
+  if (st.step_count == 0) {
+    if (st.counter_dist > st.exec_segment->phase_dist) { 
+      // Segment is complete. Discard current segment and advance segment indexing.
+      st.exec_segment = NULL;
       if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; }
     }
-
-    st.out_bits ^= settings.invert_mask;  // Apply step port invert mask    
   }
+  
   busy = false;
 // SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
 }
 
 
-// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the step
+/* The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the step
-// pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
+   pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
-// finish, if Timer2 is disabled after completing a move.
+   finish, if Timer2 is disabled after completing a move. */
 ISR(TIMER0_OVF_vect)
 {
   STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); 
@@ -383,21 +341,15 @@ ISR(TIMER0_OVF_vect)
 // Reset and clear stepper subsystem variables
 void st_reset()
 {
+  memset(&prep, 0, sizeof(prep));
   memset(&st, 0, sizeof(st));
-  
+  st.exec_segment = NULL;
-  st.load_flag = LOAD_BLOCK;
+  pl_block = NULL;  // Planner block pointer used by segment buffer
-  busy = false;
-
-  pl_current_block = NULL; // Planner block pointer used by stepper algorithm
-  pl_prep_block = NULL;  // Planner block pointer used by segment buffer
-  pl_prep_index =   0; // Planner buffer indices are also reset to zero.
-  st_data_prep_index = 0;
   
   segment_buffer_tail = 0;
   segment_buffer_head = 0; // empty = tail
   segment_next_head = 1;
-    
+  busy = false;
-  pl_partial_block_flag = false;
 }
 
 
@@ -445,6 +397,8 @@ void st_feed_hold()
 {
   if (sys.state == STATE_CYCLE) {
     sys.state = STATE_HOLD;
+    st_update_plan_block_parameters();
+    st_prep_buffer();
     sys.auto_start = false; // Disable planner auto start upon feed hold.
   }
 }
@@ -457,17 +411,8 @@ void st_feed_hold()
 // Only the planner de/ac-celerations profiles and stepper rates have been updated.
 void st_cycle_reinitialize()
 {
-//   if (pl_current_block != NULL) {
+//   if (pl_current_block != NULL) {    
-    // Replan buffer from the feed hold stop location.
+//     plan_cycle_reinitialize(st_exec_block->step_events_remaining);
-    
-    // TODO: Need to add up all of the step events in the current planner block to give 
-    // back to the planner. Should only need it for the current block. 
-    // BUT! The planner block millimeters is all changed and may be changed into the next
-    // planner block. The block millimeters would need to be recalculated via step counts
-    // and the mm/step variable.
-    // OR. Do we plan the feed hold itself down with the planner.
-    
-//     plan_cycle_reinitialize(st_current_data->step_events_remaining);
 //     st.ramp_type = RAMP_ACCEL;
 //     st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; 
 //     st.ramp_rate = 0;
@@ -480,6 +425,17 @@ void st_cycle_reinitialize()
 }
 
 
+// Called by planner_recalculate() when the executing block is updated by the new plan.
+void st_update_plan_block_parameters()
+{ 
+  if (pl_block != NULL) { // Ignore if at start of a new block.
+    pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed; // Update entry speed.
+    pl_block = NULL; // Flag st_prep_segment() to load new velocity profile.
+    prep.partial_block_flag = true; // Flag st_prep_segment() to indicate block continuation.
+  }
+}
+
+
 /* Prepares step segment buffer. Continuously called from main program. 
 
    The segment buffer is an intermediary buffer interface between the execution of steps
@@ -490,214 +446,247 @@ void st_cycle_reinitialize()
    The number of steps "checked-out" from the planner buffer and the number of segments in
    the segment buffer is sized and computed such that no operation in the main program takes
    longer than the time it takes the stepper algorithm to empty it before refilling it. 
-   Currently, the segment buffer conservatively holds roughly up to 40-60 msec of steps. 
+   Currently, the segment buffer conservatively holds roughly up to 40-50 msec of steps. 
 
-   NOTE: The segment buffer executes a set number of steps over an approximate time period.
+   NOTE: The segment buffer executes a computed number of steps over a configured segment 
-   If we try to execute over a fixed time period, it is difficult to guarantee or predict 
+   execution time period, except at an end of a planner block where the segment execution
-   how many steps will execute over it, especially when the step pulse phasing between the
+   gets truncated by the lack of travel distance. Since steps are integer values and to keep
-   neighboring segments must also be kept consistent. Meaning that, if the last segment step 
+   the distance traveled over the segment exact, a fractional step remaining after the last
-   pulses right before a segment end, the next segment must delay its first pulse so that the
+   executed step in a segment is handled by allowing the stepper algorithm distance counters
-   step pulses are consistently spaced apart over time to keep the step pulse train nice and
+   to tick to this fractional value without triggering a full step. So, when the next segment
-   smooth. Keeping track of phasing and ensuring that the exact number of steps are executed
+   is loaded for execution, its first full step will already have the distance counters primed
-   as defined by the planner block, the related computational overhead can get quickly and
+   with the previous segment fractional step and will execute exactly on time according to 
-   prohibitively expensive, especially in real-time.
+   the planner block velocity profile. This ensures the step phasing between segments are kept
-     Since the stepper algorithm automatically takes care of the step pulse phasing with
+   in sync and prevents artificially created accelerations between segments if they are not
-   its ramp and inverse time counters by retaining the count remainders, we don't have to
+   accounted for. This allows the stepper algorithm to run at very high step rates without
-   explicitly and expensively track and synchronize the exact number of steps, time, and
+   losing steps.
-   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. 
 */
-
 /* 
-   TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. 
+   TODO: With feedrate overrides, increases to the override value will not significantly
-     The problem is that when an override is reduced, the planner may not plan back to 
+     change the planner and stepper current operation. When the value increases, we simply
-     the current rate. Meaning that the velocity profiles for certain conditions no longer
+     need to recompute the block plan with new nominal speeds and maximum junction velocities.
-     are trapezoidal or triangular. For example, if the current block is cruising at a
+     However with a decreasing feedrate override, this gets a little tricky. The current block
-     nominal rate and the feedrate override is reduced, the new nominal rate will now be
+     plan is optimal, so if we try to reduce the feed rates, it may be impossible to create 
-     lower. The velocity profile must first decelerate to the new nominal rate and then 
+     a feasible plan at its current operating speed and decelerate down to zero at the end of
-     follow on the new plan. So the remaining velocity profile will have a decelerate, 
+     the buffer. We first have to enforce a deceleration to meet and intersect with the reduced
-     cruise, and another decelerate.
+     feedrate override plan. For example, if the current block is cruising at a nominal rate
+     and the feedrate override is reduced, the new nominal rate will now be lower. The velocity
+     profile must first decelerate to the new nominal rate and then follow on the new plan. 
         Another issue is whether or not a feedrate override reduction causes a deceleration
-     that acts over several planner blocks. For example, say that the plan is already 
+     that acts over several planner blocks. For example, say that the plan is already heavily
-     heavily decelerating throughout it, reducing the feedrate will not do much to it. So,
+     decelerating throughout it, reducing the feedrate override will not do much to it. So,
-     how do we determine when to resume the new plan? How many blocks do we have to wait 
+     how do we determine when to resume the new plan? One solution is to tie into the feed hold
-     until the new plan intersects with the deceleration curve? One plus though, the 
+     handling code to enforce a deceleration, but check when the current speed is less than or
-     deceleration will never be more than the number of blocks in the entire planner buffer,
+     equal to the block maximum speed and is in an acceleration or cruising ramp. At this 
-     but it theoretically can be equal to it when all planner blocks are decelerating already.
+     point, we know that we can recompute the block velocity profile to meet and continue onto
+     the new block plan.
 */
 void st_prep_buffer()
 {
   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
-    st_segment_t *prep_segment = &segment_buffer[segment_buffer_head];
-    prep_segment->flag = SEGMENT_NOOP;
-    
-    // Determine if we need to load a new planner block.
-    if (pl_prep_block == NULL) {
-      pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block
-      if (pl_prep_block == NULL) { return; } // No planner blocks. Exit.
-        
-      // Increment stepper common data index 
-      if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; }
-        
-      // Check if the planner has re-computed this block mid-execution. If so, push the previous segment
-      // data. Otherwise, prepare a new segment data for the new planner block.
-      if (pl_partial_block_flag) {
-        
-        // Prepare new shared segment block data and copy the relevant last segment block data.
-        st_data_t *last_st_prep_data;
-        last_st_prep_data = st_prep_data;
-        st_prep_data = &segment_data[st_data_prep_index];
-            
-        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_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;
 
-        pl_partial_block_flag = false; // Reset flag
+    // Determine if we need to load a new planner block. If so, prepare step data.
-        
+    if (pl_block == NULL) {
+      pl_block = plan_get_current_block(); // Query planner for a queued block
+      if (pl_block == NULL) { return; } // No planner blocks. Exit.
+      
+// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; 
+                
+      // Check if the planner has re-computed this block mid-execution.
+      if (prep.partial_block_flag) {
+        // Retain last Bresenham data, but recompute the velocity profile.      
+        prep.partial_block_flag = false; // Reset flag
       } else {
-      
+        // Increment stepper common data index 
-        // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since 
+        if ( ++prep.st_block_index == (SEGMENT_BUFFER_SIZE-1) ) { prep.st_block_index = 0; }
-        // the planner buffer can dynamically change the velocity profile data as blocks are added.
-        st_prep_data = &segment_data[st_data_prep_index];
-      
-        // Initialize Bresenham variables
-        st_prep_data->step_events_remaining = pl_prep_block->step_event_count;
 
-        // Convert planner block velocity profile data to stepper rate and step distance data.
+        // Prepare and copy Bresenham algorithm segment data from the new planner block, so that
-        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)
+        // when the segment buffer completes the planner block, it may be discarded immediately.
-        st_prep_data->rate_delta = ceil(pl_prep_block->acceleration*
+        st_prep_block = &st_block_buffer[prep.st_block_index];
-          ((INV_TIME_MULTIPLIER/(60.0*60.0))/(ISR_TICKS_PER_SECOND*ACCELERATION_TICKS_PER_SECOND))); // (mult*mm/isr_tic/accel_tic)
+        st_prep_block->steps[X_AXIS] = pl_block->steps[X_AXIS];
-        st_prep_data->dist_per_step = ceil((pl_prep_block->millimeters*INV_TIME_MULTIPLIER)/pl_prep_block->step_event_count); // (mult*mm/step)
+        st_prep_block->steps[Y_AXIS] = pl_block->steps[Y_AXIS];
-        
+        st_prep_block->steps[Z_AXIS] = pl_block->steps[Z_AXIS];
-        // TODO: Check if we really need to store this.
+        st_prep_block->direction_bits = pl_block->direction_bits;
-        st_prep_data->mm_per_step = pl_prep_block->millimeters/pl_prep_block->step_event_count;        
+        st_prep_block->step_event_count = pl_block->step_event_count;
-    
+
+        // Initialize planner block step count, unit distance data, and remainder tracker.
+        prep.step_per_mm = st_prep_block->step_event_count/pl_block->millimeters;        
+        prep.step_events_remaining = st_prep_block->step_event_count;
       }
       
-      // Convert planner entry speed to stepper initial rate. 
+      // Compute velocity profile for a feed hold in-process.
-      st_prep_data->initial_rate = ceil(sqrt(pl_prep_block->entry_speed_sqr)*(INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))); // (mult*mm/isr_tic)
+//       prep.current_speed = sqrt(pl_block->entry_speed_sqr); // !! For a new block, this needs to be updated.
-
+//       float inv_2_accel = 0.5/pl_block->acceleration;
-      // TODO: Nominal rate changes with feedrate override.
+//       if (sys.state == STATE_HOLD) {
-      // 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)
+//         prep.maximum_speed = prep.current_speed;
-
+//         prep.decelerate_after = pl_block->millimeters;
-      st_prep_data->current_approx_rate = st_prep_data->initial_rate;
+//         prep.accelerate_until = pl_block->millimeters;
-
+//         prep.ramp_type = DECEL_RAMP; // or FEED_HOLD_RAMP?
-      // Calculate the planner block velocity profile type, determine deceleration point, and initial ramp.
+//         float decelerate_distance = inv_2_accel*pl_block->entry_speed_sqr;
-      float mm_decelerate_after = plan_calculate_velocity_profile(pl_prep_index);
+//         if (decelerate_distance > pl_block->millimeters) {
-      st_prep_data->decelerate_after = ceil( mm_decelerate_after/st_prep_data->mm_per_step );
+//           // Keep decelerating through to the end of the block.
-      if (st_prep_data->decelerate_after > 0) { // If 0, RAMP_CHANGE_DECEL flag is set later.
+//           // !! Need to update the next block's entry speed with current speed when loaded.
-        if (st_prep_data->initial_rate != st_prep_data->nominal_rate) { prep_segment->flag = RAMP_CHANGE_ACCEL; }
+//           prep.exit_speed = sqrt(pl_block->entry_speed_sqr - 2*pl_block->acceleration*pl_block->millimeters);
+//         } else {
+//           prep.exit_speed = 0.0;
+//           ***millimeters = decelerate_distance; // !! Need separate millimeters to track.
+//           // !! When target mm is reached, don't discard block until it really is at its end.
+//           // Return state to QUEUED and replan remaining buffer. That's about it.
+//         }
+//       } else {  
+              
+      // Compute the critical velocity profile parameters of the prepped planner block.
+      prep.current_speed = sqrt(pl_block->entry_speed_sqr);
+      prep.exit_speed = plan_get_exec_block_exit_speed();   
+      prep.ramp_type = RAMP_ACCEL; // Initialize as acceleration ramp.
+      float exit_speed_sqr = prep.exit_speed*prep.exit_speed;
+      float inv_2_accel = 0.5/pl_block->acceleration;
+      float intersection_dist =
+              0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr));
+      if (intersection_dist > 0.0) {
+        if (intersection_dist < pl_block->millimeters) { // Either trapezoid or triangle types
+          // NOTE: For acceleration-cruise trapezoid, following calculation will be 0.0.
+          prep.decelerate_after = inv_2_accel*(pl_block->nominal_speed_sqr-exit_speed_sqr);
+          if (prep.decelerate_after < intersection_dist) { // Trapezoid type
+            prep.maximum_speed = sqrt(pl_block->nominal_speed_sqr);
+            if (pl_block->entry_speed_sqr == pl_block->nominal_speed_sqr) { 
+              // Cruise-deceleration or cruise-only type.
+              prep.ramp_type = RAMP_CRUISE;
+              prep.accelerate_until = pl_block->millimeters; 
+            } else {
+              // Full-trapezoid or acceleration-cruise types
+              prep.accelerate_until = 
+                pl_block->millimeters-inv_2_accel*(pl_block->nominal_speed_sqr-pl_block->entry_speed_sqr); 
+            }
+          } else { // Triangle type
+            prep.accelerate_until = intersection_dist;
+            prep.decelerate_after = intersection_dist;
+            prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersection_dist+exit_speed_sqr);
+          }          
+        } else { // Deceleration-only type
+          prep.ramp_type = RAMP_DECEL;
+          prep.maximum_speed = prep.current_speed;
+          prep.accelerate_until = pl_block->millimeters;
+          prep.decelerate_after = pl_block->millimeters;
+        }
+      } else { // Acceleration-only type
+        prep.maximum_speed = prep.exit_speed;
+        prep.accelerate_until = 0.0;
+        prep.decelerate_after = 0.0;
       }
+      
     }
 
+    // Initialize new segment
+    segment_t *prep_segment = &segment_buffer[segment_buffer_head];
+
     // Set new segment to point to the current segment data block.
-    prep_segment->st_data_index = st_data_prep_index;
+    prep_segment->st_block_index = prep.st_block_index;
 
-    // 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.
+       Compute the average velocity of this new segment by determining the total distance
-    // We do this to minimize memory and computational requirements. However, this could easily be replaced with
+       traveled over the segment time DT_SEGMENT. The follow code first attempts to create 
-    // a more exact approximation or have an user-defined time per segment, if CPU and memory overhead allows.
+       a full segment based on the current ramp conditions. If the segment time is incomplete 
-    if (st_prep_data->decelerate_after <= 0) {
+       by terminating at a ramp state change, the code will continue to loop through the
-      if (st_prep_data->decelerate_after == 0) { prep_segment->flag = RAMP_CHANGE_DECEL; } // Set segment deceleration flag
+       progressing ramp states to fill the remaining segment execution time. However, if 
-      else { st_prep_data->current_approx_rate -= st_prep_data->rate_delta; }
+       an incomplete segment terminates at the end of the planner block, the segment is 
-      if (st_prep_data->current_approx_rate < st_prep_data->rate_delta) { st_prep_data->current_approx_rate >>= 1; }
+       considered completed despite having a truncated execution time less than DT_SEGMENT. 
-    } else {    
+    */   
-      if (st_prep_data->current_approx_rate < st_prep_data->nominal_rate) {
+    float dt = 0.0;
-        st_prep_data->current_approx_rate += st_prep_data->rate_delta;
+    float mm_remaining = pl_block->millimeters;
-        if (st_prep_data->current_approx_rate > st_prep_data->nominal_rate) {
+    float time_var = DT_SEGMENT; // Time worker variable
-          st_prep_data->current_approx_rate = st_prep_data->nominal_rate;
+    float mm_var; // mm distance worker variable
-        }
+    do {
+      switch (prep.ramp_type) {
+        case RAMP_ACCEL: 
+          // NOTE: Acceleration ramp always computes during first loop only.
+          mm_remaining -= DT_SEGMENT*(prep.current_speed + pl_block->acceleration*(0.5*DT_SEGMENT));
+          if (mm_remaining < prep.accelerate_until) { // End of acceleration ramp.
+            // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block.
+            mm_remaining = prep.accelerate_until; // NOTE: 0.0 at EOB
+            time_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed);
+            if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; }
+            else { prep.ramp_type = RAMP_CRUISE; }
+            prep.current_speed = prep.maximum_speed;
+          } else { // Acceleration only. 
+            prep.current_speed += pl_block->acceleration*time_var;
+          }
+          break;
+        case RAMP_CRUISE: 
+          // NOTE: mm_var used to retain the last mm_remaining for incomplete segment time_var calculations.
+          mm_var = mm_remaining - prep.maximum_speed*time_var;
+          if (mm_var < prep.decelerate_after) { // End of cruise. 
+            // Cruise-deceleration junction or end of block.
+            time_var = (mm_remaining - prep.decelerate_after)/prep.maximum_speed;
+            mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB
+            prep.ramp_type = RAMP_DECEL;
+          } else { // Cruising only.         
+            mm_remaining = mm_var; 
+          } 
+          break;
+        default: // case RAMP_DECEL:
+          // NOTE: mm_var used to catch negative decelerate distance values near zero speed.
+          mm_var = time_var*(prep.current_speed - 0.5*pl_block->acceleration*time_var);
+          if ((mm_var > 0.0) && (mm_var < mm_remaining)) { // Deceleration only.
+            prep.current_speed -= pl_block->acceleration*time_var;
+            // Check for near-zero speed and prevent divide by zero in rare scenarios.
+            if (prep.current_speed > prep.exit_speed) { mm_remaining -= mm_var; }
+            else { mm_remaining = 0.0; } // NOTE: Force EOB for now. May change later. 
+          } else { // End of block.
+            time_var = 2.0*mm_remaining/(prep.current_speed+prep.exit_speed);
+            mm_remaining = 0.0;
+            // prep.current_speed = prep.exit_speed;
+          }
       }
-    }
+      dt += time_var; // Add computed ramp time to total segment time.
-    
+      if (dt < DT_SEGMENT) { time_var = DT_SEGMENT - dt; } // **Incomplete** At ramp junction.
-    // TODO: Look into replacing the following dist_per_step divide with multiplying its inverse to save cycles.
+      else { break; } // **Complete** Exit loop. Segment execution time maxed.      
-    
+    } while ( mm_remaining > 0.0 ); // **Complete** Exit loop. End of planner block.
-    // Compute the number of steps in the prepped segment based on the approximate current rate.
+   
-    // 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)*
+       Compute segment step rate, steps to execute, and step phase correction parameters.
-                                (ISR_TICKS_PER_SECOND/ACCELERATION_TICKS_PER_SECOND)/st_prep_data->dist_per_step);    
+       NOTE: Steps are computed by direct scalar conversion of the millimeter distance 
-    // NOTE: Ensures it moves for very slow motions, but MINIMUM_STEP_RATE should always set this too. Perhaps
+       remaining in the block, rather than incrementally tallying the steps executed per
-    // a compile-time check to see if MINIMUM_STEP_RATE is set high enough is all that is needed.
+       segment. This helps in removing floating point round-off issues of several additions. 
-    prep_segment->n_step = max(prep_segment->n_step,MINIMUM_STEPS_PER_SEGMENT);
+       However, since floats have only 7.2 significant digits, long moves with extremely 
-    // NOTE: As long as the ACCELERATION_TICKS_PER_SECOND is valid, n_step should never exceed 255 and overflow.
+       high step counts can exceed the precision of floats, which can lead to lost steps.
-    // prep_segment->n_step = min(prep_segment->n_step,MAXIMUM_STEPS_PER_BLOCK); // Prevent unsigned int8 overflow.
+       Fortunately, this scenario is highly unlikely and unrealistic in CNC machines
-    
+       supported by Grbl (i.e. exceeding 10 meters axis travel at 200 step/mm).
-    // Check if n_step exceeds steps remaining in planner block. If so, truncate.
+    */
-    if (prep_segment->n_step > st_prep_data->step_events_remaining) { 
+    // Use time_var to pre-compute dt inversion with integer multiplier.
-      prep_segment->n_step = st_prep_data->step_events_remaining; 
+    time_var = (INV_TIME_MULTIPLIER/(60.0*ISR_TICKS_PER_SECOND))/dt; // (mult/isr_tic)
+    if (mm_remaining > 0.0) { 
+      float steps_remaining = prep.step_per_mm*mm_remaining; 
+      prep_segment->dist_per_tick = ceil( (prep.step_events_remaining-steps_remaining)*time_var ); // (mult*step/isr_tic)
+
+      // Compute number of steps to execute and segment step phase correction. 
+      prep_segment->phase_dist = ceil(INV_TIME_MULTIPLIER*(ceil(steps_remaining)-steps_remaining));
+      prep_segment->n_step = ceil(prep.step_events_remaining)-ceil(steps_remaining);
+
+      // Update step execution variables
+      pl_block->millimeters = mm_remaining;      
+      prep.step_events_remaining = steps_remaining;                                    
+      
+    } else { // End of block.
+      // Set to execute the remaining steps and no phase correction upon finishing the block. 
+      prep_segment->dist_per_tick = ceil( prep.step_events_remaining*time_var ); // (mult*step/isr_tic)
+      prep_segment->phase_dist = 0;
+      prep_segment->n_step = ceil(prep.step_events_remaining);
+      
+      // The planner block is complete. All steps are set to be executed in the segment buffer.
+      // TODO: Ignore this for feed holds. Need to recalculate the planner buffer at this time.
+      pl_block = NULL;
+      plan_discard_current_block();
     }
 
-    // Check if n_step crosses decelerate point in block. If so, truncate to ensure the deceleration
+    // New step segment initialization completed. Increment segment buffer indices.
-    // ramp counters are set correctly during execution.
-    if (st_prep_data->decelerate_after > 0) {
-      if (prep_segment->n_step > st_prep_data->decelerate_after) { 
-        prep_segment->n_step = st_prep_data->decelerate_after; 
-      }
-    }
- 
-    // Update stepper common data variables.
-    st_prep_data->decelerate_after -= prep_segment->n_step;
-    st_prep_data->step_events_remaining -= prep_segment->n_step;
-    
-    // Check for end of planner block
-    if ( st_prep_data->step_events_remaining == 0 ) { 
-    
-      // TODO: When a feed hold ends, the step_events_remaining will also be zero, even though a block
-      // have partially been completed. We need to flag the stepper algorithm to indicate a stepper shutdown
-      // when complete, but not remove the planner block unless it truly is the end of the block (rare).
-    
-      // Set EOB bitflag so stepper algorithm discards the planner block after this segment completes.
-      prep_segment->flag |= SEGMENT_END_OF_BLOCK;
-      // Move planner pointer to next block and flag to load a new block for the next segment.
-      pl_prep_index = plan_next_block_index(pl_prep_index);
-      pl_prep_block = NULL;
-    }    
-
-    // New step segment completed. Increment segment buffer indices.
     segment_buffer_head = segment_next_head;
     if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; }
-    
-// long a = prep_segment->n_step;    
-// printInteger(a);
-// printString(" ");
 
+// int32_t blength = segment_buffer_head - segment_buffer_tail;
+// if (blength < 0) { blength += SEGMENT_BUFFER_SIZE; } 
+// printInteger(blength);
+//     SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
   } 
 }      
-
-uint8_t st_get_prep_block_index() 
-{
-// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this?
-  return(pl_prep_index);
-}
-
-void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating)
-{
- // if called, can we assume that this always changes and needs to be updated? if so, then
- // we can perform all of the segment buffer setup tasks here to make sure the next time
- // the segments are loaded, the st_data buffer is updated correctly.
- // !!! Make sure that this is always pointing to the correct st_prep_data block. 
- 
- // When a mid-block acceleration occurs, we have to make sure the ramp counters are updated
- // correctly, much in the same fashion as the deceleration counters. Need to think about this 
- // make sure this is right, but i'm pretty sure it is.
-  
-  // TODO: NULL means that the segment buffer has just completed a planner block. Clean up!
-  if (pl_prep_block != NULL) {
-    *millimeters_remaining = st_prep_data->step_events_remaining*st_prep_data->mm_per_step;
-    if (st_prep_data->decelerate_after > 0) { *is_decelerating = false; } 
-    else { *is_decelerating = true; }
-
-    // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes.
-    pl_partial_block_flag = true;
-    pl_prep_block = NULL;
-  }
-  return;
-}

stepper.h

@@ -3,7 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
-  Copyright (c) 2011 Sungeun K. Jeon
+  Copyright (c) 2011-2013 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
@@ -24,6 +24,10 @@
 
 #include <avr/io.h>
 
+#ifndef SEGMENT_BUFFER_SIZE
+  #define SEGMENT_BUFFER_SIZE 7
+#endif
+
 // Initialize and setup the stepper motor subsystem
 void st_init();
 
@@ -45,10 +49,10 @@ void st_cycle_reinitialize();
 // Initiates a feed hold of the running program
 void st_feed_hold();
 
+// Reloads step segment buffer. Called continuously by runtime execution protocol.
 void st_prep_buffer();
 
-uint8_t st_get_prep_block_index();
+// Called by planner_recalculate() when the executing block is updated by the new plan.
-
+void st_update_plan_block_parameters();
-void st_fetch_partial_block_parameters(uint8_t block_index, float *millimeters_remaining, uint8_t *is_decelerating);
 
 #endif

stepper_test.c

@@ -0,0 +1,788 @@
+/*
+  stepper.c - stepper motor driver: executes motion plans using stepper motors
+  Part of Grbl
+
+  Copyright (c) 2011-2013 Sungeun K. Jeon
+  Copyright (c) 2009-2011 Simen Svale Skogsrud
+  
+  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/>.
+*/
+
+#include <avr/interrupt.h>
+#include "stepper.h"
+#include "config.h"
+#include "settings.h"
+#include "planner.h"
+#include "nuts_bolts.h"
+
+// Some useful constants
+#define TICKS_PER_MICROSECOND (F_CPU/1000000)
+
+#define RAMP_NOOP_CRUISE 0
+#define RAMP_ACCEL 1
+#define RAMP_DECEL 2
+
+#define LOAD_NOOP 0
+#define LOAD_SEGMENT 1
+#define LOAD_BLOCK 2
+
+#define SEGMENT_NOOP 0
+#define SEGMENT_END_OF_BLOCK bit(0)
+#define RAMP_CHANGE_ACCEL bit(1)
+#define RAMP_CHANGE_DECEL bit(2)
+
+#define MINIMUM_STEPS_PER_SEGMENT 1 // Don't change
+
+#define SEGMENT_BUFFER_SIZE 6
+
+#define DT_SEGMENT (1/ACCELERATION_TICKS_PER_SECOND)
+
+// Stepper state variable. Contains running data and trapezoid variables.
+typedef struct {
+  // Used by the bresenham line algorithm
+  int32_t counter_x,        // Counter variables for the bresenham line tracer
+          counter_y, 
+          counter_z;
+
+  // Used by inverse time algorithm to track step rate
+  int32_t counter_dist;    // Inverse time distance traveled since last step event
+  
+  uint8_t step_count;  // Steps remaining in line segment motion
+  uint8_t phase_count; // Phase ticks remaining after line segment steps complete
+  
+  // Used by the stepper driver interrupt
+  uint8_t execute_step;    // Flags step execution for each interrupt.
+  uint8_t step_pulse_time; // Step pulse reset time after step rise
+  uint8_t out_bits;        // The next stepping-bits to be output
+  uint8_t load_flag;
+} stepper_t;
+static stepper_t st;
+
+// Stores stepper common data for executing steps in the segment buffer. Data can change mid-block when the
+// planner updates the remaining block velocity profile with a more optimal plan or a feedrate override occurs.
+// NOTE: Normally, this buffer is partially in-use, but, for the worst case scenario, it will never exceed
+// the number of accessible stepper buffer segments (SEGMENT_BUFFER_SIZE-1).
+typedef struct {  
+  
+  // TODO: Retain step[N_AXIS], step_event_count, and direction byte here, so that we can throw
+  // away the planner block when the segment prep is complete.
+  
+  float step_events_remaining; // Tracks step event count for the executing planner block
+
+  // Planner block velocity profile parameters used to trace and execute steps.;
+  float accelerate_until;
+  float decelerate_after;
+  float current_speed;
+  float maximum_speed;
+  float exit_speed;
+} st_data_t;
+static st_data_t segment_data[SEGMENT_BUFFER_SIZE-1];
+
+// Primary stepper segment ring buffer. Contains small, short line segments for the stepper algorithm to execute,
+// which are "checked-out" incrementally from the first block in the planner buffer. Once "checked-out", the steps
+// in the segments buffer cannot be modified by the planner, where the remaining planner block steps still can.
+typedef struct {
+  uint8_t n_step;         // Number of step events to be executed for this segment
+  uint8_t n_phase_tick;
+  uint32_t dist_per_tick;
+  uint8_t st_data_index;  // Stepper buffer common data index. Uses this information to execute this segment.
+  uint8_t flag;           // Stepper algorithm bit-flag for special execution conditions.
+} st_segment_t;
+static st_segment_t segment_buffer[SEGMENT_BUFFER_SIZE];
+
+// Step segment ring buffer indices
+static volatile uint8_t segment_buffer_tail;
+static volatile uint8_t segment_buffer_head;
+static uint8_t segment_next_head;
+
+static volatile uint8_t busy;   // Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
+static plan_block_t *pl_current_block;  // A pointer to the planner block currently being traced
+static st_segment_t *st_current_segment;
+static st_data_t *st_current_data;
+
+// Pointers for the step segment being prepped from the planner buffer. Accessed only by the
+// main program. Pointers may be planning segments or planner blocks ahead of what being executed.
+static plan_block_t *pl_prep_block;   // Pointer to the planner block being prepped
+static st_data_t *st_prep_data;       // Pointer to the stepper common data being prepped
+static uint8_t pl_prep_index;         // Index of planner block being prepped
+static uint8_t st_data_prep_index;    // Index of stepper common data block being prepped
+static uint8_t pl_partial_block_flag; // Flag indicating the planner has modified the prepped planner block
+
+static float st_prep_step_per_mm;
+
+// TODO: All this stuff only needs to be retained for the prepped planner block. Once changed
+// or complete, we do not need this information anymore. Duh!
+// typedef struct {
+//   float step_events_remaining; // Tracks step event count for the executing planner block
+//   float accelerate_until;
+//   float decelerate_after;
+//   float current_speed;
+//   float maximum_speed;
+//   float exit_speed;
+//   float step_per_mm;
+// } st_prep_data_t;
+
+/*        __________________________
+         /|                        |\     _________________         ^
+        / |                        | \   /|               |\        |
+       /  |                        |  \ / |               | \       s
+      /   |                        |   |  |               |  \      p
+     /    |                        |   |  |               |   \     e
+    +-----+------------------------+---+--+---------------+----+    e
+    |               BLOCK 1            |      BLOCK 2          |    d
+
+                            time ----->
+  
+   The trapezoid is the shape the speed curve over time. It starts at block->initial_rate, accelerates by block->rate_delta
+   until reaching cruising speed block->nominal_rate, and/or until step_events_remaining reaches block->decelerate_after
+   after which it decelerates until the block is completed. The driver uses constant acceleration, which is applied as
+   +/- block->rate_delta velocity increments by the midpoint rule at each ACCELERATION_TICKS_PER_SECOND.
+*/
+
+// Stepper state initialization. Cycle should only start if the st.cycle_start flag is
+// enabled. Startup init and limits call this function but shouldn't start the cycle.
+void st_wake_up() 
+{
+  // Enable steppers by resetting the stepper disable port
+  if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { 
+    STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); 
+  } else { 
+    STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT);
+  }
+  if (sys.state == STATE_CYCLE) {
+    // Initialize stepper output bits
+    st.out_bits = settings.invert_mask; 
+    // Initialize step pulse timing from settings.
+    st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);
+    // Enable stepper driver interrupt
+    st.execute_step = false;
+    st.load_flag = LOAD_BLOCK;
+    
+    TCNT2 = 0; // Clear Timer2
+    TIMSK2 |= (1<<OCIE2A); // Enable Timer2 Compare Match A interrupt
+    TCCR2B = (1<<CS21); // Begin Timer2. Full speed, 1/8 prescaler
+  }
+}
+
+
+// Stepper shutdown
+void st_go_idle() 
+{
+  // Disable stepper driver interrupt. Allow Timer0 to finish. It will disable itself.
+  TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt
+  TCCR2B = 0; // Disable Timer2
+  busy = false;
+
+  // Disable steppers only upon system alarm activated or by user setting to not be kept enabled.
+  if ((settings.stepper_idle_lock_time != 0xff) || bit_istrue(sys.execute,EXEC_ALARM)) {
+    // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
+    // stop and not drift from residual inertial forces at the end of the last movement.
+    delay_ms(settings.stepper_idle_lock_time);
+    if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { 
+      STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT); 
+    } else { 
+      STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); 
+    }   
+  }
+}
+
+
+/* "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Grbl. It is based
+   on an inverse time stepper algorithm, where a timer ticks at a constant frequency and uses 
+   time-distance counters to track when its the approximate time for a step event. For reference,
+   a similar inverse-time algorithm by Pramod Ranade is susceptible to numerical round-off, as
+   described, meaning that some axes steps may not execute correctly for a given multi-axis motion.
+     Grbl's algorithm differs by using a single inverse time-distance counter to manage a
+   Bresenham line algorithm for multi-axis step events, which ensures the number of steps for
+   each axis are executed exactly. In other words, Grbl uses a Bresenham within a Bresenham 
+   algorithm, where one tracks time for step events and the other steps for multi-axis moves.
+   Grbl specifically uses the Bresenham algorithm due to its innate mathematical exactness and
+   low computational overhead, requiring simple integer +,- counters only.
+     This interrupt pops blocks from the step segment buffer and executes them by pulsing the 
+   stepper pins appropriately. It is supported by The Stepper Port Reset Interrupt which it uses 
+   to reset the stepper port after each pulse. The bresenham line tracer algorithm controls all 
+   three stepper outputs simultaneously with these two interrupts.
+*/
+/* TODO: 
+   - Measure time in ISR. Typical and worst-case. Should be virtually identical to last algorithm.
+     There are no major changes to the base operations of this ISR with the new segment buffer.
+   - Write how the acceleration counters work and why they are set at half via mid-point rule.
+   - Determine if placing the position counters elsewhere (or change them to 8-bit variables that
+     are added to the system position counters at the end of a segment) frees up cycles.
+   - Write a blurb about how the acceleration should be handled within the ISR. All of the 
+     time/step/ramp counters accurately keep track of the remainders and phasing of the variables
+     with time. This means we do not have to compute them via expensive floating point beforehand.
+   - Need to do an analysis to determine if these counters are really that much cheaper. At least
+     find out when it isn't anymore. Particularly when the ISR is at a very high frequency.
+   - Create NOTE: to describe that the total time in this ISR must be less than the ISR frequency
+     in its worst case scenario.
+*/
+ISR(TIMER2_COMPA_vect)
+{
+// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT; // Debug: Used to time ISR
+  if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt
+  
+  // Pulse stepper port pins, if flagged. New block dir will always be set one timer tick 
+  // before any step pulse due to algorithm design.
+  if (st.execute_step) {
+    st.execute_step = false;
+    STEPPING_PORT = ( STEPPING_PORT & ~(DIRECTION_MASK | STEP_MASK) ) | st.out_bits;
+    TCNT0 = st.step_pulse_time; // Reload Timer0 counter.
+    TCCR0B = (1<<CS21); // Begin Timer0. Full speed, 1/8 prescaler
+  }
+  
+  busy = true;
+  sei(); // Re-enable interrupts to allow Stepper Port Reset Interrupt to fire on-time. 
+         // NOTE: The remaining code in this ISR will finish before returning to main program.
+    
+  // If there is no step segment, attempt to pop one from the stepper buffer
+  if (st.load_flag != LOAD_NOOP) {
+    
+    // Anything in the buffer? If so, load and initialize next step segment.
+    if (segment_buffer_head != segment_buffer_tail) {
+      
+      // Initialize new step segment and load number of steps to execute
+      st_current_segment = &segment_buffer[segment_buffer_tail];
+      st.step_count = st_current_segment->n_step;
+    
+      // If the new segment starts a new planner block, initialize stepper variables and counters.
+      // NOTE: For new segments only, the step counters are not updated to ensure step phasing is continuous.
+      if (st.load_flag == LOAD_BLOCK) {
+        pl_current_block = plan_get_current_block(); // Should always be there. Stepper buffer handles this.
+        st_current_data = &segment_data[segment_buffer[segment_buffer_tail].st_data_index];
+        
+        // Initialize direction bits for block. Set execute flag to set directions bits upon next ISR tick.
+        st.out_bits = pl_current_block->direction_bits ^ settings.invert_mask;
+        st.execute_step = true;
+        
+        // Initialize Bresenham line counters
+        st.counter_x = (pl_current_block->step_event_count >> 1);
+        st.counter_y = st.counter_x;
+        st.counter_z = st.counter_x;
+        
+        // Initialize inverse time, step rate data, and acceleration ramp counters
+        st.counter_dist = INV_TIME_MULTIPLIER;  // dist_per_step always greater than dist_per_tick.
+      }
+      
+      st.load_flag = LOAD_NOOP; // Segment motion loaded. Set no-operation flag to skip during execution.
+
+    } else {
+      // Can't discard planner block here if a feed hold stops in middle of block.
+      st_go_idle();
+      bit_true(sys.execute,EXEC_CYCLE_STOP); // Flag main program for cycle end
+      return; // Nothing to do but exit.
+    }  
+    
+  } 
+    
+  // Iterate inverse time counter. Triggers each Bresenham step event.
+  st.counter_dist -= st_current_segment->dist_per_tick; 
+  
+  // Execute Bresenham step event, when it's time to do so.
+  if (st.counter_dist < 0) {
+    if (st.step_count > 0) {  // Block phase correction from executing step. 
+      st.counter_dist += INV_TIME_MULTIPLIER; // Reload inverse time counter
+  
+      st.out_bits = pl_current_block->direction_bits; // Reset out_bits and reload direction bits
+      st.execute_step = true;
+      
+      // Execute step displacement profile by Bresenham line algorithm
+      st.counter_x -= pl_current_block->steps[X_AXIS];
+      if (st.counter_x < 0) {
+        st.out_bits |= (1<<X_STEP_BIT);
+        st.counter_x += pl_current_block->step_event_count;
+        if (st.out_bits & (1<<X_DIRECTION_BIT)) { sys.position[X_AXIS]--; }
+        else { sys.position[X_AXIS]++; }
+      }
+      st.counter_y -= pl_current_block->steps[Y_AXIS];
+      if (st.counter_y < 0) {
+        st.out_bits |= (1<<Y_STEP_BIT);
+        st.counter_y += pl_current_block->step_event_count;
+        if (st.out_bits & (1<<Y_DIRECTION_BIT)) { sys.position[Y_AXIS]--; }
+        else { sys.position[Y_AXIS]++; }
+      }
+      st.counter_z -= pl_current_block->steps[Z_AXIS];
+      if (st.counter_z < 0) {
+        st.out_bits |= (1<<Z_STEP_BIT);
+        st.counter_z += pl_current_block->step_event_count;
+        if (st.out_bits & (1<<Z_DIRECTION_BIT)) { sys.position[Z_AXIS]--; }
+        else { sys.position[Z_AXIS]++; }
+      }
+  
+      // Check step events for trapezoid change or end of block.
+      st.step_count--; // Decrement step events count    
+  
+      st.out_bits ^= settings.invert_mask;  // Apply step port invert mask    
+    }
+  }
+  
+  if (st.step_count == 0) {
+    if (st.phase_count == 0) {
+      // Line move is complete, set load line flag to check for new move.
+      // Check if last line move in planner block. Discard if so.
+      if (st_current_segment->flag & SEGMENT_END_OF_BLOCK) {
+        plan_discard_current_block();
+        st.load_flag = LOAD_BLOCK;
+      } else {
+        st.load_flag = LOAD_SEGMENT;
+      }
+      
+      // Discard current segment by advancing buffer tail index
+      if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; }
+    }
+    st.phase_count--;
+  }
+  
+  
+  busy = false;
+// SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
+}
+
+
+// The Stepper Port Reset Interrupt: Timer0 OVF interrupt handles the falling edge of the step
+// pulse. This should always trigger before the next Timer2 COMPA interrupt and independently
+// finish, if Timer2 is disabled after completing a move.
+ISR(TIMER0_OVF_vect)
+{
+  STEPPING_PORT = (STEPPING_PORT & ~STEP_MASK) | (settings.invert_mask & STEP_MASK); 
+  TCCR0B = 0; // Disable timer until needed.
+}
+
+
+// Reset and clear stepper subsystem variables
+void st_reset()
+{
+  memset(&st, 0, sizeof(st));
+  
+  st.load_flag = LOAD_BLOCK;
+  busy = false;
+
+  pl_current_block = NULL; // Planner block pointer used by stepper algorithm
+  pl_prep_block = NULL;  // Planner block pointer used by segment buffer
+  pl_prep_index =   0; // Planner buffer indices are also reset to zero.
+  st_data_prep_index = 0;
+  
+  segment_buffer_tail = 0;
+  segment_buffer_head = 0; // empty = tail
+  segment_next_head = 1;
+    
+  pl_partial_block_flag = false;
+}
+
+
+// Initialize and start the stepper motor subsystem
+void st_init()
+{
+  // Configure directions of interface pins
+  STEPPING_DDR |= STEPPING_MASK;
+  STEPPING_PORT = (STEPPING_PORT & ~STEPPING_MASK) | settings.invert_mask;
+  STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
+	
+  // Configure Timer 2
+  TIMSK2 &= ~(1<<OCIE2A); // Disable Timer2 interrupt while configuring it
+  TCCR2B = 0; // Disable Timer2 until needed
+  TCNT2 = 0; // Clear Timer2 counter
+  TCCR2A = (1<<WGM21);  // Set CTC mode
+  OCR2A = (F_CPU/ISR_TICKS_PER_SECOND)/8 - 1; // Set Timer2 CTC rate
+
+  // Configure Timer 0
+  TIMSK0 &= ~(1<<TOIE0);
+  TCCR0A = 0; // Normal operation
+  TCCR0B = 0; // Disable Timer0 until needed
+  TIMSK0 |= (1<<TOIE0); // Enable overflow interrupt
+  
+  // Start in the idle state, but first wake up to check for keep steppers enabled option.
+  st_wake_up();
+  st_go_idle();
+}
+
+
+// Planner external interface to start stepper interrupt and execute the blocks in queue. Called
+// by the main program functions: planner auto-start and run-time command execution.
+void st_cycle_start() 
+{
+  if (sys.state == STATE_QUEUED) {
+    sys.state = STATE_CYCLE;
+    st_prep_buffer(); // Initialize step segment buffer before beginning cycle.
+    st_wake_up();
+  }
+}
+
+
+// Execute a feed hold with deceleration, only during cycle. Called by main program.
+void st_feed_hold() 
+{
+  if (sys.state == STATE_CYCLE) {
+    sys.state = STATE_HOLD;
+    sys.auto_start = false; // Disable planner auto start upon feed hold.
+  }
+}
+
+
+// Reinitializes the cycle plan and stepper system after a feed hold for a resume. Called by 
+// runtime command execution in the main program, ensuring that the planner re-plans safely.
+// NOTE: Bresenham algorithm variables are still maintained through both the planner and stepper
+// cycle reinitializations. The stepper path should continue exactly as if nothing has happened.
+// Only the planner de/ac-celerations profiles and stepper rates have been updated.
+void st_cycle_reinitialize()
+{
+//   if (pl_current_block != NULL) {
+    // Replan buffer from the feed hold stop location.
+    
+    // TODO: Need to add up all of the step events in the current planner block to give 
+    // back to the planner. Should only need it for the current block. 
+    // BUT! The planner block millimeters is all changed and may be changed into the next
+    // planner block. The block millimeters would need to be recalculated via step counts
+    // and the mm/step variable.
+    // OR. Do we plan the feed hold itself down with the planner.
+    
+//     plan_cycle_reinitialize(st_current_data->step_events_remaining);
+//     st.ramp_type = RAMP_ACCEL;
+//     st.counter_ramp = ISR_TICKS_PER_ACCELERATION_TICK/2; 
+//     st.ramp_rate = 0;
+//     sys.state = STATE_QUEUED;
+//   } else {
+//     sys.state = STATE_IDLE;
+//   }
+    sys.state = STATE_IDLE;
+
+}
+
+
+/* Prepares step segment buffer. Continuously called from main program. 
+
+   The segment buffer is an intermediary buffer interface between the execution of steps
+   by the stepper algorithm and the velocity profiles generated by the planner. The stepper
+   algorithm only executes steps within the segment buffer and is filled by the main program
+   when steps are "checked-out" from the first block in the planner buffer. This keeps the
+   step execution and planning optimization processes atomic and protected from each other.
+   The number of steps "checked-out" from the planner buffer and the number of segments in
+   the segment buffer is sized and computed such that no operation in the main program takes
+   longer than the time it takes the stepper algorithm to empty it before refilling it. 
+   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 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 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. 
+*/
+
+/* 
+   TODO: Figure out how to enforce a deceleration when a feedrate override is reduced. 
+     The problem is that when an override is reduced, the planner may not plan back to 
+     the current rate. Meaning that the velocity profiles for certain conditions no longer
+     are trapezoidal or triangular. For example, if the current block is cruising at a
+     nominal rate and the feedrate override is reduced, the new nominal rate will now be
+     lower. The velocity profile must first decelerate to the new nominal rate and then 
+     follow on the new plan. So the remaining velocity profile will have a decelerate, 
+     cruise, and another decelerate.
+        Another issue is whether or not a feedrate override reduction causes a deceleration
+     that acts over several planner blocks. For example, say that the plan is already 
+     heavily decelerating throughout it, reducing the feedrate will not do much to it. So,
+     how do we determine when to resume the new plan? How many blocks do we have to wait 
+     until the new plan intersects with the deceleration curve? One plus though, the 
+     deceleration will never be more than the number of blocks in the entire planner buffer,
+     but it theoretically can be equal to it when all planner blocks are decelerating already.
+*/
+void st_prep_buffer()
+{
+  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
+    st_segment_t *prep_segment = &segment_buffer[segment_buffer_head];
+    prep_segment->flag = SEGMENT_NOOP;
+    
+    // -----------------------------------------------------------------------------------
+    // Determine if we need to load a new planner block. If so, prepare step data.
+    if (pl_prep_block == NULL) {
+      pl_prep_block = plan_get_block_by_index(pl_prep_index); // Query planner for a queued block
+      if (pl_prep_block == NULL) { return; } // No planner blocks. Exit.
+        
+      // Increment stepper common data index 
+      if ( ++st_data_prep_index == (SEGMENT_BUFFER_SIZE-1) ) { st_data_prep_index = 0; }
+        
+      // Check if the planner has re-computed this block mid-execution. If so, push the previous 
+      // segment data. Otherwise, prepare a new segment data for the new planner block.
+      if (pl_partial_block_flag) {
+        
+        // Prepare new shared segment block data and copy the relevant last segment block data.
+        st_data_t *last_st_prep_data;
+        last_st_prep_data = st_prep_data;
+        st_prep_data = &segment_data[st_data_prep_index];
+            
+        st_prep_data->step_events_remaining = last_st_prep_data->step_events_remaining;
+
+        pl_partial_block_flag = false; // Reset flag
+        
+      } else {
+      
+        // Prepare commonly shared planner block data for the ensuing segment buffer moves ad-hoc, since 
+        // the planner buffer can dynamically change the velocity profile data as blocks are added.
+        st_prep_data = &segment_data[st_data_prep_index];
+
+        st_prep_step_per_mm = pl_prep_block->step_event_count/pl_prep_block->millimeters;        
+      
+        // Initialize planner block step data
+        st_prep_data->step_events_remaining = pl_prep_block->step_event_count;
+        
+      }
+      
+      
+      st_prep_data->current_speed = sqrt(pl_prep_block->entry_speed_sqr);
+  
+      // Determine current block exit speed
+      plan_block_t *pl_next_block = plan_get_block_by_index(plan_next_block_index(pl_prep_index));
+      float exit_speed_sqr;
+      if (pl_next_block == NULL) { 
+        exit_speed_sqr = 0.0;
+        st_prep_data->exit_speed = 0.0;
+      } else {
+        exit_speed_sqr = pl_next_block->entry_speed_sqr; 
+        st_prep_data->exit_speed = sqrt(exit_speed_sqr);
+      }
+      
+//       st_prep_data->accelerate_until = 0.5*(pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(pl_prep_block->acceleration);
+//       st_prep_data->decelerate_after = 0.5*(pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(pl_prep_block->acceleration);
+//       if (pl_prep_block->millimeters < st_prep_data->accelerate_until+st_prep_data->decelerate_after) {  
+//         st_prep_data->decelerate_after = 0.5*( pl_prep_block->millimeters + 0.5*(pl_prep_block->entry_speed_sqr
+//                                           - exit_speed_sqr)/(pl_prep_block->acceleration) );
+//         st_prep_data->accelerate_until = pl_prep_block->millimeters-st_prep_data->decelerate_after;
+//         st_prep_data->maximum_speed = sqrt(2*pl_prep_block->acceleration*st_prep_data->decelerate_after+exit_speed_sqr);
+//       } else {
+//         st_prep_data->accelerate_until = pl_prep_block->millimeters-st_prep_data->accelerate_until;
+//         st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr);
+//       }
+      
+      // Determine velocity profile based on the 7 possible types: Cruise-only, cruise-deceleration,
+      // acceleration-cruise, acceleration-only, deceleration-only, trapezoid, and triangle.
+      st_prep_data->accelerate_until = pl_prep_block->millimeters;
+      if (pl_prep_block->entry_speed_sqr == pl_prep_block->nominal_speed_sqr) {
+        st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr);
+        if (exit_speed_sqr == pl_prep_block->nominal_speed_sqr) { // Cruise-only type
+          st_prep_data->decelerate_after = 0.0;
+        } else { // Cruise-deceleration type
+          st_prep_data->decelerate_after = 0.5*(pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(pl_prep_block->acceleration);
+        }
+      } else if (exit_speed_sqr == pl_prep_block->nominal_speed_sqr) { 
+        // Acceleration-cruise type
+        st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr);
+        st_prep_data->decelerate_after = 0.0;
+        st_prep_data->accelerate_until -= 0.5*(pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(pl_prep_block->acceleration);
+      } else {
+        float intersection_dist = 0.5*( pl_prep_block->millimeters + 0.5*(pl_prep_block->entry_speed_sqr
+                                  - exit_speed_sqr)/(pl_prep_block->acceleration) );
+        if (intersection_dist > 0.0) {
+          if (intersection_dist < pl_prep_block->millimeters) { // Either trapezoid or triangle types
+            st_prep_data->decelerate_after = 0.5*(pl_prep_block->nominal_speed_sqr-exit_speed_sqr)/(pl_prep_block->acceleration);
+            if (st_prep_data->decelerate_after < intersection_dist) { // Trapezoid type
+              st_prep_data->maximum_speed = sqrt(pl_prep_block->nominal_speed_sqr);
+              st_prep_data->accelerate_until -= 0.5*(pl_prep_block->nominal_speed_sqr-pl_prep_block->entry_speed_sqr)/(pl_prep_block->acceleration);
+            } else { // Triangle type
+              st_prep_data->decelerate_after = intersection_dist;
+              st_prep_data->maximum_speed = sqrt(2*pl_prep_block->acceleration*st_prep_data->decelerate_after+exit_speed_sqr);
+              st_prep_data->accelerate_until -= st_prep_data->decelerate_after;
+            }          
+          } else { // Deceleration-only type
+            st_prep_data->maximum_speed = st_prep_data->current_speed;
+            st_prep_data->decelerate_after = pl_prep_block->millimeters;
+          }
+        } else { // Acceleration-only type
+          st_prep_data->maximum_speed = st_prep_data->exit_speed;
+          st_prep_data->decelerate_after = 0.0;
+          st_prep_data->accelerate_until = 0.0;
+        }
+      }         
+    }
+
+    // Set new segment to point to the current segment data block.
+    prep_segment->st_data_index = st_data_prep_index;
+
+    // -----------------------------------------------------------------------------------
+    // Initialize segment execute distance. Attempt to create a full segment over DT_SEGMENT.
+
+    float mm_remaining = pl_prep_block->millimeters;
+    float dt = DT_SEGMENT;       
+    if (mm_remaining > st_prep_data->accelerate_until) { // Acceleration ramp
+      mm_remaining -= (st_prep_data->current_speed*DT_SEGMENT
+                         + pl_prep_block->acceleration*(0.5*DT_SEGMENT*DT_SEGMENT));
+      if (mm_remaining < st_prep_data->accelerate_until) { // **Incomplete** Acceleration ramp end.
+        // Acceleration-cruise, acceleration-deceleration ramp junction, or end of block.
+        mm_remaining = st_prep_data->accelerate_until; // NOTE: 0.0 at EOB
+        dt = 2*(pl_prep_block->millimeters-mm_remaining)/
+             (st_prep_data->current_speed+st_prep_data->maximum_speed);
+        st_prep_data->current_speed = st_prep_data->maximum_speed;
+      } else { // **Complete** Acceleration only. 
+        st_prep_data->current_speed += pl_prep_block->acceleration*DT_SEGMENT;
+      }
+    } else if (mm_remaining <= st_prep_data->decelerate_after) { // Deceleration ramp
+      mm_remaining -= (st_prep_data->current_speed*DT_SEGMENT
+                         - pl_prep_block->acceleration*(0.5*DT_SEGMENT*DT_SEGMENT));
+      if (mm_remaining > 0.0) { // **Complete** Deceleration only.
+        st_prep_data->current_speed -= pl_prep_block->acceleration*DT_SEGMENT;
+      } else { // **Complete* End of block.
+        dt = 2*pl_prep_block->millimeters/(st_prep_data->current_speed+st_prep_data->exit_speed);
+        mm_remaining = 0.0;
+        // st_prep_data->current_speed = st_prep_data->exit_speed;
+      }
+    } else { // Cruising profile
+      mm_remaining -= st_prep_data->maximum_speed*DT_SEGMENT;
+      if (mm_remaining < st_prep_data->decelerate_after) { // **Incomplete** End of cruise. 
+        // Cruise-deceleration junction or end of block.
+        mm_remaining = st_prep_data->decelerate_after; // NOTE: 0.0 at EOB
+        dt = (pl_prep_block->millimeters-mm_remaining)/st_prep_data->maximum_speed;
+      } // Otherwise **Complete** Cruising only.
+    }
+    
+    // -----------------------------------------------------------------------------------
+    // If segment is incomplete, attempt to fill the remainder.
+    // NOTE: Segment remainder always spans a cruise and/or a deceleration ramp.
+    
+    if (dt < DT_SEGMENT) {
+      if (mm_remaining > 0.0) { // Skip if end of block.
+        float last_mm_remaining;
+        float dt_remainder;
+        
+        // Fill incomplete segment with an acceleration junction. 
+        if (mm_remaining > st_prep_data->decelerate_after) { // Cruising profile
+          last_mm_remaining = mm_remaining;
+          dt_remainder = DT_SEGMENT-dt;
+          mm_remaining -= st_prep_data->current_speed*dt_remainder;
+          if (mm_remaining < st_prep_data->decelerate_after) { // **Incomplete**
+            mm_remaining = st_prep_data->decelerate_after;
+            dt += (last_mm_remaining-mm_remaining)/st_prep_data->maximum_speed;
+            // current_speed = maximum_speed;
+          } else { // **Complete** Segment filled. 
+            dt = DT_SEGMENT;
+          }
+        }
+        
+        // Fill incomplete segment with a deceleration junction.
+        if (mm_remaining > 0.0) {
+          if (mm_remaining <= st_prep_data->decelerate_after) { // Deceleration ramp
+            last_mm_remaining = mm_remaining;
+            dt_remainder = DT_SEGMENT-dt;
+            mm_remaining -= (dt_remainder*(st_prep_data->current_speed
+                               - 0.5*pl_prep_block->acceleration*dt_remainder));
+            if (mm_remaining > 0.0) { // **Complete** Segment filled.
+              st_prep_data->current_speed -= pl_prep_block->acceleration*dt_remainder;
+              dt = DT_SEGMENT;
+            } else { // **Complete** End of block.
+              mm_remaining = 0.0;
+              dt += (2*last_mm_remaining/(st_prep_data->current_speed+st_prep_data->exit_speed));
+              // st_prep_data->current_speed = st_prep_data->exit_speed;
+            }
+          }
+        }
+        
+      }
+    }
+ 
+    // -----------------------------------------------------------------------------------
+    // Compute segment step rate, steps to execute, and step phase correction parameters.
+    
+    // Convert segment distance in terms of steps.
+//     float dist_travel = pl_prep_block->millimeters;
+//     if (mm_remaining > 0.0) { dist_travel -= mm_remaining; }
+
+
+    if (mm_remaining > 0.0) { 
+
+      float steps_remaining = st_prep_step_per_mm*mm_remaining; 
+      prep_segment->dist_per_tick = ceil( (INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND)*
+                                    (st_prep_data->step_events_remaining-steps_remaining)/dt ); // (mult*step/isr_tic)
+
+      // Compute number of steps to execute and segment step phase correction. 
+      prep_segment->n_step = ceil(st_prep_data->step_events_remaining)-ceil(steps_remaining);
+      prep_segment->n_phase_tick = ceil(INV_TIME_MULTIPLIER*(ceil(steps_remaining)-steps_remaining)/prep_segment->dist_per_tick);
+
+      // Update step execution variables
+      st_prep_data->step_events_remaining = steps_remaining;                                    
+      pl_prep_block->millimeters = mm_remaining;
+      
+    } else { // End of block. Finish it out.
+
+      prep_segment->dist_per_tick = ceil( (INV_TIME_MULTIPLIER/ISR_TICKS_PER_SECOND)*
+                                    st_prep_data->step_events_remaining/dt ); // (mult*step/isr_tic)
+
+      // Set to execute the remaining steps and no phase correction upon finishing the block. 
+      prep_segment->n_step = ceil(st_prep_data->step_events_remaining);
+      prep_segment->n_phase_tick = 0;
+      
+      // NOTE: Not required. Planner will ignore this block as it is now complete.
+      // st_prep_data->step_events_remaining = 0.0;
+      // pl_prep_block->millimeters = 0.0;
+      
+      // Move planner pointer to next block and flag to load a new block for the next segment.
+      pl_prep_index = plan_next_block_index(pl_prep_index);
+      pl_prep_block = NULL;
+      prep_segment->flag |= SEGMENT_END_OF_BLOCK;
+      
+    }
+
+    // !!! PROBLEM. Step events remaining in floating point can limit the number of steps
+    // we can accurately track, since floats have ~7.2 significant digits. However, this only
+    // becomes a problem if there are more than 1,000,000, which translates to a CNC machine
+    // with 200 step/mm and 5 meters of axis travel. Possible but unlikely. Could have more 
+    // issues with user setting up their machine with too high of steps.
+    
+    // TODO: dist_per_tick must be less than INV_TIME_MULTIPLIER. A check can be made to 
+    // make this a hard limit. Need to make sure this doesn't affect the velocity profiles..
+    // it shouldn't. The same could said for the minimum allowable step rate too. This should
+    // not affect the tracing of the profiles either.
+
+   // Ensure the initial step rate exceeds the MINIMUM_STEP_RATE.
+   // TODO: Use config.h error checking to do this. Otherwise, counters get screwy.
+
+    // New step segment initialization completed. Increment segment buffer indices.
+    segment_buffer_head = segment_next_head;
+    if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; }
+    
+    SPINDLE_ENABLE_PORT ^= 1<<SPINDLE_ENABLE_BIT;  
+  } 
+}      
+
+
+uint8_t st_get_prep_block_index() 
+{
+// Returns only the index but doesn't state if the block has been partially executed. How do we simply check for this?
+  return(pl_prep_index);
+}
+
+
+void st_update_plan_block_parameters()
+{ 
+  if (pl_prep_block != NULL) { // Ignore if at start of a new block.
+    plan_block_t *pl_partial_block = plan_get_block_by_index(pl_prep_index); // Point to partially completed block
+    pl_partial_block->entry_speed_sqr = st_prep_data->current_speed*st_prep_data->current_speed;
+    // pl_partial_block->max_entry_speed_sqr = pl_partial_block->entry_speed_sqr; // Not sure if this needs to be updated.
+
+    // Flag for new prep_block when st_prep_buffer() is called after the planner recomputes.
+    pl_partial_block_flag = true;
+    pl_prep_block = NULL;
+  }
+  return;
+}